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