[3] | 1 | #ifndef __UTILITIES_HPP__
|
---|
| 2 | #define __UTILITIES_HPP__
|
---|
| 3 |
|
---|
| 4 |
|
---|
| 5 | namespace math {
|
---|
| 6 |
|
---|
| 7 | namespace utility {
|
---|
| 8 |
|
---|
| 9 |
|
---|
| 10 | /*!
|
---|
| 11 | * \fn inline std::vector<T> LineCircleIntersection(const boost::numeric::ublas::vector<T> & A,const boost::numeric::ublas::vector<T> & B, const boost::numeric::ublas::vector<T> & C,const double R)
|
---|
| 12 | * \brief Compute points of intersection of circle and line defined through two poinst A and B
|
---|
| 13 | * \param A : a point of the line
|
---|
| 14 | * \param B : a point of the line
|
---|
| 15 | * \param C :
|
---|
| 16 | * \param R : circle radius
|
---|
| 17 | * \return a list of abscissas with respect of point A
|
---|
| 18 | */
|
---|
| 19 | template <class T> inline std::vector<T> LineCircleIntersection(const boost::numeric::ublas::vector<T> & A,
|
---|
| 20 | const boost::numeric::ublas::vector<T> & B,
|
---|
| 21 | const boost::numeric::ublas::vector<T> & C,
|
---|
| 22 | const double R){
|
---|
| 23 | std::vector<T> intersection;
|
---|
| 24 |
|
---|
| 25 | double alpha = (B[0]-A[0])*(B[0]-A[0]) + (B[1]-A[1])*(B[1]-A[1]);
|
---|
| 26 | double norm = std::sqrt(alpha);
|
---|
| 27 | double beta = 2 *(B[0]-A[0])*(A[0]-C[0]) + (B[1]-A[1])*(A[1]-C[1]);
|
---|
| 28 | double gamma = A[0]*A[0] + A[1]*A[1] + C[0]*C[0] + C[1]*C[1] - 2*(A[0]*C[0] + A[1]*C[1] ) - R*R;
|
---|
| 29 | double delta = beta*beta - 4*alpha*gamma;
|
---|
| 30 |
|
---|
| 31 | if(delta>0){
|
---|
| 32 | intersection.push_back( (-beta -sqrt(delta))/(2*norm) );
|
---|
| 33 | intersection.push_back( (-beta +sqrt(delta))/(2*norm) );
|
---|
| 34 | }
|
---|
| 35 |
|
---|
| 36 | return intersection;
|
---|
| 37 | }
|
---|
| 38 |
|
---|
| 39 | /*!
|
---|
| 40 | * \fn inline std::vector<T> SegmentCircleIntersection(const boost::numeric::ublas::vector<T> & A,const boost::numeric::ublas::vector<T> & B, const boost::numeric::ublas::vector<T> & C,const double R)
|
---|
| 41 | * \brief Compute points of intersection of circle and segment defined through two poinst A and B
|
---|
| 42 | * \param A : a point of the line
|
---|
| 43 | * \param B : a point of the line
|
---|
| 44 | * \param C :
|
---|
| 45 | * \param R : circle radius
|
---|
| 46 | * \return a list of abscissas with respect of point A and in taking account the hypothesis abscissas in [A,B]
|
---|
| 47 | */
|
---|
| 48 | template <class T> inline std::vector<T> SegmentCircleIntersection(const boost::numeric::ublas::vector<T> & A,
|
---|
| 49 | const boost::numeric::ublas::vector<T> & B,
|
---|
| 50 | const boost::numeric::ublas::vector<T> & C,
|
---|
| 51 | const double R){
|
---|
| 52 |
|
---|
| 53 |
|
---|
| 54 | std::vector<T> intersection;
|
---|
| 55 | double alpha = (B[0] - A[0]) * (B[0] - A[0]) + (B[1] - A[1]) * (B[1] - A[1]);
|
---|
| 56 | double norm = std::sqrt(alpha);
|
---|
| 57 | double beta = 2 * ((B[0] - A[0]) * (A[0] - C[0]) + (B[1] - A[1]) * (A[1] - C[1]));
|
---|
| 58 | double gamma = A[0] * A[0] + A[1] * A[1] + C[0] * C[0] + C[1] * C[1] - 2 * (A[0] * C[0] + A[1] * C[1]) - R * R;
|
---|
| 59 |
|
---|
| 60 | double delta = beta*beta - 4*alpha*gamma;
|
---|
| 61 |
|
---|
| 62 | if(delta>0){
|
---|
| 63 | intersection.push_back( (-beta -sqrt(delta))/(2*norm) );
|
---|
| 64 | intersection.push_back( (-beta +sqrt(delta))/(2*norm) );
|
---|
| 65 |
|
---|
| 66 | if(intersection[0]<1 && intersection[1]>0){
|
---|
| 67 | if(intersection[0] <0 ) intersection[0]=0;
|
---|
| 68 | if(intersection[1] >norm) intersection[1]=norm;
|
---|
| 69 | }else{
|
---|
| 70 | intersection.clear();
|
---|
| 71 | }
|
---|
| 72 | }
|
---|
| 73 |
|
---|
| 74 |
|
---|
| 75 | return intersection;
|
---|
| 76 | }
|
---|
| 77 |
|
---|
| 78 | /*!
|
---|
| 79 | * \fn inline boost::numeric::ublas::vector<RealType> Cov2Ellipse(const RealType & pxx,const RealType & pxy,const RealType & pyy,const RealType &proba)
|
---|
| 80 | * \brief Convert 2D covariance to a ellipse parameters
|
---|
| 81 | * \param pxx : variance X
|
---|
| 82 | * \param pxy : covariance XY
|
---|
| 83 | * \param pyy : variance Y
|
---|
| 84 | * \param proba : percentage
|
---|
| 85 | * \return ublas vector containing semi-major axis, semi-minor axis and orientation of the ellipse
|
---|
| 86 | */
|
---|
| 87 | template <class RealType> inline boost::numeric::ublas::vector<RealType> Cov2Ellipse(const RealType & pxx,const RealType & pxy,const RealType & pyy,const RealType &proba){
|
---|
| 88 |
|
---|
| 89 | boost::numeric::ublas::vector<RealType> ellipse(3);
|
---|
| 90 |
|
---|
| 91 | // le scalaire "k" definit l'ellipse avec l'equation :(x-mx)T*(1/P)*(x-mx)=k^2
|
---|
| 92 | double k=sqrt(-2*log(1-proba));
|
---|
| 93 |
|
---|
| 94 | // coeficient de correlation
|
---|
| 95 | double ro = pxy / sqrt(pxx * pyy);
|
---|
| 96 | if ( fabs( ro ) > 1 )
|
---|
| 97 | {
|
---|
| 98 | std::cout << "ro=" << ro << "pxx=" << pxx << "pxy=" << pxy << "pyy=" << pyy << std::endl;
|
---|
| 99 | throw math_error("Cov2Ellipse: correlation coefficient is not included between -1 and 1. Covariance matrix is not defined positive");
|
---|
| 100 | }
|
---|
| 101 | double a = 1/(pxx*(1- ro * ro));
|
---|
| 102 | double b = -ro/(sqrt(pyy*pxx)*(1- ro * ro));
|
---|
| 103 | double c = 1/(pyy*(1- ro * ro));
|
---|
| 104 |
|
---|
| 105 | // calcul des deux valeurs propres
|
---|
| 106 | // la gde vp (lambda1) est associee au petit axe.
|
---|
| 107 | double delta = (a-c)*(a-c)+4*b*b;
|
---|
| 108 | double lambda1 = 0.5*(a+c+sqrt(delta));
|
---|
| 109 | double lambda2 = 0.5*(a+c-sqrt(delta));
|
---|
| 110 |
|
---|
| 111 | // vecteur directeur du grand axe
|
---|
| 112 | double aux = (lambda2-a)/b;
|
---|
| 113 | double deno=sqrt(1+aux*aux);
|
---|
| 114 | double Ux = 1/deno;
|
---|
| 115 | double Uy = aux/deno;
|
---|
| 116 |
|
---|
| 117 | // longueur des axes dans le repere propre
|
---|
| 118 | double axeX = k/sqrt(lambda2); // demi axe
|
---|
| 119 | double axeY = k/sqrt(lambda1); // demi axe
|
---|
| 120 |
|
---|
| 121 | ellipse(2) = - atan2(Uy, Ux);//heading
|
---|
| 122 | ellipse(0) = axeY * 2 * 3; // width x3 (sigma) si PROBA = 0.4 ellipsoide a deux dimensions (test du khi2)
|
---|
| 123 | ellipse(1) = axeX * 2 * 3; //height
|
---|
| 124 |
|
---|
| 125 | // heading = - atan2(Uy, Ux);
|
---|
| 126 | // width = axeY * 2 * 3; // x3 (sigma) si PROBA = 0.4 ellipsoide a deux dimensions (test du khi2)
|
---|
| 127 | // height = axeX * 2 * 3;
|
---|
| 128 |
|
---|
| 129 | return ellipse;
|
---|
| 130 | }
|
---|
| 131 |
|
---|
| 132 | /*!
|
---|
| 133 | * \fn inline boost::numeric::ublas::vector<RealType> Cov2Ellipse(boost::numeric::ublas::matrix<RealType> P,const RealType &proba)
|
---|
| 134 | * \brief Convert 2D covariance to a ellipse parameters
|
---|
| 135 | * \param P : 2D covariance matrix
|
---|
| 136 | * \param proba :
|
---|
| 137 | * \return ublas vector containing semi-major axis, semi-minor axis and orientation of the ellipse
|
---|
| 138 | */
|
---|
| 139 | template <class RealType> inline boost::numeric::ublas::vector<RealType> Cov2Ellipse(boost::numeric::ublas::matrix<RealType> P,const RealType &proba){
|
---|
| 140 | if(P.size1()==2 & P.size2()==2) throw math_error("Cov2Ellipse: covariance is not a 2D square matrix");
|
---|
| 141 | return Cov2Ellipse(P(0,0),P(0,1),P(1,1),proba);
|
---|
| 142 | }
|
---|
| 143 |
|
---|
| 144 | };
|
---|
| 145 | };
|
---|
| 146 | #endif
|
---|