1 /* -------------------------------------------------------------------------- *
2 * Simbody(tm): SimTKcommon *
3 * -------------------------------------------------------------------------- *
4 * This is part of the SimTK biosimulation toolkit originating from *
5 * Simbios, the NIH National Center for Physics-Based Simulation of *
6 * Biological Structures at Stanford, funded under the NIH Roadmap for *
7 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
8 * *
9 * Portions copyright (c) 2005-12 Stanford University and the Authors. *
10 * Authors: Paul Mitiguy *
11 * Contributors: Michael Sherman *
12 * *
13 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
14 * not use this file except in compliance with the License. You may obtain a *
15 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
16 * *
17 * Unless required by applicable law or agreed to in writing, software *
18 * distributed under the License is distributed on an "AS IS" BASIS, *
19 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
20 * See the License for the specific language governing permissions and *
21 * limitations under the License. *
22 * -------------------------------------------------------------------------- */
23
24 /**@file
25 * Tests for the classes defined in Rotation.h.
26 */
27
28 //-----------------------------------------------------------------------------
29 #include "SimTKcommon.h"
30 #include "SimTKcommon/Testing.h"
31
32 #include <iostream>
33 using std::cout; using std::endl;
34
35 //-----------------------------------------------------------------------------
36 using namespace SimTK;
37
38 bool doRequiredTasks();
WriteStringToScreen(const char outputString[])39 void WriteStringToScreen( const char outputString[] ) { std::cout << outputString; } // fputs( outputString,stdout ); if include<stdio>
40
41 //-------------------------------------------------------------------
main()42 int main() {
43
44 // Default value is program failed
45 bool programSucceeded = false;
46
47 // It is a good programming practice to do little in the main function of a program.
48 // The try-catch code in this main routine catches exceptions thrown by functions in the
49 // try block, e.g., catching an exception that occurs when a NULL pointer is de-referenced.
50 try {
51 // Do the required programming tasks
52 programSucceeded = doRequiredTasks();
53 }
54 // This catch statement handles certain types of exceptions
55 catch( const std::exception& e ) {
56 WriteStringToScreen( "\n\n Error: Programming error encountered.\n The exception thrown is: " );
57 WriteStringToScreen( e.what() );
58 WriteStringToScreen( "\n\n" );
59 }
60 // The exception-declaration statement (...) handles any type of exception,
61 // including C exceptions and system/application generated exceptions.
62 // This includes exceptions such as memory protection and floating-point violations.
63 // An ellipsis catch handler must be the last handler for its try block.
64 catch( ... ) {
65 WriteStringToScreen( "\n\n Error: Programming error encountered.\n An unhandled exception was thrown.\n\n" );
66 }
67
68 if (!programSucceeded)
69 WriteStringToScreen("\nError: a test failed.\n");
70
71 // The value returned by the main function is the exit status of the program.
72 // A normal program exit returns 0 (other return values usually signal an error).
73 return programSucceeded == true ? 0 : 1;
74 }
75
76
77 //-------------------------------------------------------------------
78 // Prototypes for methods in this test
79 //-------------------------------------------------------------------
80 bool testRotationOneAxis( const Real angle, const CoordinateAxis& axis );
81 bool testRotationTwoAxes( const BodyOrSpaceType bodyOrSpace, const Real angle1, const CoordinateAxis& axis1, const Real angle2, const CoordinateAxis &axis2 );
82 bool testRotationThreeAxes( const BodyOrSpaceType bodyOrSpace, const Real angle1, const CoordinateAxis& axis1, const Real angle2, const CoordinateAxis &axis2, const Real angle3, const CoordinateAxis &axis3 );
83 bool testQuaternion( Real e0, Real e1, Real e2, Real e3 );
84 bool testSetRotationToBodyFixedXYZ();
85
86 bool testInverseRotation1Angle( Real angle, Real theta );
87 bool testInverseRotation2Angle( Real angle1, Real theta1, Real angle2, Real theta2 );
88 bool testInverseRotation3AngleTwoAxes( Real angle1, Real theta1, Real angle2, Real theta2, Real angle3, Real theta3 );
89 bool testInverseRotation3AngleThreeAxes( Real angle1, Real theta1, Real angle2, Real theta2, Real angle3, Real theta3 );
90
91
92 bool exhaustiveTestof1AngleRotation();
93 bool exhaustiveTestof2AngleRotation();
94 bool exhaustiveTestof3AngleRotation();
95 bool exhaustiveTestof3AngleTwoAxesRotationNearSingularity();
96 bool exhaustiveTestof3AngleThreeAxesRotationNearSingularity();
97 bool exhaustiveTestofQuaternions();
98
99 bool testRotationFromTwoGivenAxes( const Vec3& vi, const CoordinateAxis& ai, const Vec3& vj, const CoordinateAxis& aj);
100 bool testReexpressSymMat33();
101
102 //-------------------------------------------------------------------
doRequiredTasks()103 bool doRequiredTasks( ) {
104
105 // Use the next Rotation to test against (simple theta-lambda rotation)
106 Rotation testRotation;
107
108 // Test default constructor
109 Rotation defaultRotationConstructor;
110 testRotation.setRotationToIdentityMatrix();
111 bool test = defaultRotationConstructor.areAllRotationElementsSameToMachinePrecision( testRotation );
112
113 // Test copy constructor
114 testRotation.setRotationFromAngleAboutNonUnitVector( 1.0, Vec3(0.2, 0.4, 0.6) );
115 Rotation rotationCopyConstructor( testRotation );
116 test = test && rotationCopyConstructor.areAllRotationElementsSameToMachinePrecision( testRotation );
117
118 // Test operator =
119 Rotation rotationOperatorEqual = testRotation;
120 test = test && rotationOperatorEqual.areAllRotationElementsSameToMachinePrecision( testRotation );
121
122 // Test rotation by angle about arbitrary CoordinateAxis
123 testRotation.setRotationFromAngleAboutNonUnitVector( 0.1, Vec3(1.0, 0.0, 0.0) );
124 CoordinateAxis coordAxis = XAxis;
125 Rotation rotationCoordAxis( 0.1, coordAxis );
126 test = test && rotationCoordAxis.areAllRotationElementsSameToMachinePrecision( testRotation );
127
128 Real testTheta = rotationCoordAxis.convertOneAxisRotationToOneAngle( coordAxis );
129 test = test && fabs(0.1 - testTheta) < 10*SignificantReal;
130
131 // Test rotation by angle about XAxis, YAxis, ZAxis
132 test = test && testRotationOneAxis( 0.2, XAxis );
133 test = test && testRotationOneAxis( -0.2, XAxis );
134 test = test && testRotationOneAxis( 2.1, YAxis );
135 test = test && testRotationOneAxis( -2.1, YAxis );
136 test = test && testRotationOneAxis( 3.1, ZAxis );
137 test = test && testRotationOneAxis( -3.1, ZAxis );
138
139 // Test rotation with two angles and two axes XX, XY, XZ
140 test = test && testRotationTwoAxes( BodyRotationSequence, 0.2, XAxis, 0.3, XAxis );
141 test = test && testRotationTwoAxes( SpaceRotationSequence, 0.2, XAxis, 0.3, XAxis );
142 test = test && testRotationTwoAxes( BodyRotationSequence, 1.2, XAxis,-1.3, YAxis );
143 test = test && testRotationTwoAxes( SpaceRotationSequence, 1.2, XAxis,-1.3, YAxis );
144 test = test && testRotationTwoAxes( BodyRotationSequence, -3.1, XAxis, 1.2, ZAxis );
145 test = test && testRotationTwoAxes( SpaceRotationSequence, -3.1, XAxis, 1.2, ZAxis );
146
147 // Test rotation with two angles and two axes YX, YY, YZ
148 test = test && testRotationTwoAxes( BodyRotationSequence, 1.2, YAxis, 0.3, XAxis );
149 test = test && testRotationTwoAxes( SpaceRotationSequence, 1.2, YAxis, 0.3, XAxis );
150 test = test && testRotationTwoAxes( BodyRotationSequence, 2.2, YAxis,-1.3, YAxis );
151 test = test && testRotationTwoAxes( SpaceRotationSequence, 2.2, YAxis,-1.3, YAxis );
152 test = test && testRotationTwoAxes( BodyRotationSequence, -3.1, YAxis, 1.2, ZAxis );
153 test = test && testRotationTwoAxes( SpaceRotationSequence, -3.1, YAxis, 1.2, ZAxis );
154
155 // Test rotation with two angles and two axes ZX, ZY, ZZ
156 test = test && testRotationTwoAxes( BodyRotationSequence, 1.2, ZAxis, 0.3, XAxis );
157 test = test && testRotationTwoAxes( SpaceRotationSequence, 1.2, ZAxis, 0.3, XAxis );
158 test = test && testRotationTwoAxes( BodyRotationSequence, 2.2, ZAxis,-1.3, YAxis );
159 test = test && testRotationTwoAxes( SpaceRotationSequence, 2.2, ZAxis,-1.3, YAxis );
160 test = test && testRotationTwoAxes( BodyRotationSequence, -3.1, ZAxis, 1.2, ZAxis );
161 test = test && testRotationTwoAxes( SpaceRotationSequence, -3.1, ZAxis, 1.2, ZAxis );
162
163 // Test the construction of rotations from two given axes.
164 const UnitVec3 vi(0.01, 0.02, .9);
165 const Vec3 vj(-0.5, 0.5, 0.2);
166
167 test = test && testRotationFromTwoGivenAxes(vi, XAxis, vj, YAxis);
168 test = test && testRotationFromTwoGivenAxes(vi, YAxis, vj, XAxis);
169
170 test = test && testRotationFromTwoGivenAxes(vi, YAxis, vj, ZAxis);
171 test = test && testRotationFromTwoGivenAxes(vi, ZAxis, vj, YAxis);
172
173 test = test && testRotationFromTwoGivenAxes(vi, ZAxis, vj, XAxis);
174 test = test && testRotationFromTwoGivenAxes(vi, XAxis, vj, ZAxis);
175
176 // Test rotation with three angles and three axes XXX, XXY, XXZ, XYX, XYY, XYZ, XZX, XZY, XZZ
177 test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, XAxis, 0.3, XAxis, 0.4, XAxis );
178 test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, XAxis, 0.3, XAxis, 0.4, XAxis );
179 test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, XAxis,-1.3, XAxis,-1.4, YAxis );
180 test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, XAxis,-1.3, XAxis,-1.4, YAxis );
181 test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, XAxis, 1.2, XAxis, 1.3, ZAxis );
182 test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, XAxis, 1.2, XAxis, 1.3, ZAxis );
183
184 test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, XAxis, 0.3, YAxis, 0.4, XAxis );
185 test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, XAxis, 0.3, YAxis, 0.4, XAxis );
186 test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, XAxis,-1.3, YAxis,-1.4, YAxis );
187 test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, XAxis,-1.3, YAxis,-1.4, YAxis );
188 test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, XAxis, 1.2, YAxis, 1.3, ZAxis );
189 test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, XAxis, 1.2, YAxis, 1.3, ZAxis );
190
191 test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, XAxis, 0.3, ZAxis, 0.4, XAxis );
192 test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, XAxis, 0.3, ZAxis, 0.4, XAxis );
193 test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, XAxis,-1.3, ZAxis,-1.4, YAxis );
194 test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, XAxis,-1.3, ZAxis,-1.4, YAxis );
195 test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, XAxis, 1.2, ZAxis, 1.3, ZAxis );
196 test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, XAxis, 1.2, ZAxis, 1.3, ZAxis );
197
198 // Test rotation with three angles and three axes YXX, YXY, YXZ, YYX, YYY, YYZ, YZX, YZY, YZZ
199 test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, YAxis, 0.3, XAxis, 0.4, XAxis );
200 test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, YAxis, 0.3, XAxis, 0.4, XAxis );
201 test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, YAxis,-1.3, XAxis,-1.4, YAxis );
202 test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, YAxis,-1.3, XAxis,-1.4, YAxis );
203 test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, YAxis, 1.2, XAxis, 1.3, ZAxis );
204 test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, YAxis, 1.2, XAxis, 1.3, ZAxis );
205
206 test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, YAxis, 0.3, YAxis, 0.4, XAxis );
207 test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, YAxis, 0.3, YAxis, 0.4, XAxis );
208 test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, YAxis,-1.3, YAxis,-1.4, YAxis );
209 test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, YAxis,-1.3, YAxis,-1.4, YAxis );
210 test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, YAxis, 1.2, YAxis, 1.3, ZAxis );
211 test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, YAxis, 1.2, YAxis, 1.3, ZAxis );
212
213 test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, YAxis, 0.3, ZAxis, 0.4, XAxis );
214 test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, YAxis, 0.3, ZAxis, 0.4, XAxis );
215 test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, YAxis,-1.3, ZAxis,-1.4, YAxis );
216 test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, YAxis,-1.3, ZAxis,-1.4, YAxis );
217 test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, YAxis, 1.2, ZAxis, 1.3, ZAxis );
218 test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, YAxis, 1.2, ZAxis, 1.3, ZAxis );
219
220 // Test rotation with three angles and three axes ZXX, ZXY, ZXZ, ZYX, ZYY, ZYZ, ZZX, ZZY, ZZZ
221 test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, ZAxis, 0.3, XAxis, 0.4, XAxis );
222 test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, ZAxis, 0.3, XAxis, 0.4, XAxis );
223 test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, ZAxis,-1.3, XAxis,-1.4, YAxis );
224 test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, ZAxis,-1.3, XAxis,-1.4, YAxis );
225 test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, ZAxis, 1.2, XAxis, 1.3, ZAxis );
226 test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, ZAxis, 1.2, XAxis, 1.3, ZAxis );
227
228 test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, ZAxis, 0.3, YAxis, 0.4, XAxis );
229 test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, ZAxis, 0.3, YAxis, 0.4, XAxis );
230 test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, ZAxis,-1.3, YAxis,-1.4, YAxis );
231 test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, ZAxis,-1.3, YAxis,-1.4, YAxis );
232 test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, ZAxis, 1.2, YAxis, 1.3, ZAxis );
233 test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, ZAxis, 1.2, YAxis, 1.3, ZAxis );
234
235 test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, ZAxis, 0.3, ZAxis, 0.4, XAxis );
236 test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, ZAxis, 0.3, ZAxis, 0.4, XAxis );
237 test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, ZAxis,-1.3, ZAxis,-1.4, YAxis );
238 test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, ZAxis,-1.3, ZAxis,-1.4, YAxis );
239 test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, ZAxis, 1.2, ZAxis, 1.3, ZAxis );
240 test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, ZAxis, 1.2, ZAxis, 1.3, ZAxis );
241
242 // This used to fail
243 test = test && testRotationThreeAxes( BodyRotationSequence, -3.2288591161895095, XAxis, -3.1415926535897931, YAxis, -3.1415926535897931, XAxis );
244 test = test && testRotationThreeAxes( SpaceRotationSequence, -3.2288591161895095, XAxis, -3.1415926535897931, YAxis, -3.1415926535897931, XAxis );
245
246 // Test Rotation quaternion methods.
247 test = test && testQuaternion( 0.5, 0.1, 0.2, 0.3 );
248 test = test && testQuaternion(-0.5, 0.1, 0.2, -0.3 );
249
250 Quaternion unnorm(Vec4(1,2,3,4), true); // don't do this at home
251 SimTK_TEST_NOTEQ(unnorm.norm(), Real(1)); // shouldn't have normalized
252 Quaternion fixedUp;
253 fixedUp = unnorm.normalize();
254 SimTK_TEST_EQ(fixedUp.norm(), Real(1));
255 unnorm.normalizeThis();
256 SimTK_TEST_EQ(unnorm.norm(), Real(1));
257
258
259 // Test construction of nearby orthogonal rotation matrix from a generic Mat33.
260 Rotation nearbyRotation( testRotation.asMat33() );
261 test = test && nearbyRotation.areAllRotationElementsSameToMachinePrecision( testRotation );
262
263 // Exhaustive test of 1-angle, 2-angle, and 3-angle rotations
264 test = test && exhaustiveTestof1AngleRotation();
265 test = test && exhaustiveTestof2AngleRotation();
266 test = test && exhaustiveTestof3AngleRotation();
267
268 // Exhaustive test of 3-angle rotations near singularity
269 test = test && exhaustiveTestof3AngleTwoAxesRotationNearSingularity();
270 test = test && exhaustiveTestof3AngleThreeAxesRotationNearSingularity();
271
272 // Exhaustive test of Quaterions
273 test = test && exhaustiveTestofQuaternions();
274
275 // Test special handling of body-fixed XYZ.
276 test = test && testSetRotationToBodyFixedXYZ();
277
278 // Test out special code for rotating symmetric matrices.
279 test = test && testReexpressSymMat33();
280
281 return test;
282 }
283
284
285 //-------------------------------------------------------------------
testRotationOneAxis(const Real angle,const CoordinateAxis & axis)286 bool testRotationOneAxis( const Real angle, const CoordinateAxis& axis ) {
287
288 // Form rotation about specified axis
289 Rotation rotationSpecified;
290 if( axis == XAxis ) rotationSpecified.setRotationFromAngleAboutX( angle );
291 if( axis == YAxis ) rotationSpecified.setRotationFromAngleAboutY( angle );
292 if( axis == ZAxis ) rotationSpecified.setRotationFromAngleAboutZ( angle );
293
294 // Form equivalent rotation by another means
295 Real unitX = axis == XAxis ? 1 : 0;
296 Real unitY = axis == YAxis ? 1 : 0;
297 Real unitZ = axis == ZAxis ? 1 : 0;
298 UnitVec3 unitVector( unitX, unitY, unitZ );
299 Rotation testRotation( angle, unitVector );
300
301 // Test to see if they are the same
302 bool test = rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );
303
304 // Do the inverse problem to back out the angle
305 const Real theta = rotationSpecified.convertOneAxisRotationToOneAngle( axis );
306
307 // Create a Rotation matrix with the backed-out angle and compare to the original Rotation matrix
308 testRotation.setRotationFromAngleAboutAxis( theta, axis );
309 test = test && rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );
310
311 // Conversion should produce angle = theta if angle is in proper range (-pi < angle <= pi)
312 test = test && testInverseRotation1Angle( angle, theta );
313
314 return test;
315 }
316
317
318 //-------------------------------------------------------------------
testRotationTwoAxes(const BodyOrSpaceType bodyOrSpace,const Real angle1,const CoordinateAxis & axis1,const Real angle2,const CoordinateAxis & axis2)319 bool testRotationTwoAxes( const BodyOrSpaceType bodyOrSpace, const Real angle1, const CoordinateAxis& axis1, const Real angle2, const CoordinateAxis &axis2 ) {
320
321 // Form rotation about specified axes
322 Rotation rotationSpecified( bodyOrSpace, angle1, axis1, angle2, axis2 );
323
324 // Form equivalent rotation by another means
325 Rotation AB( angle1, axis1 );
326 Rotation BC( angle2, axis2 );
327 Rotation testRotation = (bodyOrSpace == BodyRotationSequence) ? AB * BC : BC * AB;
328
329 // Test to see if they are the same
330 bool test = rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );
331
332 // Do the inverse problem to back out the angles
333 const Vec2 testVec = rotationSpecified.convertTwoAxesRotationToTwoAngles( bodyOrSpace, axis1, axis2 );
334 const Real theta1 = testVec[0];
335 const Real theta2 = testVec[1];
336
337 // Create a Rotation matrix with the backed-out angles and compare to the original Rotation matrix
338 testRotation.setRotationFromTwoAnglesTwoAxes( bodyOrSpace, theta1, axis1, theta2, axis2 );
339 test = test && rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );
340
341 // Conversion should produce same angles for for appropriate ranges of angle1 and angle2
342 if( axis1.isSameAxis(axis2) )
343 test = test && testInverseRotation1Angle( angle1+angle2, theta1+theta2 );
344 else
345 test = test && testInverseRotation2Angle( angle1,theta1, angle2,theta2 );
346
347 return test;
348 }
349
350
351 //-------------------------------------------------------------------
testRotationThreeAxes(const BodyOrSpaceType bodyOrSpace,const Real angle1,const CoordinateAxis & axis1,const Real angle2,const CoordinateAxis & axis2,const Real angle3,const CoordinateAxis & axis3)352 bool testRotationThreeAxes( const BodyOrSpaceType bodyOrSpace, const Real angle1, const CoordinateAxis& axis1, const Real angle2, const CoordinateAxis &axis2, const Real angle3, const CoordinateAxis &axis3 ) {
353
354 // Form rotation about specified axes
355 Rotation rotationSpecified( bodyOrSpace, angle1, axis1, angle2, axis2, angle3, axis3 );
356
357 // Form equivalent rotation by another means
358 Rotation AB( angle1, axis1 );
359 Rotation BC( angle2, axis2 );
360 Rotation CD( angle3, axis3 );
361 Rotation testRotation = (bodyOrSpace == BodyRotationSequence) ? AB * BC * CD : CD * BC * AB;
362
363 // Test to see if they are the same
364 bool test = rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );
365
366 // Do the inverse problem to back out the angles
367 const Vec3 testVec = rotationSpecified.convertThreeAxesRotationToThreeAngles( bodyOrSpace, axis1, axis2, axis3 );
368 const Real theta1 = testVec[0];
369 const Real theta2 = testVec[1];
370 const Real theta3 = testVec[2];
371
372 // Create a Rotation matrix with the backed-out angles and compare to the original Rotation matrix
373 testRotation.setRotationFromThreeAnglesThreeAxes( bodyOrSpace, theta1, axis1, theta2, axis2, theta3, axis3 );
374 test = test && rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );
375
376 // Conversion should produce same angles for for appropriate ranges of angle1 and angle2
377 if( axis1.areAllSameAxes(axis2,axis3) )
378 test = test && testInverseRotation1Angle( angle1+angle2+angle3, theta1+theta2+theta3 );
379 else if( axis1.isSameAxis(axis2) )
380 test = test && testInverseRotation2Angle( angle1+angle2, theta1+theta1, angle3,theta3 );
381 else if( axis2.isSameAxis(axis3) )
382 test = test && testInverseRotation2Angle( angle1,theta1, angle2+angle3, theta2+theta3 );
383 else if( axis1.isSameAxis(axis3) )
384 test = test && testInverseRotation3AngleTwoAxes( angle1,theta1, angle2,theta2, angle3,theta3 );
385 else
386 test = test && testInverseRotation3AngleThreeAxes( angle1,theta1, angle2,theta2, angle3,theta3 );
387
388 return test;
389 }
390
391
392 //-------------------------------------------------------------------
testInverseRotation1Angle(Real angle,Real theta)393 bool testInverseRotation1Angle( Real angle, Real theta ) {
394 bool test = true;
395 bool angleInProperRange = ( -SimTK_PI <= angle && angle <= SimTK_PI );
396 if( angleInProperRange )
397 test = fabs( angle - theta ) < 10*SignificantReal;
398 return test;
399 }
400
401
402 //-------------------------------------------------------------------
testInverseRotation2Angle(Real angle1,Real theta1,Real angle2,Real theta2)403 bool testInverseRotation2Angle( Real angle1, Real theta1, Real angle2, Real theta2 ) {
404 bool test = true;
405 bool angle1InProperRange = ( -SimTK_PI <= angle1 && angle1 <= SimTK_PI );
406 bool angle2InProperRange = ( -SimTK_PI <= angle2 && angle2 <= SimTK_PI );
407 if( angle1InProperRange && angle2InProperRange ) {
408 test = test && fabs( angle1 - theta1 ) < 10*SignificantReal;
409 test = test && fabs( angle2 - theta2 ) < 10*SignificantReal;
410 }
411 return test;
412 }
413
414
415 //-------------------------------------------------------------------
testInverseRotation3AngleTwoAxes(Real angle1,Real theta1,Real angle2,Real theta2,Real angle3,Real theta3)416 bool testInverseRotation3AngleTwoAxes( Real angle1, Real theta1, Real angle2, Real theta2, Real angle3, Real theta3 ) {
417 bool test = true;
418 bool angle1InProperRange = ( -SimTK_PI <= angle1 && angle1 <= SimTK_PI );
419 bool angle2InProperRange = ( 0 <= angle2 && angle2 <= SimTK_PI );
420 bool angle3InProperRange = ( -SimTK_PI <= angle3 && angle3 <= SimTK_PI );
421 if( angle1InProperRange && angle2InProperRange && angle3InProperRange ) {
422 test = test && fabs( angle1 - theta1 ) < 10*SignificantReal;
423 test = test && fabs( angle2 - theta2 ) < 10*SignificantReal;
424 test = test && fabs( angle3 - theta3 ) < 10*SignificantReal;
425
426 // Test needs to be modified if near singularity
427 const Real singularity = 0.0;
428 if( test == false && fabs(angle2-singularity) <= SignificantReal ) {
429 const Real angle1PlusAngle3 = angle1 + angle3;
430 const Real theta1PlusTheta3 = theta1 + theta3;
431 bool angleSumInProperRange = ( -SimTK_PI <= angle1PlusAngle3 && angle1PlusAngle3 <= SimTK_PI );
432 if( angleSumInProperRange == false ) test = true;
433 else {
434 test = fabs( angle2 - theta2 ) < 10*SignificantReal;
435 test = test && fabs( angle1PlusAngle3 - theta1PlusTheta3 ) < 10*SignificantReal;
436 }
437 }
438 }
439 return test;
440 }
441
442
443 //-------------------------------------------------------------------
testInverseRotation3AngleThreeAxes(Real angle1,Real theta1,Real angle2,Real theta2,Real angle3,Real theta3)444 bool testInverseRotation3AngleThreeAxes( Real angle1, Real theta1, Real angle2, Real theta2, Real angle3, Real theta3 ) {
445 bool test = true;
446 bool angle1InProperRange = ( -SimTK_PI <= angle1 && angle1 <= SimTK_PI );
447 bool angle2InProperRange = (-0.5*SimTK_PI <= angle2 && angle2 <= 0.5*SimTK_PI );
448 bool angle3InProperRange = ( -SimTK_PI <= angle3 && angle3 <= SimTK_PI );
449 if( angle1InProperRange && angle2InProperRange && angle3InProperRange ) {
450 test = test && fabs( angle1 - theta1 ) < 10*SignificantReal;
451 test = test && fabs( angle2 - theta2 ) < 10*SignificantReal;
452 test = test && fabs( angle3 - theta3 ) < 10*SignificantReal;
453
454 // Test needs to be modified if near singularity
455 const Real singularity = 0.5*SimTK_PI;
456 if( test == false && fabs(angle2-singularity) <= SignificantReal ) {
457 const Real angle1PlusAngle3 = angle1 + angle3;
458 const Real theta1PlusTheta3 = theta1 + theta3;
459 bool angleSumInProperRange = ( -SimTK_PI <= angle1PlusAngle3 && angle1PlusAngle3 <= SimTK_PI );
460 if( angleSumInProperRange == false ) test = true;
461 else {
462 test = fabs( angle2 - theta2 ) < 10*SignificantReal;
463 test = test && fabs( angle1PlusAngle3 - theta1PlusTheta3 ) < 10*SignificantReal;
464 }
465 }
466 }
467 return test;
468 }
469
470
471 //-------------------------------------------------------------------
testQuaternion(Real e0,Real e1,Real e2,Real e3)472 bool testQuaternion( Real e0, Real e1, Real e2, Real e3 )
473 {
474 // Construct quaternion and normalize it
475 Quaternion qe0e1e2e3( e0, e1, e2, e3 );
476
477 // Convert quaternion to a Rotation matrix
478 Rotation rotationSpecified( qe0e1e2e3 );
479
480 // Convert Rotation back to quaternion
481 Quaternion qTest = rotationSpecified.convertRotationToQuaternion();
482
483 // Convert quaternion to a Rotation matrix
484 Rotation testRotation; testRotation.setRotationFromQuaternion( qTest );
485
486 // Test to see if they are the same
487 bool test = rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );
488
489 return test;
490 }
491
492 //-------------------------------------------------------------------
testSetRotationToBodyFixedXYZ()493 bool testSetRotationToBodyFixedXYZ() {
494 bool test = true;
495 const Real q0=.123, q1=-.234, q2=.787;
496 Rotation R0, R1, R2;
497 // The general case and special case implementation should produce
498 // the same result.
499 R0.setRotationFromThreeAnglesThreeAxes(
500 BodyRotationSequence, q0, XAxis, q1, YAxis, q2, ZAxis);
501 R1.setRotationToBodyFixedXYZ(Vec3(q0,q1,q2));
502 R2.setRotationToBodyFixedXYZ(
503 Vec3(std::cos(q0),std::cos(q1),std::cos(q2)),
504 Vec3(std::sin(q0),std::sin(q1),std::sin(q2)));
505
506 test = test && R0.areAllRotationElementsSameToMachinePrecision(R1);
507 test = test && R0.areAllRotationElementsSameToMachinePrecision(R2);
508 test = test && R1.areAllRotationElementsSameToMachinePrecision(R2);
509
510 return test;
511 }
512
513
514 //-------------------------------------------------------------------
exhaustiveTestof1AngleRotation()515 bool exhaustiveTestof1AngleRotation( ) {
516 bool test = true;
517
518 // Range to check angles
519 Real negativeStartAngle = convertDegreesToRadians( -385 );
520 Real positiveStartAngle = convertDegreesToRadians( 385 );
521 Real incrementAngle = convertDegreesToRadians( 0.5 );
522
523 // Test each axis
524 for( int i=0; i<=2; i++ ) {
525 CoordinateAxis axisi = CoordinateAxis::getCoordinateAxis(i);
526 for( Real anglei = negativeStartAngle; anglei < positiveStartAngle; anglei += incrementAngle )
527 test = test && testRotationOneAxis( anglei, axisi );
528 }
529 return test;
530 }
531
532
533 //-------------------------------------------------------------------
exhaustiveTestof2AngleRotation()534 bool exhaustiveTestof2AngleRotation( ) {
535 bool test = true;
536
537 // Range to check angles
538 Real negativeStartAngle = convertDegreesToRadians( -200 );
539 Real positiveStartAngle = convertDegreesToRadians( 200 );
540 Real incrementAngle = convertDegreesToRadians( 10.0 );
541
542 // Test each axis
543 for( int i=0; i<=2; i++ ) {
544 CoordinateAxis axisi = CoordinateAxis::getCoordinateAxis(i);
545
546 for( int j=0; j<=2; j++ ) {
547 CoordinateAxis axisj = CoordinateAxis::getCoordinateAxis(j);
548
549 for( Real anglei = negativeStartAngle; anglei < positiveStartAngle; anglei += incrementAngle )
550 for( Real anglej = negativeStartAngle; anglej < positiveStartAngle; anglej += incrementAngle ) {
551 test = test && testRotationTwoAxes( BodyRotationSequence, anglei, axisi, anglej, axisj );
552 test = test && testRotationTwoAxes( SpaceRotationSequence, anglei, axisi, anglej, axisj );
553 }
554 }
555 }
556 return test;
557 }
558
559
560 //-------------------------------------------------------------------
exhaustiveTestof3AngleRotation()561 bool exhaustiveTestof3AngleRotation( ) {
562 bool test = true;
563
564 // Range to check angles
565 Real negativeStartAngle = convertDegreesToRadians( -200 );
566 Real positiveStartAngle = convertDegreesToRadians( 200 );
567 Real incrementAngle = convertDegreesToRadians( 40.0 );
568
569 // Test each axis
570 for( int i=0; i<=2; i++ ) {
571 CoordinateAxis axisi = CoordinateAxis::getCoordinateAxis(i);
572
573 for( int j=0; j<=2; j++ ) {
574 CoordinateAxis axisj = CoordinateAxis::getCoordinateAxis(j);
575
576 for( int k=0; k<=2; k++ ) {
577 CoordinateAxis axisk = CoordinateAxis::getCoordinateAxis(k);
578
579 for( Real anglei = negativeStartAngle; anglei < positiveStartAngle; anglei += incrementAngle )
580 for( Real anglej = negativeStartAngle; anglej < positiveStartAngle; anglej += incrementAngle )
581 for( Real anglek = negativeStartAngle; anglek < positiveStartAngle; anglek += incrementAngle ) {
582 test = test && testRotationThreeAxes( BodyRotationSequence, anglei, axisi, anglej, axisj, anglek, axisk );
583 test = test && testRotationThreeAxes( SpaceRotationSequence, anglei, axisi, anglej, axisj, anglek, axisk );
584 }
585 }
586 }
587 }
588 return test;
589 }
590
591
592 //-------------------------------------------------------------------
exhaustiveTestof3AngleTwoAxesRotationNearSingularity()593 bool exhaustiveTestof3AngleTwoAxesRotationNearSingularity() {
594 bool test = true;
595
596 // Range to check angles anglei and anglek
597 Real negativeStartAngle = convertDegreesToRadians( -200 );
598 Real positiveStartAngle = convertDegreesToRadians( 200 );
599 Real incrementStartAngle = convertDegreesToRadians( 20.0 );
600
601 // Range to check around singularity
602 Real negativeSinglAngle = convertDegreesToRadians( 0 - 1.0E-11 );
603 Real positiveSinglAngle = convertDegreesToRadians( 0 + 1.0E-11 );
604 Real incrementSinglAngle = convertDegreesToRadians( 1.0E-12 );
605
606 // Test each axis
607 for( int i=0; i<=2; i++ ) {
608 CoordinateAxis axisi = CoordinateAxis::getCoordinateAxis(i);
609
610 for( int j=0; j<=2; j++ ) {
611 CoordinateAxis axisj = CoordinateAxis::getCoordinateAxis(j);
612
613 for( int k=0; k<=2; k++ ) {
614 CoordinateAxis axisk = CoordinateAxis::getCoordinateAxis(k);
615
616 for( Real anglei = negativeStartAngle; anglei < positiveStartAngle; anglei += incrementStartAngle )
617 for( Real anglej = negativeSinglAngle; anglej < positiveSinglAngle; anglej += incrementSinglAngle )
618 for( Real anglek = negativeStartAngle; anglek < positiveStartAngle; anglek += incrementStartAngle ) {
619 test = test && testRotationThreeAxes( BodyRotationSequence, anglei, axisi, anglej, axisj, anglek, axisk );
620 test = test && testRotationThreeAxes( SpaceRotationSequence, anglei, axisi, anglej, axisj, anglek, axisk );
621 }
622 }
623 }
624 }
625 return test;
626 }
627
628
629 //-------------------------------------------------------------------
exhaustiveTestof3AngleThreeAxesRotationNearSingularity()630 bool exhaustiveTestof3AngleThreeAxesRotationNearSingularity() {
631 bool test = true;
632
633 // Range to check angles anglei and anglek
634 Real negativeStartAngle = convertDegreesToRadians( -200 );
635 Real positiveStartAngle = convertDegreesToRadians( 200 );
636 Real incrementStartAngle = convertDegreesToRadians( 20.0 );
637
638 // Range to check around singularity
639 Real negativeSinglAngle = convertDegreesToRadians( 90 - 1.0E-11 );
640 Real positiveSinglAngle = convertDegreesToRadians( 90 + 1.0E-11 );
641 Real incrementSinglAngle = convertDegreesToRadians( 1.0E-12 );
642
643 // Test each axis
644 for( int i=0; i<=2; i++ ) {
645 CoordinateAxis axisi = CoordinateAxis::getCoordinateAxis(i);
646
647 for( int j=0; j<=2; j++ ) {
648 CoordinateAxis axisj = CoordinateAxis::getCoordinateAxis(j);
649
650 for( int k=0; k<=2; k++ ) {
651 CoordinateAxis axisk = CoordinateAxis::getCoordinateAxis(k);
652
653 for( Real anglei = negativeStartAngle; anglei < positiveStartAngle; anglei += incrementStartAngle )
654 for( Real anglej = negativeSinglAngle; anglej < positiveSinglAngle; anglej += incrementSinglAngle )
655 for( Real anglek = negativeStartAngle; anglek < positiveStartAngle; anglek += incrementStartAngle ) {
656 test = test && testRotationThreeAxes( BodyRotationSequence, anglei, axisi, anglej, axisj, anglek, axisk );
657 test = test && testRotationThreeAxes( SpaceRotationSequence, anglei, axisi, anglej, axisj, anglek, axisk );
658 }
659 }
660 }
661 }
662 return test;
663 }
664
665
666 //-------------------------------------------------------------------
exhaustiveTestofQuaternions()667 bool exhaustiveTestofQuaternions() {
668 bool test = true;
669
670 for( Real e0 = -1; e0 <= 1; e0 += 0.2 )
671 for( Real e1 = -1; e1 <= 1; e1 += 0.2 )
672 for( Real e2 = -1; e2 <= 1; e2 += 0.2 )
673 for( Real e3 = -1; e3 <= 1; e3 += 0.2 )
674 test = test && testQuaternion( e0, e1, e2, e3 );
675
676 return test;
677 }
678
679 //-------------------------------------------------------------------
680 // Here we need to check that the resulting rotation has determinant 1 (a previous bug
681 // left it with determinant -1) and that the j'th axis is at least pointing in the
682 // direction of the given vj.
testRotationFromTwoGivenAxes(const Vec3 & vi,const CoordinateAxis & ai,const Vec3 & vj,const CoordinateAxis & aj)683 bool testRotationFromTwoGivenAxes( const Vec3& vi, const CoordinateAxis& ai, const Vec3& vj, const CoordinateAxis& aj) {
684 bool test = true;
685
686 // This makes a Rotation with vi as axis i, but axis j will only be in the general direction of vj.
687 const Rotation testRotation(UnitVec3(vi), ai, vj, aj);
688
689 test = test && std::fabs(det(testRotation) - 1) <= SignificantReal;
690
691 test = test && dot(testRotation(ai), vi) > 0;
692 test = test && dot(testRotation(aj), vj) > 0;
693
694 return test;
695 }
696
697 //-------------------------------------------------------------------
698 // Test the tricked-out code used to rotation a symmetric dyadic
699 // matrix S_AA=R_AB*S_BB*R_BA.
testReexpressSymMat33()700 bool testReexpressSymMat33() {
701 bool test = true;
702
703 const Rotation R_AB(Test::randRotation());
704 const SymMat33 S_BB(Test::randSymMat<3>());
705 const Mat33 M_BB(S_BB);
706
707 const SymMat33 S_AA = R_AB.reexpressSymMat33(S_BB);
708 const Mat33 M_AA = R_AB*M_BB*~R_AB;
709
710 const Mat33 MS_AA(S_AA);
711 test = test && (MS_AA-M_AA).norm() <= SignificantReal;
712
713 // Now put it back with an InverseRotation.
714 const SymMat33 isS_BB = (~R_AB).reexpressSymMat33(S_AA);
715 test = test && (S_BB-isS_BB).norm() <= SignificantReal;
716
717 const Rotation I; // identity
718 const SymMat33 S_BB_still = I.reexpressSymMat33(S_BB);
719 test = test && (S_BB_still-S_BB).norm() <= SignificantReal;
720
721 // Test symmetric matrix multiply (doesn't belong here).
722 const SymMat33 S1(Test::randSymMat<3>()), S2(Test::randSymMat<3>());
723 const Mat33 M1(S1), M2(S2);
724 const Mat33 S(S1*S2);
725 const Mat33 M(M1*M2);
726 test = test && (S-M).norm() <= SignificantReal;
727
728 const SymMat<3,Complex> SC1(Test::randComplex(),
729 Test::randComplex(), Test::randComplex(),
730 Test::randComplex(), Test::randComplex(), Test::randComplex() );
731 const SymMat<3,Complex> SC2(Test::randComplex(),
732 Test::randComplex(), Test::randComplex(),
733 Test::randComplex(), Test::randComplex(), Test::randComplex() );
734
735 SimTK_TEST_EQ(SC1.elt(1,0), conj(SC1.elt(0,1)));
736 SimTK_TEST_EQ(SC1.elt(2,0), conj(SC1.elt(0,2)));
737 SimTK_TEST_EQ(SC1.elt(1,2), conj(SC1.elt(2,1)));
738
739 const Mat<3,3,Complex> MC1(SC1), MC2(SC2);
740 const Mat<3,3,Complex> SC(SC1*SC2);
741 const Mat<3,3,Complex> MC(MC1*MC2);
742 SimTK_TEST_EQ(SC, MC);
743
744 return test;
745 }
746