3 # include <simgear_config.h>
19 // Check if the equivalent function works
20 v1 = SGVec3<T>(1, 2, 3);
21 v2 = SGVec3<T>(3, 2, 1);
22 if (equivalent(v1, v2))
25 // Check the unary minus operator
26 v3 = SGVec3<T>(-1, -2, -3);
27 if (!equivalent(-v1, v3))
30 // Check the unary plus operator
31 v3 = SGVec3<T>(1, 2, 3);
32 if (!equivalent(+v1, v3))
35 // Check the addition operator
36 v3 = SGVec3<T>(4, 4, 4);
37 if (!equivalent(v1 + v2, v3))
40 // Check the subtraction operator
41 v3 = SGVec3<T>(-2, 0, 2);
42 if (!equivalent(v1 - v2, v3))
45 // Check the scaler multiplication operator
46 v3 = SGVec3<T>(2, 4, 6);
47 if (!equivalent(2*v1, v3))
50 // Check the dot product
51 if (fabs(dot(v1, v2) - 10) > 10*SGLimits<T>::epsilon())
54 // Check the cross product
55 v3 = SGVec3<T>(-4, 8, -4);
56 if (!equivalent(cross(v1, v2), v3))
59 // Check the euclidean length
60 if (fabs(14 - length(v1)*length(v1)) > 14*SGLimits<T>::epsilon())
70 const SGVec3<T> e1(1, 0, 0);
71 const SGVec3<T> e2(0, 1, 0);
72 const SGVec3<T> e3(0, 0, 1);
74 SGQuat<T> q1, q2, q3, q4;
75 // Check a rotation around the x axis
76 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e1);
77 v1 = SGVec3<T>(1, 2, 3);
78 v2 = SGVec3<T>(1, -2, -3);
79 if (!equivalent(q1.transform(v1), v2))
82 // Check a rotation around the x axis
83 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
84 v2 = SGVec3<T>(1, 3, -2);
85 if (!equivalent(q1.transform(v1), v2))
88 // Check a rotation around the y axis
89 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e2);
90 v2 = SGVec3<T>(-1, 2, -3);
91 if (!equivalent(q1.transform(v1), v2))
94 // Check a rotation around the y axis
95 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
96 v2 = SGVec3<T>(-3, 2, 1);
97 if (!equivalent(q1.transform(v1), v2))
100 // Check a rotation around the z axis
101 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e3);
102 v2 = SGVec3<T>(-1, -2, 3);
103 if (!equivalent(q1.transform(v1), v2))
106 // Check a rotation around the z axis
107 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e3);
108 v2 = SGVec3<T>(2, -1, 3);
109 if (!equivalent(q1.transform(v1), v2))
112 // Now check some successive transforms
113 // We can reuse the prevously tested stuff
114 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
115 q2 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
117 v2 = q2.transform(q1.transform(v1));
118 if (!equivalent(q3.transform(v1), v2))
121 /// Test from Euler angles
122 float x = 0.2*SGMisc<T>::pi();
123 float y = 0.3*SGMisc<T>::pi();
124 float z = 0.4*SGMisc<T>::pi();
125 q1 = SGQuat<T>::fromAngleAxis(z, e3);
126 q2 = SGQuat<T>::fromAngleAxis(y, e2);
127 q3 = SGQuat<T>::fromAngleAxis(x, e1);
128 v2 = q3.transform(q2.transform(q1.transform(v1)));
129 q4 = SGQuat<T>::fromEuler(z, y, x);
130 if (!equivalent(q4.transform(v1), v2))
133 /// Test angle axis forward and back transform
134 q1 = SGQuat<T>::fromAngleAxis(0.2*SGMisc<T>::pi(), e1);
135 q2 = SGQuat<T>::fromAngleAxis(0.7*SGMisc<T>::pi(), e2);
138 q1.getAngleAxis(angleAxis);
139 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
140 if (!equivalent(q1, q4))
142 q2.getAngleAxis(angleAxis);
143 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
144 if (!equivalent(q2, q4))
146 q3.getAngleAxis(angleAxis);
147 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
148 if (!equivalent(q3, q4))
158 // Create some test matrix
159 SGVec3<T> v0(2, 7, 17);
160 SGQuat<T> q0 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), normalize(v0));
161 SGMatrix<T> m0(q0, v0);
163 // Check the tqo forms of the inverse for that kind of special matrix
167 if (!equivalent(m1, m2))
170 // Check matrix multiplication and inversion
171 if (!equivalent(m0*m1, SGMatrix<T>::unit()))
173 if (!equivalent(m1*m0, SGMatrix<T>::unit()))
175 if (!equivalent(m0*m2, SGMatrix<T>::unit()))
177 if (!equivalent(m2*m0, SGMatrix<T>::unit()))
186 // We know that the values are on the order of 1
187 double epsDeg = 10*SGLimits<double>::epsilon();
188 // For the altitude values we need to tolerate relative errors in the order
190 double epsM = 1e6*SGLimits<double>::epsilon();
192 SGVec3<double> cart0, cart1;
196 // create some geodetic position
197 geod0 = SGGeod::fromDegM(30, 20, 17);
199 // Test the conversion routines to cartesian coordinates
202 if (epsDeg < fabs(geod0.getLongitudeDeg() - geod1.getLongitudeDeg()) ||
203 epsDeg < fabs(geod0.getLatitudeDeg() - geod1.getLatitudeDeg()) ||
204 epsM < fabs(geod0.getElevationM() - geod1.getElevationM()))
207 // Test the conversion routines to radial coordinates
210 if (!equivalent(cart0, cart1))
218 sgInterfaceTest(void)
220 SGVec3f v3f = SGVec3f::e2();
221 SGVec4f v4f = SGVec4f::e2();
222 SGQuatf qf = SGQuatf::fromEuler(1.2, 1.3, -0.4);
223 SGMatrixf mf(qf, v3f);
225 // Copy to and from plibs types check if result is equal,
226 // test for exact equality
229 sgCopyVec3(sv3f, v3f.sg());
230 sgCopyVec3(tv3f.sg(), sv3f);
234 // Copy to and from plibs types check if result is equal,
235 // test for exact equality
238 sgCopyVec4(sv4f, v4f.sg());
239 sgCopyVec4(tv4f.sg(), sv4f);
243 // Copy to and from plibs types check if result is equal,
244 // test for exact equality
247 sgCopyQuat(sqf, qf.sg());
248 sgCopyQuat(tqf.sg(), sqf);
252 // Copy to and from plibs types check if result is equal,
253 // test for exact equality
256 sgCopyMat4(smf, mf.sg());
257 sgCopyMat4(tmf.sg(), smf);
265 sgdInterfaceTest(void)
267 SGVec3d v3d = SGVec3d::e2();
268 SGVec4d v4d = SGVec4d::e2();
269 SGQuatd qd = SGQuatd::fromEuler(1.2, 1.3, -0.4);
270 SGMatrixd md(qd, v3d);
272 // Copy to and from plibs types check if result is equal,
273 // test for exact equality
276 sgdCopyVec3(sv3d, v3d.sg());
277 sgdCopyVec3(tv3d.sg(), sv3d);
281 // Copy to and from plibs types check if result is equal,
282 // test for exact equality
285 sgdCopyVec4(sv4d, v4d.sg());
286 sgdCopyVec4(tv4d.sg(), sv4d);
290 // Copy to and from plibs types check if result is equal,
291 // test for exact equality
294 sgdCopyQuat(sqd, qd.sg());
295 sgdCopyQuat(tqd.sg(), sqd);
299 // Copy to and from plibs types check if result is equal,
300 // test for exact equality
303 sgdCopyMat4(smd, md.sg());
304 sgdCopyMat4(tmd.sg(), smd);
315 if (!Vec3Test<float>())
317 if (!Vec3Test<double>())
320 // Do quaternion tests
321 if (!QuatTest<float>())
323 if (!QuatTest<double>())
327 if (!MatrixTest<float>())
329 if (!MatrixTest<double>())
332 // Check geodetic/geocentric/cartesian conversions
333 // if (!GeodesyTest())
334 // return EXIT_FAILURE;
336 // Check interaction with sg*/sgd*
337 if (!sgInterfaceTest())
339 if (!sgdInterfaceTest())
342 std::cout << "Successfully passed all tests!" << std::endl;