33 #include "../Quaternion.hh"
34 #include <Base/Common/CompareFloatingPoint.hh>
44 cout <<
"q = " << q <<
", p = " << p << endl;
53 cout <<
"q*p = " << qp << endl;
54 cout <<
"L(q)*p = " << Lp << endl;
55 cout <<
"R(p)*q = " << Rq << endl;
56 BIASASSERT(
BIAS::Equal((Lp-qp).NormL2(), 0.0, 1e-12));
57 BIASASSERT(
BIAS::Equal((Rq-qp).NormL2(), 0.0, 1e-12));
65 #ifdef BIASASSERT_ISACTIVE
67 BIASASSERT(res_GetQuat == 0);
75 cout <<
"v = " << v <<
", q*v*q' = " << qvq << endl;
77 BIASASSERT(
BIAS::Equal((qvq-Rv).NormL2(), 0.0, 1e-12));
int SetFromQuaternion(const Quaternion< ROTATION_MATRIX_TYPE > &q)
Set rotation matrix from a quaternion.
Matrix4x4< QUAT_TYPE > GetQuaternionMultMatrixLeft() const
Returns matrix which expresses (left) quaternion multiplication.
int GetQuaternion(Quaternion< ROTATION_MATRIX_TYPE > &quat) const
Calculates quaternion representation for this rotation matrix.
void ScalarProduct(const Vector4< T > &argvec, T &result) const
scalar product (=inner product) of two vectors, storing the result in result
bool Equal(const T left, const T right, const T eps)
comparison function for floating point values See http://www.boost.org/libs/test/doc/components/test_...
Implements a 3D rotation matrix.
void SetValueAsAxisRad(const Vector3< QUAT_TYPE > &axis, QUAT_TYPE angle)
sets the quaternion with given rotation axis and angle (in rad)