25 Quatf q_check(0.98334744
f, 0.0342708
f, 0.10602051
f, .14357218
f);
27 0.93629336f, -0.27509585f, 0.21835066f,
28 0.28962948f, 0.95642509f, -0.03695701f,
29 -0.19866933f, 0.0978434f, 0.97517033f
31 Dcmf dcm_check(dcm_data);
38 Eulerf e_zero = zeros<float, 3, 1>();
51 TEST(fabs(q(0) - 1) < eps);
52 TEST(fabs(q(1) - 2) < eps);
53 TEST(fabs(q(2) - 3) < eps);
54 TEST(fabs(q(3) - 4) < eps);
62 quat_v =
Quatf(v1, v);
66 quat_v =
Quatf(v1, -v1);
70 quat_v =
Quatf(v1, -v1);
74 quat_v =
Quatf(v1, -v1);
78 quat_v =
Quatf(v1, -v1);
84 0.54772256
f, 0.73029674
f)));
93 q =
Quatf(euler_check);
97 Dcmf dcm(euler_check);
109 Vector3f R_z(dcm_check(0, 2), dcm_check(1, 2), dcm_check(2, 2));
126 Dcmf A = eye<float, 3>();
128 for (
size_t i = 0; i < 1000; i++) {
135 for (
size_t r = 0; r < 3; r++) {
137 err += fabs(1.0
f - rvec.length());
142 double deg2rad =
M_PI / 180.0;
143 double rad2deg = 180.0 /
M_PI;
146 for (
double roll = -90; roll <= 90; roll += 90) {
147 for (
double pitch = -90; pitch <= 90; pitch += 90) {
148 for (
double yaw = -179; yaw <= 180; yaw += 90) {
150 double roll_expected = roll;
151 double yaw_expected = yaw;
153 if (fabs(pitch -90) < eps) {
155 yaw_expected = yaw - roll;
157 }
else if (fabs(pitch + 90) < eps) {
159 yaw_expected = yaw + roll;
162 if (yaw_expected < -180) {
166 if (yaw_expected > 180) {
172 deg2rad * roll_expected,
174 deg2rad * yaw_expected);
182 TEST(
isEqual(rad2deg * euler_expected, rad2deg * euler_out));
185 float(deg2rad)*
float(roll_expected),
186 float(deg2rad)*
float(pitch),
187 float(deg2rad)*
float(yaw_expected));
188 Eulerf eulerf(
float(deg2rad)*
float(roll),
189 float(deg2rad)*
float(pitch),
190 float(deg2rad)*
float(yaw));
192 dcm_from_eulerf = eulerf;
195 float(rad2deg)*euler_outf));
201 float data_v4[] = {1, 2, 3, 4};
211 Quatf q1(0, 1, 0, 0);
213 float data_q_dot1_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
219 float data_q_dot2_check[] = { -0.5f, 0.0f, 1.5f, -1.0f};
225 0.93394439
f, 0.0674002
f, 0.20851
f, 0.28236266
f);
232 Quatf q_scalar_mul(1.0
f, 2.0
f, 3.0
f, 4.0
f);
233 Quatf q_scalar_mul_check(1.0
f * scalar, 2.0
f * scalar,
234 3.0
f * scalar, 4.0
f * scalar);
235 Quatf q_scalar_mul_res = scalar * q_scalar_mul;
236 TEST(
isEqual(q_scalar_mul_check, q_scalar_mul_res));
237 Quatf q_scalar_mul_res2 = q_scalar_mul * scalar;
238 TEST(
isEqual(q_scalar_mul_check, q_scalar_mul_res2));
239 Quatf q_scalar_mul_res3(q_scalar_mul);
240 q_scalar_mul_res3 *= scalar;
241 TEST(
isEqual(q_scalar_mul_check, q_scalar_mul_res3));
245 TEST(fabs(q_check(0) - q(0)) < eps);
246 TEST(fabs(q_check(1) + q(1)) < eps);
247 TEST(fabs(q_check(2) + q(2)) < eps);
248 TEST(fabs(q_check(3) + q(3)) < eps);
252 TEST(fabs(q_check(0) - q(0)) < eps);
253 TEST(fabs(q_check(1) + q(1)) < eps);
254 TEST(fabs(q_check(2) + q(2)) < eps);
255 TEST(fabs(q_check(3) + q(3)) < eps);
258 Quatf q_non_canonical(-0.7
f,0.4
f, 0.3
f, -0.3
f);
259 Quatf q_canonical(0.7
f,-0.4
f, -0.3
f, 0.3
f);
260 Quatf q_canonical_ref(0.7
f,-0.4
f, -0.3
f, 0.3
f);
269 Quatf q_nonIdentity(-0.7
f, 0.4
f, 0.5
f, -0.3
f);
270 Quatf q_identity(1.0
f, 0.0
f, 0.0
f, 0.0
f);
282 rot(1) = rot(2) = 0.0f;
290 rot(1) = rot(2) = 0.0f;
299 q_true =
Quatf(0.3019
f, 0.2645
f, 0.2268
f, 0.8874
f);
305 TEST(fabs(rot(0)) < eps);
306 TEST(fabs(rot(1) - 1.0
f) < eps);
307 TEST(fabs(rot(2)) < eps);
312 TEST(fabs(rot(0)) < eps);
313 TEST(fabs(rot(1)) < eps);
314 TEST(fabs(rot(2)) < eps);
317 rot(0) = rot(1) = rot(2) = 0.0f;
330 float q_array[] = {0.9833f, -0.0343f, -0.1060f, -0.1436f};
333 for (
size_t i = 0; i < 4; i++) {
334 TEST(fabs(q_from_array(i) - q_array[i]) < eps);
342 float aa_data[] = {4.0f, 5.0f, 6.0f};
350 q =
Quatf(-0.29555112749297824
f, 0.25532186
f, 0.51064372
f, 0.76596558
f);
384 Dcmf dcm34 = dcm3 * dcm4;
398 #if defined(SUPPORT_STDIOSTREAM) 399 std::cout <<
"q:" << q;
void canonicalize()
Bring quaternion to canonical form.
Matrix< Type, N, M > T() const
Dcm< Type > to_dcm()
XXX DEPRECATED, can use assignment or ctor.
Vector3< Type > dcm_z() const
Corresponding body z-axis to an attitude quaternion / last orthogonal unit basis vector.
Quaternion from_dcm(const Matrix< Type, 3, 3 > &dcm)
XXX DEPRECATED, can use assignment or ctor.
void invert()
Invert quaternion in place.
Quaternion canonical() const
Return canonical form of the quaternion.
Vector3< Type > conjugate(const Vector3< Type > &vec) const
Rotates vector v_1 in frame 1 to vector v_2 in frame 2 using the rotation quaternion q_21 describing ...
Vector normalized() const
Vector< Type, 3 > to_axis_angle() const
Rotation vector from quaternion XXX DEPRECATED, use AxisAngle class.
void rotate(const AxisAngle< Type > &vec)
Rotate quaternion from rotation vector.
Matrix41 derivative2(const Matrix31 &w) const
Computes the derivative of q_21 when rotated with angular velocity expressed in frame 2 v_2 = q_21 * ...
Dual< Scalar, N > cos(const Dual< Scalar, N > &a)
void from_axis_angle(Vector< Type, 3 > vec)
Rotation quaternion from vector.
Matrix41 derivative1(const Matrix31 &w) const
Computes the derivative of q_21 when rotated with angular velocity expressed in frame 1 v_2 = q_21 * ...
bool isEqual(const Matrix< Type, M, N > &x, const Matrix< Type, M, N > &y, const Type eps=1e-4f)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Vector3< Type > imag() const
Imaginary components of quaternion.
Vector3< float > Vector3f
Quaternion inversed() const
Invert quaternion.
Quaternion< float > Quatf
const Slice< Type, 1, N, M, N > row(size_t i) const
AxisAngle< float > AxisAnglef
bool isEqualF(const Type x, const Type y, const Type eps=1e-4f)
Compare if two floating point numbers are equal.
Vector3< Type > conjugate_inversed(const Vector3< Type > &vec) const
Rotates vector v_2 in frame 2 to vector v_1 in frame 1 using the rotation quaternion q_21 describing ...
Dual< Scalar, N > sin(const Dual< Scalar, N > &a)
Dual< Scalar, N > sqrt(const Dual< Scalar, N > &a)