test/math/SquareMatrix3TestCase.cpp

#include "math/SquareMatrix3TestCase.hpp"

// Registers the fixture into the 'registry'
CPPUNIT_TEST_SUITE_REGISTRATION( SquareMatrix3TestCase );


void SquareMatrix3TestCase::testSetupRotationMatrix(){
    //test setupRotationMatrix by quaternion

    RotMat3x3d m1L(0.0, -0.6, 0.0, -0.8);
    RotMat3x3d m1R;
    m1R(0,0) = -0.28;
    m1R(0,1) =  0;
    m1R(0,2) =  0.96;
    m1R(1,0) =  0.0;
    m1R(1,1) = -1.0;
    m1R(1,2) =  0.0;
    m1R(2,0) = 0.96;
    m1R(2,1) = 0;
    m1R(2,2) = 0.28;

    CPPUNIT_ASSERT(m1L == m1R);

    Quat4d q1(0.0, -0.6, 0.0, -0.8);
    RotMat3x3d m2L(q1);
    RotMat3x3d m2R(m1R);
    CPPUNIT_ASSERT(m2L == m2R);
    
    //test setupRotationMatrix by euler angles
    Vector3d v1(0.0, M_PI/2.0, 0.0);
    RotMat3x3d m3L(v1);
    RotMat3x3d m3R;
    m3R(0,0) = 1.0;
    m3R(0,1) = 0;
    m3R(0,2) = 0.0;
    m3R(1,0) = 0.0;
    m3R(1,1) = 0.0;
    m3R(1,2) = 1.0;
    m3R(2,0) = 0.0;
    m3R(2,1) = -1.0;
    m3R(2,2) = 0.0;    
    CPPUNIT_ASSERT( m3L == m3R);

    RotMat3x3d m4L(0.0, M_PI/2.0, 0.0);
    RotMat3x3d m4R = m3R;    
    CPPUNIT_ASSERT( m4L == m4R);    

    Vector3d v2(M_PI/4.0, M_PI/4.0, M_PI/4.0);
    RotMat3x3d m5L(v2);
    RotMat3x3d m5R;
    double root2Over4 = sqrt(2)/4.0;
    m5R(0,0) = 0.5 - root2Over4;
    m5R(0,1) = 0.5 + root2Over4;
    m5R(0,2) = 0.5;
    m5R(1,0) = -0.5 -root2Over4;
    m5R(1,1) = -0.5 + root2Over4;
    m5R(1,2) = 0.5;
    m5R(2,0) = 0.5;
    m5R(2,1) = -0.5;
    m5R(2,2) = sqrt(2)/2.0;    
    CPPUNIT_ASSERT( m5L == m5R);

    
}

void SquareMatrix3TestCase::testOtherMemberFunctions() {
    //test inverse
    RotMat3x3d ident = RotMat3x3d::identity();
    CPPUNIT_ASSERT(ident == ident.inverse());

    RotMat3x3d m1;
    m1(0,0) = 1.0;
    m1(0,1) = 5.0;
    m1(0,2) = 3.0;
    m1(1,0) = 3.0;
    m1(1,1) = 1.0;
    m1(1,2) = 2.0;
    m1(2,0) = 0.0;
    m1(2,1) = -21.0;
    m1(2,2) = -81.0; 
    
    CPPUNIT_ASSERT(m1 == (m1.inverse()).inverse());
    
    //test determinant
    RotMat3x3d m2;
    m2(0,0) = 1.0;
    m2(0,1) = 5.0;
    m2(0,2) = 3.0;
    m2(1,0) = 6.0;
    m2(1,1) = 0.0;
    m2(1,2) = 2.0;
    m2(2,0) = 0.0;
    m2(2,1) = -1.0;
    m2(2,2) = 1.0; 
    CPPUNIT_ASSERT_DOUBLES_EQUAL(m2.determinant(), -46.0, oopse::epsilon);
}
void SquareMatrix3TestCase::testTransformation(){

    //test toQuaternion
    RotMat3x3d m1;
    Quat4d q1L;
    Quat4d q1R(0.0, -0.6, 0.0, -0.8);
    m1(0,0) = -0.28;
    m1(0,1) =  0;
    m1(0,2) =  0.96;
    m1(1,0) =  0.0;
    m1(1,1) = -1.0;
    m1(1,2) =  0.0;
    m1(2,0) = 0.96;
    m1(2,1) = 0;
    m1(2,2) = 0.28;
    q1L = m1.toQuaternion();
    //CPPUNIT_ASSERT( q1L == q1R);    

    RotMat3x3d m2;
    Quat4d q2L(0.4, -0.6, 0.3, -0.8);
    Quat4d q2R;
    q2L.normalize();

    m2 = q2L.toRotationMatrix3();    
    q2R = m2.toQuaternion();
    CPPUNIT_ASSERT( q2L == q2R);    
    
    //test toEuler
    Vector3d v1L;
    Vector3d v1R(M_PI/4.0, M_PI/4.0, M_PI/4.0);
    RotMat3x3d m3;
    double root2Over4 = sqrt(2)/4.0;
    m3(0,0) = 0.5 - root2Over4;
    m3(0,1) = 0.5 + root2Over4;
    m3(0,2) = 0.5;
    m3(1,0) = -0.5 -root2Over4;
    m3(1,1) = -0.5 + root2Over4;
    m3(1,2) = 0.5;
    m3(2,0) = 0.5;
    m3(2,1) = -0.5;
    m3(2,2) = sqrt(2)/2.0;    
    v1L = m3.toEulerAngles();
    CPPUNIT_ASSERT( v1L == v1R);

    //test diagonalize

    RotMat3x3d m4;    
    RotMat3x3d a;
    Vector3d w;
    RotMat3x3d m5L;
    RotMat3x3d m5R;
    m4(0, 0) = 3.0;
    m4(0, 1) = 4.0;
    m4(0, 2) = 5.0;
    m4(1, 0) = 4.0;
    m4(1, 1) = 5.0;
    m4(1, 2) = 6.0;    
    m4(2, 0) = 5.0;
    m4(2, 1) = 6.0;
    m4(2, 2) = 7.0; 
    a = m4;
    
    RotMat3x3d::diagonalize(a, w, m5L);

    m5R(0, 0) = 0.789067 ;
    m5R(0, 1) = -0.408248;
    m5R(0, 2) = 0.459028;
    m5R(1, 0) = 0.090750;
    m5R(1, 1) = 0.816497;
    m5R(1, 2) = 0.570173;    
    m5R(2, 0) = -0.607567;
    m5R(2, 1) = -0.408248 ;
    m5R(2, 2) = 0.681319; 

    CPPUNIT_ASSERT(m5L == m5R);
}
Revision:	1616
Committed:	Wed Oct 20 18:07:08 2004 UTC (19 years, 8 months ago) by tim
File size:	3925 byte(s)
Log Message:	Math library pass the unit test
#	User	Rev	Content
1	tim	1593	#include "math/SquareMatrix3TestCase.hpp"
2
3			// Registers the fixture into the 'registry'
4			CPPUNIT_TEST_SUITE_REGISTRATION( SquareMatrix3TestCase );
5
6
7	tim	1603	void SquareMatrix3TestCase::testSetupRotationMatrix(){
8			//test setupRotationMatrix by quaternion
9	tim	1593
10	tim	1606	RotMat3x3d m1L(0.0, -0.6, 0.0, -0.8);
11			RotMat3x3d m1R;
12			m1R(0,0) = -0.28;
13			m1R(0,1) = 0;
14			m1R(0,2) = 0.96;
15			m1R(1,0) = 0.0;
16			m1R(1,1) = -1.0;
17			m1R(1,2) = 0.0;
18			m1R(2,0) = 0.96;
19			m1R(2,1) = 0;
20			m1R(2,2) = 0.28;
21	tim	1593
22	tim	1606	CPPUNIT_ASSERT(m1L == m1R);
23
24			Quat4d q1(0.0, -0.6, 0.0, -0.8);
25			RotMat3x3d m2L(q1);
26			RotMat3x3d m2R(m1R);
27			CPPUNIT_ASSERT(m2L == m2R);
28
29	tim	1603	//test setupRotationMatrix by euler angles
30	tim	1606	Vector3d v1(0.0, M_PI/2.0, 0.0);
31			RotMat3x3d m3L(v1);
32			RotMat3x3d m3R;
33			m3R(0,0) = 1.0;
34			m3R(0,1) = 0;
35			m3R(0,2) = 0.0;
36			m3R(1,0) = 0.0;
37			m3R(1,1) = 0.0;
38			m3R(1,2) = 1.0;
39			m3R(2,0) = 0.0;
40			m3R(2,1) = -1.0;
41			m3R(2,2) = 0.0;
42			CPPUNIT_ASSERT( m3L == m3R);
43
44			RotMat3x3d m4L(0.0, M_PI/2.0, 0.0);
45			RotMat3x3d m4R = m3R;
46			CPPUNIT_ASSERT( m4L == m4R);
47
48			Vector3d v2(M_PI/4.0, M_PI/4.0, M_PI/4.0);
49			RotMat3x3d m5L(v2);
50			RotMat3x3d m5R;
51			double root2Over4 = sqrt(2)/4.0;
52			m5R(0,0) = 0.5 - root2Over4;
53			m5R(0,1) = 0.5 + root2Over4;
54			m5R(0,2) = 0.5;
55			m5R(1,0) = -0.5 -root2Over4;
56			m5R(1,1) = -0.5 + root2Over4;
57			m5R(1,2) = 0.5;
58			m5R(2,0) = 0.5;
59			m5R(2,1) = -0.5;
60			m5R(2,2) = sqrt(2)/2.0;
61			CPPUNIT_ASSERT( m5L == m5R);
62
63
64	tim	1593	}
65
66	tim	1603	void SquareMatrix3TestCase::testOtherMemberFunctions() {
67			//test inverse
68	tim	1606	RotMat3x3d ident = RotMat3x3d::identity();
69			CPPUNIT_ASSERT(ident == ident.inverse());
70	tim	1593
71	tim	1606	RotMat3x3d m1;
72			m1(0,0) = 1.0;
73			m1(0,1) = 5.0;
74			m1(0,2) = 3.0;
75			m1(1,0) = 3.0;
76			m1(1,1) = 1.0;
77			m1(1,2) = 2.0;
78			m1(2,0) = 0.0;
79			m1(2,1) = -21.0;
80			m1(2,2) = -81.0;
81
82			CPPUNIT_ASSERT(m1 == (m1.inverse()).inverse());
83
84	tim	1603	//test determinant
85	tim	1606	RotMat3x3d m2;
86			m2(0,0) = 1.0;
87			m2(0,1) = 5.0;
88			m2(0,2) = 3.0;
89			m2(1,0) = 6.0;
90			m2(1,1) = 0.0;
91			m2(1,2) = 2.0;
92			m2(2,0) = 0.0;
93			m2(2,1) = -1.0;
94			m2(2,2) = 1.0;
95			CPPUNIT_ASSERT_DOUBLES_EQUAL(m2.determinant(), -46.0, oopse::epsilon);
96	tim	1593	}
97	tim	1603	void SquareMatrix3TestCase::testTransformation(){
98	tim	1593
99	tim	1603	//test toQuaternion
100	tim	1606	RotMat3x3d m1;
101			Quat4d q1L;
102			Quat4d q1R(0.0, -0.6, 0.0, -0.8);
103			m1(0,0) = -0.28;
104			m1(0,1) = 0;
105			m1(0,2) = 0.96;
106			m1(1,0) = 0.0;
107			m1(1,1) = -1.0;
108			m1(1,2) = 0.0;
109			m1(2,0) = 0.96;
110			m1(2,1) = 0;
111			m1(2,2) = 0.28;
112			q1L = m1.toQuaternion();
113			//CPPUNIT_ASSERT( q1L == q1R);
114	tim	1593
115	tim	1606	RotMat3x3d m2;
116			Quat4d q2L(0.4, -0.6, 0.3, -0.8);
117			Quat4d q2R;
118			q2L.normalize();
119
120			m2 = q2L.toRotationMatrix3();
121			q2R = m2.toQuaternion();
122			CPPUNIT_ASSERT( q2L == q2R);
123
124	tim	1603	//test toEuler
125	tim	1606	Vector3d v1L;
126			Vector3d v1R(M_PI/4.0, M_PI/4.0, M_PI/4.0);
127			RotMat3x3d m3;
128			double root2Over4 = sqrt(2)/4.0;
129			m3(0,0) = 0.5 - root2Over4;
130			m3(0,1) = 0.5 + root2Over4;
131			m3(0,2) = 0.5;
132			m3(1,0) = -0.5 -root2Over4;
133			m3(1,1) = -0.5 + root2Over4;
134			m3(1,2) = 0.5;
135			m3(2,0) = 0.5;
136			m3(2,1) = -0.5;
137			m3(2,2) = sqrt(2)/2.0;
138			v1L = m3.toEulerAngles();
139			CPPUNIT_ASSERT( v1L == v1R);
140	tim	1603
141			//test diagonalize
142	tim	1606
143	tim	1616	RotMat3x3d m4;
144	tim	1606	RotMat3x3d a;
145			Vector3d w;
146	tim	1616	RotMat3x3d m5L;
147			RotMat3x3d m5R;
148			m4(0, 0) = 3.0;
149			m4(0, 1) = 4.0;
150			m4(0, 2) = 5.0;
151			m4(1, 0) = 4.0;
152			m4(1, 1) = 5.0;
153			m4(1, 2) = 6.0;
154			m4(2, 0) = 5.0;
155			m4(2, 1) = 6.0;
156			m4(2, 2) = 7.0;
157			a = m4;
158
159			RotMat3x3d::diagonalize(a, w, m5L);
160	tim	1606
161	tim	1616	m5R(0, 0) = 0.789067 ;
162			m5R(0, 1) = -0.408248;
163			m5R(0, 2) = 0.459028;
164			m5R(1, 0) = 0.090750;
165			m5R(1, 1) = 0.816497;
166			m5R(1, 2) = 0.570173;
167			m5R(2, 0) = -0.607567;
168			m5R(2, 1) = -0.408248 ;
169			m5R(2, 2) = 0.681319;
170	tim	1606
171	tim	1616	CPPUNIT_ASSERT(m5L == m5R);
172	tim	1593	}