14 // Check if the equivalent function works
15 v1 = SGVec3<T>(1, 2, 3);
16 v2 = SGVec3<T>(3, 2, 1);
17 if (equivalent(v1, v2))
20 // Check the unary minus operator
21 v3 = SGVec3<T>(-1, -2, -3);
22 if (!equivalent(-v1, v3))
25 // Check the unary plus operator
26 v3 = SGVec3<T>(1, 2, 3);
27 if (!equivalent(+v1, v3))
30 // Check the addition operator
31 v3 = SGVec3<T>(4, 4, 4);
32 if (!equivalent(v1 + v2, v3))
35 // Check the subtraction operator
36 v3 = SGVec3<T>(-2, 0, 2);
37 if (!equivalent(v1 - v2, v3))
40 // Check the scaler multiplication operator
41 v3 = SGVec3<T>(2, 4, 6);
42 if (!equivalent(2*v1, v3))
45 // Check the dot product
46 if (fabs(dot(v1, v2) - 10) > 10*SGLimits<T>::epsilon())
49 // Check the cross product
50 v3 = SGVec3<T>(-4, 8, -4);
51 if (!equivalent(cross(v1, v2), v3))
54 // Check the euclidean length
55 if (fabs(14 - length(v1)*length(v1)) > 14*SGLimits<T>::epsilon())
65 const SGVec3<T> e1(1, 0, 0);
66 const SGVec3<T> e2(0, 1, 0);
67 const SGVec3<T> e3(0, 0, 1);
69 SGQuat<T> q1, q2, q3, q4;
70 // Check a rotation around the x axis
71 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e1);
72 v1 = SGVec3<T>(1, 2, 3);
73 v2 = SGVec3<T>(1, -2, -3);
74 if (!equivalent(q1.transform(v1), v2))
77 // Check a rotation around the x axis
78 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
79 v2 = SGVec3<T>(1, 3, -2);
80 if (!equivalent(q1.transform(v1), v2))
83 // Check a rotation around the y axis
84 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e2);
85 v2 = SGVec3<T>(-1, 2, -3);
86 if (!equivalent(q1.transform(v1), v2))
89 // Check a rotation around the y axis
90 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
91 v2 = SGVec3<T>(-3, 2, 1);
92 if (!equivalent(q1.transform(v1), v2))
95 // Check a rotation around the z axis
96 q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e3);
97 v2 = SGVec3<T>(-1, -2, 3);
98 if (!equivalent(q1.transform(v1), v2))
101 // Check a rotation around the z axis
102 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e3);
103 v2 = SGVec3<T>(2, -1, 3);
104 if (!equivalent(q1.transform(v1), v2))
107 // Now check some successive transforms
108 // We can reuse the prevously tested stuff
109 q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
110 q2 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
112 v2 = q2.transform(q1.transform(v1));
113 if (!equivalent(q3.transform(v1), v2))
116 /// Test from Euler angles
117 float x = 0.2*SGMisc<T>::pi();
118 float y = 0.3*SGMisc<T>::pi();
119 float z = 0.4*SGMisc<T>::pi();
120 q1 = SGQuat<T>::fromAngleAxis(z, e3);
121 q2 = SGQuat<T>::fromAngleAxis(y, e2);
122 q3 = SGQuat<T>::fromAngleAxis(x, e1);
123 v2 = q3.transform(q2.transform(q1.transform(v1)));
124 q4 = SGQuat<T>::fromEuler(z, y, x);
125 if (!equivalent(q4.transform(v1), v2))
128 /// Test angle axis forward and back transform
129 q1 = SGQuat<T>::fromAngleAxis(0.2*SGMisc<T>::pi(), e1);
130 q2 = SGQuat<T>::fromAngleAxis(0.7*SGMisc<T>::pi(), e2);
133 q1.getAngleAxis(angleAxis);
134 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
135 if (!equivalent(q1, q4))
137 q2.getAngleAxis(angleAxis);
138 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
139 if (!equivalent(q2, q4))
141 q3.getAngleAxis(angleAxis);
142 q4 = SGQuat<T>::fromAngleAxis(angleAxis);
143 if (!equivalent(q3, q4))
153 // Create some test matrix
154 SGVec3<T> v0(2, 7, 17);
155 SGQuat<T> q0 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), normalize(v0));
156 SGMatrix<T> m0(q0, v0);
158 // Check the tqo forms of the inverse for that kind of special matrix
162 if (!equivalent(m1, m2))
165 // Check matrix multiplication and inversion
166 if (!equivalent(m0*m1, SGMatrix<T>::unit()))
168 if (!equivalent(m1*m0, SGMatrix<T>::unit()))
170 if (!equivalent(m0*m2, SGMatrix<T>::unit()))
172 if (!equivalent(m2*m0, SGMatrix<T>::unit()))
181 // We know that the values are on the order of 1
182 double epsDeg = 10*SGLimits<double>::epsilon();
183 // For the altitude values we need to tolerate relative errors in the order
185 double epsM = 1e6*SGLimits<double>::epsilon();
187 SGVec3<double> cart0, cart1;
191 // create some geodetic position
192 geod0 = SGGeod::fromDegM(30, 20, 17);
194 // Test the conversion routines to cartesian coordinates
197 if (epsDeg < fabs(geod0.getLongitudeDeg() - geod1.getLongitudeDeg()) ||
198 epsDeg < fabs(geod0.getLatitudeDeg() - geod1.getLatitudeDeg()) ||
199 epsM < fabs(geod0.getElevationM() - geod1.getElevationM()))
202 // Test the conversion routines to radial coordinates
205 if (!equivalent(cart0, cart1))
213 sgInterfaceTest(void)
215 SGVec3f v3f = SGVec3f::e2();
216 SGVec4f v4f = SGVec4f::e2();
217 SGQuatf qf = SGQuatf::fromEuler(1.2, 1.3, -0.4);
218 SGMatrixf mf(qf, v3f);
220 // Copy to and from plibs types check if result is equal,
221 // test for exact equality
224 sgCopyVec3(sv3f, v3f.sg());
225 sgCopyVec3(tv3f.sg(), sv3f);
229 // Copy to and from plibs types check if result is equal,
230 // test for exact equality
233 sgCopyVec4(sv4f, v4f.sg());
234 sgCopyVec4(tv4f.sg(), sv4f);
238 // Copy to and from plibs types check if result is equal,
239 // test for exact equality
242 sgCopyQuat(sqf, qf.sg());
243 sgCopyQuat(tqf.sg(), sqf);
247 // Copy to and from plibs types check if result is equal,
248 // test for exact equality
251 sgCopyMat4(smf, mf.sg());
252 sgCopyMat4(tmf.sg(), smf);
260 sgdInterfaceTest(void)
262 SGVec3d v3d = SGVec3d::e2();
263 SGVec4d v4d = SGVec4d::e2();
264 SGQuatd qd = SGQuatd::fromEuler(1.2, 1.3, -0.4);
265 SGMatrixd md(qd, v3d);
267 // Copy to and from plibs types check if result is equal,
268 // test for exact equality
271 sgdCopyVec3(sv3d, v3d.sg());
272 sgdCopyVec3(tv3d.sg(), sv3d);
276 // Copy to and from plibs types check if result is equal,
277 // test for exact equality
280 sgdCopyVec4(sv4d, v4d.sg());
281 sgdCopyVec4(tv4d.sg(), sv4d);
285 // Copy to and from plibs types check if result is equal,
286 // test for exact equality
289 sgdCopyQuat(sqd, qd.sg());
290 sgdCopyQuat(tqd.sg(), sqd);
294 // Copy to and from plibs types check if result is equal,
295 // test for exact equality
298 sgdCopyMat4(smd, md.sg());
299 sgdCopyMat4(tmd.sg(), smd);
310 if (!Vec3Test<float>())
312 if (!Vec3Test<double>())
315 // Do quaternion tests
316 if (!QuatTest<float>())
318 if (!QuatTest<double>())
322 if (!MatrixTest<float>())
324 if (!MatrixTest<double>())
327 // Check geodetic/geocentric/cartesian conversions
328 // if (!GeodesyTest())
329 // return EXIT_FAILURE;
331 // Check interaction with sg*/sgd*
332 if (!sgInterfaceTest())
334 if (!sgdInterfaceTest())
337 std::cout << "Successfully passed all tests!" << std::endl;