[Toon-members] TooN/functions fadbad.h

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    TooN
Changes by:     Gerhard Reitmayr <gerhard>      11/05/01 15:27:56

Modified files:
        functions      : fadbad.h 

Log message:
        faster FADBAD SE3/SO3 creation

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/functions/fadbad.h?cvsroot=toon&r1=1.3&r2=1.4

Patches:
Index: fadbad.h
===================================================================
RCS file: /cvsroot/toon/TooN/functions/fadbad.h,v
retrieving revision 1.3
retrieving revision 1.4
diff -u -b -r1.3 -r1.4
--- fadbad.h    22 Apr 2011 21:05:23 -0000      1.3
+++ fadbad.h    1 May 2011 15:27:56 -0000       1.4
@@ -102,13 +102,25 @@
 }
 
 inline SO3<fadbad::F<double> > make_fad_so3(int start = 0, int size = 3){
-       const Vector<3, fadbad::F<double> > p = make_fad_vector(makeVector(0.0, 
0, 0), start, size);
-       return SO3<fadbad::F<double> >(p);
+       // const Vector<3, fadbad::F<double> > p = 
make_fad_vector(makeVector(0.0, 0, 0), start, size);
+       // return SO3<fadbad::F<double> >(p);
+       SO3<fadbad::F<double> > r;
+       // this is a hack
+       Matrix<3,3,fadbad::F<double> > & m = 
const_cast<Matrix<3,3,fadbad::F<double> > &>(r.get_matrix());
+       m(2,1).diff(start, size);
+       m(1,2) = m(2,1) * -1;
+
+       m(0,2).diff(start+1, size);
+       m(2,0) = m(0,2) * -1;
+
+       m(1,0).diff(start+2, size);
+       m(0,1) = m(1,0) * -1;
+
+       return r;
 }
 
 inline SE3<fadbad::F<double> > make_fad_se3( int start = 0, int size = 6){
-       const Vector<6, fadbad::F<double> > p = make_fad_vector(makeVector(0.0, 
0, 0, 0, 0, 0), start, size);
-       return SE3<fadbad::F<double> >(p);
+       return SE3<fadbad::F<double> >(make_fad_so3( start+3, size ), 
make_fad_vector(makeVector(0.0, 0, 0), start, size));
 }
 
 inline SE2<fadbad::F<double> > make_fad_se2(int start = 0, int size = 3) {