[Toon-members] TooN so3.h test/SXX_test.cc

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    TooN
Changes by:     Gerhard Reitmayr <gerhard>      09/04/30 17:50:40

Modified files:
        .              : so3.h 
        test           : SXX_test.cc 

Log message:
        create SO3 R from two vectors a, b such that b = R * a around a ^ b

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/so3.h?cvsroot=toon&r1=1.35&r2=1.36
http://cvs.savannah.gnu.org/viewcvs/TooN/test/SXX_test.cc?cvsroot=toon&r1=1.8&r2=1.9

Patches:
Index: so3.h
===================================================================
RCS file: /cvsroot/toon/TooN/so3.h,v
retrieving revision 1.35
retrieving revision 1.36
diff -u -b -r1.35 -r1.36
--- so3.h       29 Apr 2009 16:05:50 -0000      1.35
+++ so3.h       30 Apr 2009 17:50:38 -0000      1.36
@@ -64,6 +64,30 @@
        template <int R, int C, typename P, typename A>
        SO3(const Matrix<R,C,P,A>& rhs) { *this = rhs; }
        
+       /// creates an SO3 as a rotation that takes Vector a into the direction 
of Vector b
+       /// with the rotation axis along a ^ b. If |a ^ b| == 0, it creates the 
identity rotation.
+       /// @param a source Vector
+       /// @param b target Vector
+       template <int S1, int S2, typename P1, typename P2, typename A1, 
typename A2>
+       SO3(const Vector<S1, P1, A1> & a, const Vector<S2, P2, A2> & b ){
+               SizeMismatch<3,S1>::test(3, a.size());
+               SizeMismatch<3,S2>::test(3, b.size());
+               Vector<3, Precision> n = a ^ b;
+               if(norm_sq(n) == 0) {
+                       my_matrix = Identity;
+                       return;
+               }
+               n = unit(n);
+               Matrix<3> R1;
+               R1.T()[0] = unit(a);
+               R1.T()[1] = n;
+               R1.T()[2] = n ^ R1.T()[0];
+               my_matrix.T()[0] = unit(b);
+               my_matrix.T()[1] = n;
+               my_matrix.T()[2] = n ^ my_matrix.T()[0];
+               my_matrix = my_matrix * R1.T();
+       }
+       
        /// Assigment operator from a general matrix. This also calls coerce()
        /// to make sure that the matrix is a valid rotation matrix.
        template <int R, int C, typename P, typename A>

Index: test/SXX_test.cc
===================================================================
RCS file: /cvsroot/toon/TooN/test/SXX_test.cc,v
retrieving revision 1.8
retrieving revision 1.9
diff -u -b -r1.8 -r1.9
--- test/SXX_test.cc    27 Apr 2009 13:36:33 -0000      1.8
+++ test/SXX_test.cc    30 Apr 2009 17:50:40 -0000      1.9
@@ -165,6 +165,13 @@
     istringstream is("0 -1 0 1 0 0 0 0 1");
     is >> r;
     cout << r << endl;
+
+    cout << "test rotation constructor\n";
+    TooN::Vector<3> a = TooN::makeVector(1,0,0), b = TooN::makeVector(0,1,1);
+    TooN::SO3<> rr(a,b);
+    cout << "a " << a << " to b " << b << " is\n" << rr << endl;
+    cout << "R * a " << rr * a << endl;
+    cout << "a " << a << " to itself is\n" << TooN::SO3<>(a,a) << endl;
 }
 
 void test_se3(){