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

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

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