source: pacpusframework/trunk/include/Pacpus/PacpusTools/math/utilities.hpp@ 64

Last change on this file since 64 was 64, checked in by Marek Kurdej, 11 years ago

Modified property: added svn:keywords=Id.
  • Property svn:keywords set to Id
File size: 6.5 KB
Line 
1#ifndef __UTILITIES_HPP__
2#define __UTILITIES_HPP__
3
4
5namespace 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
Note: See TracBrowser for help on using the repository browser.