source: pacpusframework/branches/0.0.x/include/Pacpus/PacpusTools/math/utilities.hpp@ 337

Last change on this file since 337 was 76, checked in by Marek Kurdej, 12 years ago

Added: automated license updating lines:
%pacpus:license{
%pacpus:license}
  • Property svn:keywords set to Id
File size: 7.0 KB
Line 
1// %pacpus:license{
2// This file is part of the PACPUS framework distributed under the
3// CECILL-C License, Version 1.0.
4// %pacpus:license}
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
18namespace 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
Note: See TracBrowser for help on using the repository browser.