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 (21 years ago) by tim
File size:	3925 byte(s)
Log Message:	Math library pass the unit test
#	Content
1	#include "math/SquareMatrix3TestCase.hpp"
2
3	// Registers the fixture into the 'registry'
4	CPPUNIT_TEST_SUITE_REGISTRATION( SquareMatrix3TestCase );
5
6
7	void SquareMatrix3TestCase::testSetupRotationMatrix(){
8	//test setupRotationMatrix by quaternion
9
10	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
22	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	//test setupRotationMatrix by euler angles
30	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	}
65
66	void SquareMatrix3TestCase::testOtherMemberFunctions() {
67	//test inverse
68	RotMat3x3d ident = RotMat3x3d::identity();
69	CPPUNIT_ASSERT(ident == ident.inverse());
70
71	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	//test determinant
85	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	}
97	void SquareMatrix3TestCase::testTransformation(){
98
99	//test toQuaternion
100	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
115	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	//test toEuler
125	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
141	//test diagonalize
142
143	RotMat3x3d m4;
144	RotMat3x3d a;
145	Vector3d w;
146	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
161	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
171	CPPUNIT_ASSERT(m5L == m5R);
172	}