00001
00002
00003 namespace jafar {
00004 namespace geom {
00005 template<int dimension>
00006 inline void cosAngleCov( const typename Atom<dimension>::HomogenousVecD& v1,
00007 const typename Atom<dimension>::HomogenousSymMatrixD& v1Cov,
00008 const typename Atom<dimension>::HomogenousVecD& v2,
00009 const typename Atom<dimension>::HomogenousSymMatrixD& v2Cov,
00010 double& angle, double &cov)
00011 {
00012 JFR_ASSERT( fabs(ublas::norm_2(v1) - 1.0) < 1e-6, "Vector norm should be equal to 1");
00013 JFR_ASSERT( fabs(ublas::norm_2(v2) - 1.0) < 1e-6, "Vector norm should be equal to 1");
00014 angle = ublas::inner_prod( v1, v2 );
00015 if( angle < -1.0) angle = -1.0;
00016 else if( angle > 1.0 ) angle = 1.0;
00017 typedef typename Atom<dimension>::HomogenousVecD HomogenousVecD;
00018
00019 cov = (ublas::inner_prod( HomogenousVecD( ublas::prod(v2, v1Cov) ), v2 )
00020 + ublas::inner_prod( HomogenousVecD( ublas::prod(v1, v2Cov) ), v1 ) );
00021
00022 }
00023 template<int dimension>
00024 inline void cosAngleCov(const Line<dimension>& line1, const Line<dimension>& line2,
00025 double& angle, double &cov)
00026 {
00027 cosAngleCov<dimension>( line1.direction(), line1.directionCov(), line2.direction(), line2.directionCov(),
00028 angle, cov );
00029 }
00030 template<int dimension>
00031 inline void cosAngleCov(const Line<dimension>& line1, const HyperPlane<dimension>& hp2,
00032 double& angle, double &cov)
00033 {
00034 cosAngleCov<dimension>( line1.direction(), line1.directionCov(), hp2.normal(), hp2.normalCov(), angle, cov );
00035 }
00036 template<int dimension>
00037 inline void cosAngleCov(const HyperPlane<dimension>& hp2, const Line<dimension>& line1,
00038 double& angle, double &cov)
00039 {
00040 cosAngleCov( line1, hp2, angle, cov );
00041 }
00042 template<int dimension>
00043 inline void cosAngleCov(const Segment<dimension>& line1, const Segment<dimension>& line2,
00044 double& angle, double &cov)
00045 {
00046 cosAngleCov<dimension>( line1.direction(), line1.directionCov(), line2.direction(), line2.directionCov(),
00047 angle, cov );
00048 }
00049 template<int dimension>
00050 inline void cosAngleCov(const Segment<dimension>& line1, const HyperPlane<dimension>& hp2,
00051 double& angle, double &cov)
00052 {
00053 cosAngleCov<dimension>( line1.direction(), line1.directionCov(), hp2.normal(), hp2.normalCov(), angle, cov );
00054 }
00055 template<int dimension>
00056 inline void cosAngleCov(const HyperPlane<dimension>& hp2, const Segment<dimension>& line1,
00057 double& angle, double &cov)
00058 {
00059 cosAngleCov( line1, hp2, angle, cov );
00060 }
00061 template<int dimension>
00062 inline void cosAngleCov(const HyperPlane<dimension>& hp1, const HyperPlane<dimension>& hp2,
00063 double& angle, double &cov)
00064 {
00065 cosAngleCov<dimension>( hp1.normal(), hp1.normalCov(), hp2.normal(), hp2.normalCov(), angle, cov );
00066 }
00067 }
00068 }
