ADD: new track message, Entity class and Position class

libs/eigen/unsupported/doc/examples/BVH_Example.cpp

@@ -0,0 +1,50 @@
+#include <Eigen/StdVector>
+#include <unsupported/Eigen/BVH>
+#include <iostream>
+
+using namespace Eigen;
+typedef AlignedBox<double, 2> Box2d;
+
+namespace Eigen {
+  Box2d bounding_box(const Vector2d &v) { return Box2d(v, v); } //compute the bounding box of a single point
+}
+
+struct PointPointMinimizer //how to compute squared distances between points and rectangles
+{
+  PointPointMinimizer() : calls(0) {}
+  typedef double Scalar;
+
+  double minimumOnVolumeVolume(const Box2d &r1, const Box2d &r2) { ++calls; return r1.squaredExteriorDistance(r2); }
+  double minimumOnVolumeObject(const Box2d &r, const Vector2d &v) { ++calls; return r.squaredExteriorDistance(v); }
+  double minimumOnObjectVolume(const Vector2d &v, const Box2d &r) { ++calls; return r.squaredExteriorDistance(v); }
+  double minimumOnObjectObject(const Vector2d &v1, const Vector2d &v2) { ++calls; return (v1 - v2).squaredNorm(); }
+
+  int calls;
+};
+
+int main()
+{
+  typedef std::vector<Vector2d, aligned_allocator<Vector2d> > StdVectorOfVector2d;
+  StdVectorOfVector2d redPoints, bluePoints;
+  for(int i = 0; i < 100; ++i) { //initialize random set of red points and blue points
+    redPoints.push_back(Vector2d::Random());
+    bluePoints.push_back(Vector2d::Random());
+  }
+
+  PointPointMinimizer minimizer;
+  double minDistSq = std::numeric_limits<double>::max();
+
+  //brute force to find closest red-blue pair
+  for(int i = 0; i < (int)redPoints.size(); ++i)
+    for(int j = 0; j < (int)bluePoints.size(); ++j)
+      minDistSq = std::min(minDistSq, minimizer.minimumOnObjectObject(redPoints[i], bluePoints[j]));
+  std::cout << "Brute force distance = " << sqrt(minDistSq) << ", calls = " << minimizer.calls << std::endl;
+
+  //using BVH to find closest red-blue pair
+  minimizer.calls = 0;
+  KdBVH<double, 2, Vector2d> redTree(redPoints.begin(), redPoints.end()), blueTree(bluePoints.begin(), bluePoints.end()); //construct the trees
+  minDistSq = BVMinimize(redTree, blueTree, minimizer); //actual BVH minimization call
+  std::cout << "BVH distance         = " << sqrt(minDistSq) << ", calls = " << minimizer.calls << std::endl;
+
+  return 0;
+}

libs/eigen/unsupported/doc/examples/CMakeLists.txt

@@ -0,0 +1,24 @@
+file(GLOB examples_SRCS "*.cpp")
+
+add_custom_target(unsupported_examples)
+
+include_directories(../../../unsupported ../../../unsupported/test)
+
+foreach(example_src ${examples_SRCS})
+  get_filename_component(example ${example_src} NAME_WE)
+  add_executable(example_${example} ${example_src})
+  if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO)
+    target_link_libraries(example_${example} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO})
+  endif()
+  add_custom_command(
+    TARGET example_${example}
+    POST_BUILD
+    COMMAND example_${example}
+    ARGS >${CMAKE_CURRENT_BINARY_DIR}/${example}.out
+  )
+  add_dependencies(unsupported_examples example_${example})
+endforeach(example_src)
+
+if(EIGEN_TEST_SYCL)
+  add_subdirectory(SYCL)
+endif(EIGEN_TEST_SYCL)

libs/eigen/unsupported/doc/examples/EulerAngles.cpp

@@ -0,0 +1,46 @@
+#include <unsupported/Eigen/EulerAngles>
+#include <iostream>
+
+using namespace Eigen;
+
+int main()
+{
+  // A common Euler system by many armies around the world,
+  //  where the first one is the azimuth(the angle from the north -
+  //   the same angle that is show in compass)
+  //  and the second one is elevation(the angle from the horizon)
+  //  and the third one is roll(the angle between the horizontal body
+  //   direction and the plane ground surface)
+  // Keep remembering we're using radian angles here!
+  typedef EulerSystem<-EULER_Z, EULER_Y, EULER_X> MyArmySystem;
+  typedef EulerAngles<double, MyArmySystem> MyArmyAngles;
+  
+  MyArmyAngles vehicleAngles(
+    3.14/*PI*/ / 2, /* heading to east, notice that this angle is counter-clockwise */
+    -0.3, /* going down from a mountain */
+    0.1); /* slightly rolled to the right */
+  
+  // Some Euler angles representation that our plane use.
+  EulerAnglesZYZd planeAngles(0.78474, 0.5271, -0.513794);
+  
+  MyArmyAngles planeAnglesInMyArmyAngles(planeAngles);
+  
+  std::cout << "vehicle angles(MyArmy):     " << vehicleAngles << std::endl;
+  std::cout << "plane angles(ZYZ):        " << planeAngles << std::endl;
+  std::cout << "plane angles(MyArmy):     " << planeAnglesInMyArmyAngles << std::endl;
+  
+  // Now lets rotate the plane a little bit
+  std::cout << "==========================================================\n";
+  std::cout << "rotating plane now!\n";
+  std::cout << "==========================================================\n";
+  
+  Quaterniond planeRotated = AngleAxisd(-0.342, Vector3d::UnitY()) * planeAngles;
+  
+  planeAngles = planeRotated;
+  planeAnglesInMyArmyAngles = planeRotated;
+  
+  std::cout << "new plane angles(ZYZ):     " << planeAngles << std::endl;
+  std::cout << "new plane angles(MyArmy): " << planeAnglesInMyArmyAngles << std::endl;
+  
+  return 0;
+}

libs/eigen/unsupported/doc/examples/FFT.cpp

@@ -0,0 +1,118 @@
+//  To use the simple FFT implementation
+//  g++ -o demofft -I.. -Wall -O3 FFT.cpp 
+
+//  To use the FFTW implementation
+//  g++ -o demofft -I.. -DUSE_FFTW -Wall -O3 FFT.cpp -lfftw3 -lfftw3f -lfftw3l
+
+#ifdef USE_FFTW
+#include <fftw3.h>
+#endif
+
+#include <vector>
+#include <complex>
+#include <algorithm>
+#include <iterator>
+#include <iostream>
+#include <Eigen/Core>
+#include <unsupported/Eigen/FFT>
+
+using namespace std;
+using namespace Eigen;
+
+template <typename T>
+T mag2(T a)
+{
+    return a*a;
+}
+template <typename T>
+T mag2(std::complex<T> a)
+{
+    return norm(a);
+}
+
+template <typename T>
+T mag2(const std::vector<T> & vec)
+{
+    T out=0;
+    for (size_t k=0;k<vec.size();++k)
+        out += mag2(vec[k]);
+    return out;
+}
+
+template <typename T>
+T mag2(const std::vector<std::complex<T> > & vec)
+{
+    T out=0;
+    for (size_t k=0;k<vec.size();++k)
+        out += mag2(vec[k]);
+    return out;
+}
+
+template <typename T>
+vector<T> operator-(const vector<T> & a,const vector<T> & b )
+{
+    vector<T> c(a);
+    for (size_t k=0;k<b.size();++k) 
+        c[k] -= b[k];
+    return c;
+}
+
+template <typename T>
+void RandomFill(std::vector<T> & vec)
+{
+    for (size_t k=0;k<vec.size();++k)
+        vec[k] = T( rand() )/T(RAND_MAX) - T(.5);
+}
+
+template <typename T>
+void RandomFill(std::vector<std::complex<T> > & vec)
+{
+    for (size_t k=0;k<vec.size();++k)
+        vec[k] = std::complex<T> ( T( rand() )/T(RAND_MAX) - T(.5), T( rand() )/T(RAND_MAX) - T(.5));
+}
+
+template <typename T_time,typename T_freq>
+void fwd_inv(size_t nfft)
+{
+    typedef typename NumTraits<T_freq>::Real Scalar;
+    vector<T_time> timebuf(nfft);
+    RandomFill(timebuf);
+
+    vector<T_freq> freqbuf;
+    static FFT<Scalar> fft;
+    fft.fwd(freqbuf,timebuf);
+
+    vector<T_time> timebuf2;
+    fft.inv(timebuf2,freqbuf);
+
+    T_time rmse = mag2(timebuf - timebuf2) / mag2(timebuf);
+    cout << "roundtrip rmse: " << rmse << endl;
+}
+
+template <typename T_scalar>
+void two_demos(int nfft)
+{
+    cout << "     scalar ";
+    fwd_inv<T_scalar,std::complex<T_scalar> >(nfft);
+    cout << "    complex ";
+    fwd_inv<std::complex<T_scalar>,std::complex<T_scalar> >(nfft);
+}
+
+void demo_all_types(int nfft)
+{
+    cout << "nfft=" << nfft << endl;
+    cout << "   float" << endl;
+    two_demos<float>(nfft);
+    cout << "   double" << endl;
+    two_demos<double>(nfft);
+    cout << "   long double" << endl;
+    two_demos<long double>(nfft);
+}
+
+int main()
+{
+    demo_all_types( 2*3*4*5*7 );
+    demo_all_types( 2*9*16*25 );
+    demo_all_types( 1024 );
+    return 0;
+}

libs/eigen/unsupported/doc/examples/MatrixExponential.cpp

@@ -0,0 +1,16 @@
+#include <unsupported/Eigen/MatrixFunctions>
+#include <iostream>
+
+using namespace Eigen;
+
+int main()
+{
+  const double pi = std::acos(-1.0);
+
+  MatrixXd A(3,3);
+  A << 0,    -pi/4, 0,
+       pi/4, 0,     0,
+       0,    0,     0;
+  std::cout << "The matrix A is:\n" << A << "\n\n";
+  std::cout << "The matrix exponential of A is:\n" << A.exp() << "\n\n";
+}

libs/eigen/unsupported/doc/examples/MatrixFunction.cpp

@@ -0,0 +1,23 @@
+#include <unsupported/Eigen/MatrixFunctions>
+#include <iostream>
+
+using namespace Eigen;
+
+std::complex<double> expfn(std::complex<double> x, int)
+{
+  return std::exp(x);
+}
+
+int main()
+{
+  const double pi = std::acos(-1.0);
+
+  MatrixXd A(3,3);
+  A << 0,    -pi/4, 0,
+       pi/4, 0,     0,
+       0,    0,     0;
+
+  std::cout << "The matrix A is:\n" << A << "\n\n";
+  std::cout << "The matrix exponential of A is:\n" 
+            << A.matrixFunction(expfn) << "\n\n";
+}

libs/eigen/unsupported/doc/examples/MatrixLogarithm.cpp

@@ -0,0 +1,15 @@
+#include <unsupported/Eigen/MatrixFunctions>
+#include <iostream>
+
+using namespace Eigen;
+
+int main()
+{
+  using std::sqrt;
+  MatrixXd A(3,3);
+  A << 0.5*sqrt(2), -0.5*sqrt(2), 0,
+       0.5*sqrt(2),  0.5*sqrt(2), 0,
+       0,            0,           1;
+  std::cout << "The matrix A is:\n" << A << "\n\n";
+  std::cout << "The matrix logarithm of A is:\n" << A.log() << "\n";
+}

libs/eigen/unsupported/doc/examples/MatrixPower.cpp

@@ -0,0 +1,16 @@
+#include <unsupported/Eigen/MatrixFunctions>
+#include <iostream>
+
+using namespace Eigen;
+
+int main()
+{
+  const double pi = std::acos(-1.0);
+  Matrix3d A;
+  A << cos(1), -sin(1), 0,
+       sin(1),  cos(1), 0,
+	   0 ,      0 , 1;
+  std::cout << "The matrix A is:\n" << A << "\n\n"
+	       "The matrix power A^(pi/4) is:\n" << A.pow(pi/4) << std::endl;
+  return 0;
+}

libs/eigen/unsupported/doc/examples/MatrixPower_optimal.cpp

@@ -0,0 +1,17 @@
+#include <unsupported/Eigen/MatrixFunctions>
+#include <iostream>
+
+using namespace Eigen;
+
+int main()
+{
+  Matrix4cd A = Matrix4cd::Random();
+  MatrixPower<Matrix4cd> Apow(A);
+
+  std::cout << "The matrix A is:\n" << A << "\n\n"
+	       "A^3.1 is:\n" << Apow(3.1) << "\n\n"
+	       "A^3.3 is:\n" << Apow(3.3) << "\n\n"
+	       "A^3.7 is:\n" << Apow(3.7) << "\n\n"
+	       "A^3.9 is:\n" << Apow(3.9) << std::endl;
+  return 0;
+}

libs/eigen/unsupported/doc/examples/MatrixSine.cpp

@@ -0,0 +1,20 @@
+#include <unsupported/Eigen/MatrixFunctions>
+#include <iostream>
+
+using namespace Eigen;
+
+int main()
+{
+  MatrixXd A = MatrixXd::Random(3,3);
+  std::cout << "A = \n" << A << "\n\n";
+
+  MatrixXd sinA = A.sin();
+  std::cout << "sin(A) = \n" << sinA << "\n\n";
+
+  MatrixXd cosA = A.cos();
+  std::cout << "cos(A) = \n" << cosA << "\n\n";
+  
+  // The matrix functions satisfy sin^2(A) + cos^2(A) = I, 
+  // like the scalar functions.
+  std::cout << "sin^2(A) + cos^2(A) = \n" << sinA*sinA + cosA*cosA << "\n\n";
+}

libs/eigen/unsupported/doc/examples/MatrixSinh.cpp

@@ -0,0 +1,20 @@
+#include <unsupported/Eigen/MatrixFunctions>
+#include <iostream>
+
+using namespace Eigen;
+
+int main()
+{
+  MatrixXf A = MatrixXf::Random(3,3);
+  std::cout << "A = \n" << A << "\n\n";
+
+  MatrixXf sinhA = A.sinh();
+  std::cout << "sinh(A) = \n" << sinhA << "\n\n";
+
+  MatrixXf coshA = A.cosh();
+  std::cout << "cosh(A) = \n" << coshA << "\n\n";
+  
+  // The matrix functions satisfy cosh^2(A) - sinh^2(A) = I, 
+  // like the scalar functions.
+  std::cout << "cosh^2(A) - sinh^2(A) = \n" << coshA*coshA - sinhA*sinhA << "\n\n";
+}

libs/eigen/unsupported/doc/examples/MatrixSquareRoot.cpp

@@ -0,0 +1,16 @@
+#include <unsupported/Eigen/MatrixFunctions>
+#include <iostream>
+
+using namespace Eigen;
+
+int main()
+{
+  const double pi = std::acos(-1.0);
+
+  MatrixXd A(2,2);
+  A << cos(pi/3), -sin(pi/3), 
+       sin(pi/3),  cos(pi/3);
+  std::cout << "The matrix A is:\n" << A << "\n\n";
+  std::cout << "The matrix square root of A is:\n" << A.sqrt() << "\n\n";
+  std::cout << "The square of the last matrix is:\n" << A.sqrt() * A.sqrt() << "\n";
+}

libs/eigen/unsupported/doc/examples/PolynomialSolver1.cpp

@@ -0,0 +1,53 @@
+#include <unsupported/Eigen/Polynomials>
+#include <vector>
+#include <iostream>
+
+using namespace Eigen;
+using namespace std;
+
+int main()
+{
+  typedef Matrix<double,5,1> Vector5d;
+
+  Vector5d roots = Vector5d::Random();
+  cout << "Roots: " << roots.transpose() << endl;
+  Eigen::Matrix<double,6,1> polynomial;
+  roots_to_monicPolynomial( roots, polynomial );
+
+  PolynomialSolver<double,5> psolve( polynomial );
+  cout << "Complex roots: " << psolve.roots().transpose() << endl;
+
+  std::vector<double> realRoots;
+  psolve.realRoots( realRoots );
+  Map<Vector5d> mapRR( &realRoots[0] );
+  cout << "Real roots: " << mapRR.transpose() << endl;
+
+  cout << endl;
+  cout << "Illustration of the convergence problem with the QR algorithm: " << endl;
+  cout << "---------------------------------------------------------------" << endl;
+  Eigen::Matrix<float,7,1> hardCase_polynomial;
+  hardCase_polynomial <<
+  -0.957, 0.9219, 0.3516, 0.9453, -0.4023, -0.5508, -0.03125;
+  cout << "Hard case polynomial defined by floats: " << hardCase_polynomial.transpose() << endl;
+  PolynomialSolver<float,6> psolvef( hardCase_polynomial );
+  cout << "Complex roots: " << psolvef.roots().transpose() << endl;
+  Eigen::Matrix<float,6,1> evals;
+  for( int i=0; i<6; ++i ){ evals[i] = std::abs( poly_eval( hardCase_polynomial, psolvef.roots()[i] ) ); }
+  cout << "Norms of the evaluations of the polynomial at the roots: " << evals.transpose() << endl << endl;
+
+  cout << "Using double's almost always solves the problem for small degrees: " << endl;
+  cout << "-------------------------------------------------------------------" << endl;
+  PolynomialSolver<double,6> psolve6d( hardCase_polynomial.cast<double>() );
+  cout << "Complex roots: " << psolve6d.roots().transpose() << endl;
+  for( int i=0; i<6; ++i )
+  {
+    std::complex<float> castedRoot( psolve6d.roots()[i].real(), psolve6d.roots()[i].imag() );
+    evals[i] = std::abs( poly_eval( hardCase_polynomial, castedRoot ) );
+  }
+  cout << "Norms of the evaluations of the polynomial at the roots: " << evals.transpose() << endl << endl;
+
+  cout.precision(10);
+  cout << "The last root in float then in double: " << psolvef.roots()[5] << "\t" << psolve6d.roots()[5] << endl;
+  std::complex<float> castedRoot( psolve6d.roots()[5].real(), psolve6d.roots()[5].imag() );
+  cout << "Norm of the difference: " << std::abs( psolvef.roots()[5] - castedRoot ) << endl;
+}

libs/eigen/unsupported/doc/examples/PolynomialUtils1.cpp

@@ -0,0 +1,20 @@
+#include <unsupported/Eigen/Polynomials>
+#include <iostream>
+
+using namespace Eigen;
+using namespace std;
+
+int main()
+{
+  Vector4d roots = Vector4d::Random();
+  cout << "Roots: " << roots.transpose() << endl;
+  Eigen::Matrix<double,5,1> polynomial;
+  roots_to_monicPolynomial( roots, polynomial );
+  cout << "Polynomial: ";
+  for( int i=0; i<4; ++i ){ cout << polynomial[i] << ".x^" << i << "+ "; }
+  cout << polynomial[4] << ".x^4" << endl;
+  Vector4d evaluation;
+  for( int i=0; i<4; ++i ){
+    evaluation[i] = poly_eval( polynomial, roots[i] ); }
+  cout << "Evaluation of the polynomial at the roots: " << evaluation.transpose();
+}

libs/eigen/unsupported/doc/examples/SYCL/CMakeLists.txt

@@ -0,0 +1,37 @@
+FILE(GLOB examples_SRCS "*.cpp")
+
+set(EIGEN_SYCL ON)
+list(APPEND CMAKE_EXE_LINKER_FLAGS -pthread)
+if(EIGEN_SYCL_TRISYCL)
+  set(CMAKE_CXX_STANDARD 17)
+else(EIGEN_SYCL_TRISYCL)
+  if(MSVC)
+    # Set the host and device compilers C++ standard to C++14. On Windows setting this to C++11
+    # can cause issues with the ComputeCpp device compiler parsing Visual Studio Headers.
+    set(CMAKE_CXX_STANDARD 14)
+    list(APPEND COMPUTECPP_USER_FLAGS -DWIN32)
+  else()
+    set(CMAKE_CXX_STANDARD 11)
+    list(APPEND COMPUTECPP_USER_FLAGS -Wall)
+  endif()
+  # The following flags are not supported by Clang and can cause warnings
+  # if used with -Werror so they are removed here.
+  if(COMPUTECPP_USE_COMPILER_DRIVER)
+    set(CMAKE_CXX_COMPILER ${ComputeCpp_DEVICE_COMPILER_EXECUTABLE})
+    string(REPLACE "-Wlogical-op" "" CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS})
+    string(REPLACE "-Wno-psabi" "" CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS})
+    string(REPLACE "-ansi" "" CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS})
+  endif()
+  list(APPEND COMPUTECPP_USER_FLAGS
+      -DEIGEN_NO_ASSERTION_CHECKING=1
+      -no-serial-memop
+      -Xclang
+      -cl-mad-enable)
+endif(EIGEN_SYCL_TRISYCL)
+
+FOREACH(example_src ${examples_SRCS})
+  GET_FILENAME_COMPONENT(example ${example_src} NAME_WE)
+  ei_add_test_internal(${example} example_${example})
+  ADD_DEPENDENCIES(unsupported_examples example_${example})
+ENDFOREACH(example_src)
+set(EIGEN_SYCL OFF)

libs/eigen/unsupported/doc/examples/SYCL/CwiseMul.cpp

@@ -0,0 +1,63 @@
+#include <iostream>
+#define EIGEN_USE_SYCL
+#include <unsupported/Eigen/CXX11/Tensor>
+
+using Eigen::array;
+using Eigen::SyclDevice;
+using Eigen::Tensor;
+using Eigen::TensorMap;
+
+int main()
+{
+  using DataType = float;
+  using IndexType = int64_t;
+  constexpr auto DataLayout = Eigen::RowMajor;
+
+  auto devices = Eigen::get_sycl_supported_devices();
+  const auto device_selector = *devices.begin();
+  Eigen::QueueInterface queueInterface(device_selector);
+  auto sycl_device = Eigen::SyclDevice(&queueInterface);
+  
+  // create the tensors to be used in the operation
+  IndexType sizeDim1 = 3;
+  IndexType sizeDim2 = 3;
+  IndexType sizeDim3 = 3;
+  array<IndexType, 3> tensorRange = {{sizeDim1, sizeDim2, sizeDim3}};
+
+  // initialize the tensors with the data we want manipulate to
+  Tensor<DataType, 3,DataLayout, IndexType> in1(tensorRange);
+  Tensor<DataType, 3,DataLayout, IndexType> in2(tensorRange);
+  Tensor<DataType, 3,DataLayout, IndexType> out(tensorRange);
+
+  // set up some random data in the tensors to be multiplied
+  in1 = in1.random();
+  in2 = in2.random();
+
+  // allocate memory for the tensors
+  DataType * gpu_in1_data  = static_cast<DataType*>(sycl_device.allocate(in1.size()*sizeof(DataType)));
+  DataType * gpu_in2_data  = static_cast<DataType*>(sycl_device.allocate(in2.size()*sizeof(DataType)));
+  DataType * gpu_out_data =  static_cast<DataType*>(sycl_device.allocate(out.size()*sizeof(DataType)));
+
+  // 
+  TensorMap<Tensor<DataType, 3, DataLayout, IndexType>> gpu_in1(gpu_in1_data, tensorRange);
+  TensorMap<Tensor<DataType, 3, DataLayout, IndexType>> gpu_in2(gpu_in2_data, tensorRange);
+  TensorMap<Tensor<DataType, 3, DataLayout, IndexType>> gpu_out(gpu_out_data, tensorRange);
+
+  // copy the memory to the device and do the c=a*b calculation
+  sycl_device.memcpyHostToDevice(gpu_in1_data, in1.data(),(in1.size())*sizeof(DataType));
+  sycl_device.memcpyHostToDevice(gpu_in2_data, in2.data(),(in2.size())*sizeof(DataType));
+  gpu_out.device(sycl_device) = gpu_in1 * gpu_in2;
+  sycl_device.memcpyDeviceToHost(out.data(), gpu_out_data,(out.size())*sizeof(DataType));
+  sycl_device.synchronize();
+
+  // print out the results
+   for (IndexType i = 0; i < sizeDim1; ++i) {
+    for (IndexType j = 0; j < sizeDim2; ++j) {
+      for (IndexType k = 0; k < sizeDim3; ++k) {
+        std::cout << "device_out" << "(" << i << ", " << j << ", " << k << ") : " << out(i,j,k) 
+                  << " vs host_out" << "(" << i << ", " << j << ", " << k << ") : " << in1(i,j,k) * in2(i,j,k) << "\n";
+      }
+    }
+  }
+  printf("c=a*b Done\n");
+}