/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ /* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ /* $Id: vctQuaternionRotation3Base.h,v 1.14 2007/04/26 19:33:58 anton Exp $ Author(s): Anton Deguet Created on: 2005-08-24 (C) Copyright 2005-2007 Johns Hopkins University (JHU), All Rights Reserved. --- begin cisst license - do not edit --- This software is provided "as is" under an open source license, with no warranty. The complete license can be found in license.txt and http://www.cisst.org/cisst/license.txt. --- end cisst license --- */ /*! \file \brief Declaration of vctQuaternionRotation3Base */ #ifndef _vctQuaternionRotation3Base_h #define _vctQuaternionRotation3Base_h #include #include #include #include #include #include #ifndef DOXYGEN #ifndef SWIG // helper functions for subtemplated methods of a templated class template void vctQuaternionRotation3BaseFromRaw(vctQuaternionRotation3Base<_quaternionType> & quaternionRotation, const vctMatrixRotation3Base<_matrixType> & matrixRotation); #ifdef CISST_COMPILER_IS_MSVC template CISST_EXPORT void vctQuaternionRotation3BaseFromRaw(vctQuaternionRotation3Base > & quaternionRotation, const vctMatrixRotation3Base > & matrixRotation); template CISST_EXPORT void vctQuaternionRotation3BaseFromRaw(vctQuaternionRotation3Base > & quaternionRotation, const vctMatrixRotation3Base > & matrixRotation); template CISST_EXPORT void vctQuaternionRotation3BaseFromRaw(vctQuaternionRotation3Base > & quaternionRotation, const vctMatrixRotation3Base > & matrixRotation); template CISST_EXPORT void vctQuaternionRotation3BaseFromRaw(vctQuaternionRotation3Base > & quaternionRotation, const vctMatrixRotation3Base > & matrixRotation); template CISST_EXPORT void vctQuaternionRotation3BaseFromRaw(vctQuaternionRotation3Base > & quaternionRotation, const vctMatrixRotation3Base > & matrixRotation); template CISST_EXPORT void vctQuaternionRotation3BaseFromRaw(vctQuaternionRotation3Base > & quaternionRotation, const vctMatrixRotation3Base > & matrixRotation); #endif #endif // SWIG #endif // DOXYGEN /*! \brief Define a rotation quaternion for a space of dimension 3 This class is templated by the element type. It is derived from vctQuaternionBase. This class is templated by the type of container used to store the quaternion. It is an "inner" class of the library, i.e. a regular user should use either the predefined type ::vctQuatRot3 or the class vctQuaternionRotation3. \param _containerType The type of container used for the quaternion. \sa vctQuaternionRotation3, vctQuaternionBase */ template class vctQuaternionRotation3Base: public vctQuaternionBase<_containerType> { public: enum {SIZE = 4}; enum {DIMENSION = 3}; /*! Type of base class. */ typedef vctQuaternionBase<_containerType> BaseType; /*! Type of container. This must be a vector of 4 elements (either fixed size or dynamic). In any case, this class should only be used by the library programmers. */ typedef _containerType ContainerType; typedef vctQuaternionRotation3Base<_containerType> ThisType; /* no need to document, inherit doxygen documentation from vctFixedSizeVectorBase */ VCT_CONTAINER_TRAITS_TYPEDEFS(typename ContainerType::value_type); typedef cmnTypeTraits TypeTraits; protected: /*! Throw an exception unless this rotation is normalized. */ inline void ThrowUnlessIsNormalized(void) const throw(std::runtime_error) { if (! this->IsNormalized()) { cmnThrow(std::runtime_error("vctQuaternionRotation3Base: This rotation is not normalized")); } } /*! Throw an exception unless the input is normalized. \param input An object with \c IsNormalized method. */ template inline void ThrowUnlessIsNormalized(const _inputType & input) const throw(std::runtime_error) { if (! input.IsNormalized()) { cmnThrow(std::runtime_error("vctQuaternionRotation3Base: Input is not normalized")); } } /*! Allocate memory for the underlying container if needed. By default, this methods does nothing. For any container requiring a memory allocation, it is necessary to specialize this method. */ inline void Allocate(void) {} public: /*! Default constructor. Sets the rotation quaternion to identity. */ inline vctQuaternionRotation3Base(void) { this->Allocate(); this->Assign(ThisType::Identity()); } /*! Copy constructor. \param quaternionRotation A rotation quaternion. */ inline vctQuaternionRotation3Base(const ThisType & quaternionRotation) { this->Allocate(); Assign(quaternionRotation); } /*! \name Constructors with normalization test. These constructors will check that the input is valid, i.e. normalized. If the input is not normalized, an exception (of type \c std::runtime_error) will be thrown. Each constructor uses the corresponding From() method based on the input type. \note See the cmnThrow() function if an \c abort is better than an exception for your application. */ //@{ /*! Constructor from 4 elements. \param x First component of the real part. \param y Second component of the real part. \param z Third component of the real part. \param r The imaginary part. */ inline vctQuaternionRotation3Base(const value_type & x, const value_type & y, const value_type & z, const value_type & r) throw(std::runtime_error) { this->Allocate(); this->From(x, y, z, r); } /*! Constructor from a rotation matrix. \param matrixRotation A rotation matrix. */ template inline explicit vctQuaternionRotation3Base(const vctMatrixRotation3Base<__containerType> & matrixRotation) throw(std::runtime_error) { this->Allocate(); this->From(matrixRotation); } /*! Constructor from an axis and angle. \param axisAngleRotation An axis/angle rotation. */ inline vctQuaternionRotation3Base(const vctAxisAngleRotation3 & axisAngleRotation) throw(std::runtime_error) { this->Allocate(); this->From(axisAngleRotation); } /*! Constructor from a Rodriguez rotation. \param rodriguezRotation A Rodriguez rotation. */ template inline vctQuaternionRotation3Base(const vctRodriguezRotation3Base<__containerType> & rodriguezRotation) throw(std::runtime_error) { this->Allocate(); this->From(rodriguezRotation); } //@} /*! \name Constructors without normalization test These constructors will either assume that the input is normalized or normalize the input (a copy of it, if required) based on the last parameter provided.
  • If the normalization flag is set to #VCT_DO_NOT_NORMALIZE, the input is considered already normalized and the constructor will not perform any sanity check. This can lead to numerical instabilities which have to be handled by the caller.
  • If the normalization flag is set to #VCT_NORMALIZE, the input will be normalized. This option should be used whenever it is important to obtain a result as "normalized" as possible.
*/ //@{ /*! Copy constructor. \param quaternionRotation A rotation quaternion. \param normalizeInput Force the input normalization. */ inline vctQuaternionRotation3Base(const ThisType & quaternionRotation, bool normalizeInput) { this->Allocate(); if (normalizeInput) { this->FromNormalized(quaternionRotation); } else { this->FromRaw(quaternionRotation); } } /*! Constructor from 4 elements. \param x First component of the real part. \param y Second component of the real part. \param z Third component of the real part. \param r The imaginary part. \param normalizeInput Force the input normalization. */ inline vctQuaternionRotation3Base(const value_type & x, const value_type & y, const value_type & z, const value_type & r, bool normalizeInput) { this->Allocate(); if (normalizeInput) { this->FromNormalized(x, y, z, r); } else { this->FromRaw(x, y, z, r); } } /*! Constructor from a rotation matrix. \param matrixRotation A rotation matrix. \param normalizeInput Force the input normalization. */ template inline explicit vctQuaternionRotation3Base(const vctMatrixRotation3Base<__containerType> & matrixRotation, bool normalizeInput) { this->Allocate(); if (normalizeInput) { this->FromNormalized(matrixRotation); } else { this->FromRaw(matrixRotation); } } /*! Constructor from an axis and angle. \param axisAngleRotation An axis/angle rotation. \param normalizeInput Force the input normalization. */ inline vctQuaternionRotation3Base(const vctAxisAngleRotation3 & axisAngleRotation, bool normalizeInput) { this->Allocate(); if (normalizeInput) { this->FromNormalized(axisAngleRotation); } else { this->FromRaw(axisAngleRotation); } } /*! Constructor from a Rodriguez rotation. \param rodriguezRotation A Rodriguez rotation. \param normalizeInput Force the input normalization. */ template inline vctQuaternionRotation3Base(const vctRodriguezRotation3Base<__containerType> & rodriguezRotation, bool normalizeInput) { this->Allocate(); if (normalizeInput) { this->FromNormalized(rodriguezRotation); } else { this->FromRaw(rodriguezRotation); } } //@} /*! Const reference to the identity. In this case, (0, 0, 0, 1)*/ static CISST_EXPORT const ThisType & Identity(); /*! \name Conversion from normalized input. These methods will check that the input is normalized. If the input is not normalized, an exception (\c std::runtime_error) will be thrown using cmnThrow(). \note Since all exceptions are thrown using cmnThrow(), it is possible to configure these methods to use \c abort() if the normalization requirements are not met (see cmnThrow()). */ //@{ /*! Conversion from 4 numbers. This method actually performs an assignement and then check that the result is normalized. */ inline ThisType & From(value_type x, value_type y, value_type z, value_type r) throw(std::runtime_error) { this->Assign(x, y, z, r); this->ThrowUnlessIsNormalized(*this); return *this; } /*! Conversion from axis/angle. */ inline ThisType & From(const vctAxisAngleRotation3 axisAngleRotation) throw(std::runtime_error) { this->ThrowUnlessIsNormalized(axisAngleRotation); return FromRaw(axisAngleRotation); } /*! Conversion from a Rodriguez rotation. */ template inline ThisType & From(const vctRodriguezRotation3Base<__containerType> & rodriguezRotation) throw(std::runtime_error) { this->ThrowUnlessIsNormalized(rodriguezRotation); return this->FromRaw(rodriguezRotation); } /*! Conversion from a rotation matrix. */ template inline ThisType & From(const vctMatrixRotation3Base<__containerType> & matrixRotation) throw(std::runtime_error) { this->ThrowUnlessIsNormalized(matrixRotation); return FromRaw(matrixRotation); } //@} /*! \name Conversion and normalization. These method will accept any input and attempt to either normalize the input and then convert or convert and then normalize the quaternion itself.
The order depends on the type of input. */ //@{ /*! Conversion from 4 numbers. This method actually performs an assignement and then normalize the quaternion (\c this). */ inline ThisType & FromNormalized(value_type x, value_type y, value_type z, value_type r) { this->Assign(x, y, z, r); this->NormalizedSelf(); return *this; } /*! Conversion from axis/angle. */ inline ThisType & FromNormalized(const vctAxisAngleRotation3 axisAngleRotation) { return FromRaw(axisAngleRotation.Normalized()); } /*! Conversion from a Rodriguez rotation. */ template inline ThisType & FromNormalized(const vctRodriguezRotation3Base<__containerType> & rodriguezRotation) { return this->FromRaw(rodriguezRotation.Normalized()); } /*! Conversion from a rotation matrix. This method could normalize the input first, but the rotation matrix normalization relies on -1- a conversion to quaternion, -2- normalization and -3- a conversion back to rotation matrix. Therefore it converts to quaternion and then normalizes the resulting quaternion (\c this). */ template inline ThisType & FromNormalized(const vctMatrixRotation3Base<__containerType> & matrixRotation) { this->FromRaw(matrixRotation); this->NormalizedSelf(); return *this; } //@} /*! \name Conversion. These method don't check if the input is normalized nor try to normalize the results. They should be used with caution since the resulting rotation (in this case a quaternion) might not be normalized. */ //@{ inline ThisType & FromRaw(value_type x, value_type y, value_type z, value_type r) { this->Assign(x, y, z, r); return *this; } inline ThisType & FromRaw(const vctAxisAngleRotation3 axisAngleRotation) { typedef vctAxisAngleRotation3 AxisAngleType; typedef typename AxisAngleType::AxisType AxisType; const AngleType angle = axisAngleRotation.Angle(); const AxisType axis = axisAngleRotation.Axis(); const AngleType halfAngle = angle * 0.5; const value_type s = (value_type) sin(halfAngle); const value_type c = (value_type) cos(halfAngle); this->Assign(s * axis[0], s * axis[1], s * axis[2], c); return *this; } /*! Conversion from a Rodriguez rotation. */ template inline ThisType & FromRaw(const vctRodriguezRotation3Base<__containerType> & rodriguezRotation) { return this->FromRaw(vctAxisAngleRotation3(rodriguezRotation, VCT_DO_NOT_NORMALIZE)); } /*! Conversion from a rotation matrix. The algorithm is based on http://www.j3d.org/matrix_faq/matrfaq_latest.html. This method is important since we use it to convert a rotation matrix to a quaternion to normalize the matrix. The method From can not be used since it asserts that the input matrix is already normalized. \param matrixRotation A rotation matrix */ template inline ThisType & FromRaw(const vctMatrixRotation3Base<__containerType> & matrixRotation) { vctQuaternionRotation3BaseFromRaw(*this, matrixRotation); return *this; } //@} /*! A complementary form of assigning one quaternion rotation to another. The method is provided mostly for generic programming interfaces, and for testing various operations on rotations */ inline ThisType & From(const ThisType & otherRotation) { return reinterpret_cast(this->Assign(otherRotation)); } /*! Sets this rotation quaternion as the normalized version of another one. \param otherQuaternion rotation quaternion used to compute the normalized quaternion. */ inline ThisType & NormalizedOf(const ThisType & otherQuaternion) { CMN_ASSERT(otherQuaternion.Pointer() != this->Pointer()); this->Assign(otherQuaternion); this->NormalizedSelf(); return *this; } /*! Normalizes this quaternion rotation. */ inline ThisType & NormalizedSelf(void) { BaseType::NormalizedSelf(); return *this; } /*! Returns the normalized version of this quaternion rotation. This method returns a copy of the normalized version and does not modify this quaternion. */ inline ThisType Normalized(void) const { ThisType result(*this); result.NormalizedSelf(); return result; } /*! Inverse this rotation quaternion. This methods assumes that the quaternion is normalized and sets this unit quaternion as its transposed. */ inline ThisType & InverseSelf(void) { this->ConjugateSelf(); return *this; } /*! Set this rotation as the inverse of another one. */ inline ThisType & InverseOf(const ThisType & otherRotation) { ConjugateOf(otherRotation); return *this; } /*! Create and return by copy the inverse of this matrix. This method is not the most efficient since it requires a copy. */ inline ThisType Inverse(void) const { ThisType result; result.InverseOf(*this); return result; } /*! Apply the rotation to a vector of fixed size 3. The result is stored into a vector of size 3 provided by the caller and passed by reference. \param input The input vector \param output The output vector */ template inline void ApplyTo(const vctFixedSizeConstVectorBase & input, vctFixedSizeVectorBase & output) const { CMN_ASSERT(input.Pointer() != output.Pointer()); // could build a vector with a 0 as real component, create the // conjugate and then perfom two quaternion products but his // seems more efficient. value_type qR, qX, qY, qZ; const value_type x = this->X(); const value_type y = this->Y(); const value_type z = this->Z(); const value_type r = this->R(); // compute the product of quaternions q_input = [in, 0] and q // conjugate. Since q_input[3] = 0, suppress terms with it. qR = - input.X() * x - input.Y() * y - input.Z() * z; qX = - input.X() * r + input.Y() * z - input.Z() * y; qY = - input.Y() * r + input.Z() * x - input.X() * z; qZ = - input.Z() * r + input.X() * y - input.Y() * x; // multiply q by (qX, qY, qY, qR). For out quaternion, // element 4 (qR) is not interesting since we only want the // vector. output.X() = - r * qX - x * qR - y * qZ + z * qY; output.Y() = - r * qY - y * qR - z * qX + x * qZ; output.Z() = - r * qZ - z * qR - x * qY + y * qX; } template inline void ApplyTo(const vctDynamicConstVectorBase<__vectorOwnerType, value_type> & input, vctFixedSizeVectorBase & output) const { CMN_ASSERT(input.Pointer() != output.Pointer()); CMN_ASSERT(input.size() == DIMENSION); // could build a vector with a 0 as real component, create the // conjugate and then perfom two quaternion products but his // seems more efficient. value_type qR, qX, qY, qZ; const value_type x = this->X(); const value_type y = this->Y(); const value_type z = this->Z(); const value_type r = this->R(); // compute the product of quaternions q_input = [in, 0] and q // conjugate. Since q_input[3] = 0, suppress terms with it. qR = - input.X() * x - input.Y() * y - input.Z() * z; qX = - input.X() * r + input.Y() * z - input.Z() * y; qY = - input.Y() * r + input.Z() * x - input.X() * z; qZ = - input.Z() * r + input.X() * y - input.Y() * x; // multiply q by (qX, qY, qY, qR). For out quaternion, // element 4 (qR) is not interesting since we only want the // vector. output.X() = - r * qX - x * qR - y * qZ + z * qY; output.Y() = - r * qY - y * qR - z * qX + x * qZ; output.Z() = - r * qZ - z * qR - x * qY + y * qX; } /*! Apply the rotation to a vector of fixed size 3. The result is returned by copy. This interface might be more convenient for some but one should note that it is less efficient since it requires a copy. \param input The input vector \return The output vector */ template inline vctFixedSizeVector ApplyTo(const vctFixedSizeConstVectorBase & input) const { vctFixedSizeVector result; this->ApplyTo(input, result); return result; } template inline vctFixedSizeVector ApplyTo(const vctDynamicConstVectorBase<__vectorOwnerType, value_type> & input) const { CMN_ASSERT(input.size() == DIMENSION); vctFixedSizeVector result; this->ApplyTo(input, result); return result; } /*! Apply the rotation to another rotation. The result is stored into a vctQuaternionRotation3Base (ThisType) provided by the caller and passed by reference. \param input The input rotation \param output The output rotation */ inline void ApplyTo(const ThisType & input, ThisType & output) const { CMN_ASSERT(input.Pointer() != output.Pointer()); output.ProductOf(*this, input); } /*! Apply the rotation to another rotation. The result is returned by copy. This interface might be more convenient for some but one should note that it is less efficient since it requires a copy. \param input The input rotation \return The output rotation */ inline ThisType ApplyTo(const ThisType & input) const { ThisType result; this->ApplyTo(input, result); return result; } /*! Apply the rotation to a dynamic vector. The result is stored into another dynamic vector passed by reference by the caller. It is assumed that both are of size 3. */ template inline void ApplyTo(const vctDynamicConstVectorBase<_vectorOwnerType1, value_type> & input, vctDynamicVectorBase<_vectorOwnerType2, value_type> & output) const { CMN_ASSERT(input.Pointer() != output.Pointer()); CMN_ASSERT(input.size() == DIMENSION); CMN_ASSERT(output.size() == DIMENSION); // see code for fixed size input/output value_type qR, qX, qY, qZ; const value_type x = this->X(); const value_type y = this->Y(); const value_type z = this->Z(); const value_type r = this->R(); qR = - input.X() * x - input.Y() * y - input.Z() * z; qX = - input.X() * r + input.Y() * z - input.Z() * y; qY = - input.Y() * r + input.Z() * x - input.X() * z; qZ = - input.Z() * r + input.X() * y - input.Y() * x; output.X() = - r * qX - x * qR - y * qZ + z * qY; output.Y() = - r * qY - y * qR - z * qX + x * qZ; output.Z() = - r * qZ - z * qR - x * qY + y * qX; } /*! Apply the inverse of the rotation to a vector of fixed size 3. The result is stored into a vector of size 3 provided by the caller and passed by reference. \param input The input vector \param output The output vector */ template inline void ApplyInverseTo(const vctFixedSizeConstVectorBase & input, vctFixedSizeVectorBase & output) const { CMN_ASSERT(input.Pointer() != output.Pointer()); // could build a vector with a 0 as real component, create the // conjugate and then perfom two quaternion products but his // seems more efficient. value_type qR, qX, qY, qZ; const value_type x = this->X(); const value_type y = this->Y(); const value_type z = this->Z(); const value_type r = this->R(); // compute the product of quaternions q_input = [in, 0] and q // conjugate. Since q_input[3] = 0, suppress terms with it. qR = input.X() * x + input.Y() * y + input.Z() * z; qX = - input.X() * r - input.Y() * z + input.Z() * y; qY = - input.Y() * r - input.Z() * x + input.X() * z; qZ = - input.Z() * r - input.X() * y + input.Y() * x; // multiply q by (qX, qY, qY, qR). For out quaternion, // element 4 (qR) is not interesting since we only want the // vector. output.X() = - r * qX + x * qR + y * qZ - z * qY; output.Y() = - r * qY + y * qR + z * qX - x * qZ; output.Z() = - r * qZ + z * qR + x * qY - y * qX; } template inline void ApplyInverseTo(const vctDynamicConstVectorBase<__vectorOwnerType, value_type> & input, vctFixedSizeVectorBase & output) const { CMN_ASSERT(input.Pointer() != output.Pointer()); CMN_ASSERT(input.size() == DIMENSION); // could build a vector with a 0 as real component, create the // conjugate and then perfom two quaternion products but his // seems more efficient. value_type qR, qX, qY, qZ; const value_type x = this->X(); const value_type y = this->Y(); const value_type z = this->Z(); const value_type r = this->R(); // compute the product of quaternions q_input = [in, 0] and q // conjugate. Since q_input[3] = 0, suppress terms with it. qR = input.X() * x + input.Y() * y + input.Z() * z; qX = - input.X() * r - input.Y() * z + input.Z() * y; qY = - input.Y() * r - input.Z() * x + input.X() * z; qZ = - input.Z() * r - input.X() * y + input.Y() * x; // multiply q by (qX, qY, qY, qR). For out quaternion, // element 4 (qR) is not interesting since we only want the // vector. output.X() = - r * qX + x * qR + y * qZ - z * qY; output.Y() = - r * qY + y * qR + z * qX - x * qZ; output.Z() = - r * qZ + z * qR + x * qY - y * qX; } /*! Apply the the inverse of the rotation to a vector of fixed size 3. The result is returned by copy. This interface might be more convenient for some but one should note that it is less efficient since it requires a copy. \param input The input vector \return The output vector */ template inline vctFixedSizeVector ApplyInverseTo(const vctFixedSizeConstVectorBase & input) const { vctFixedSizeVector result; this->ApplyInverseTo(input, result); return result; } template inline vctFixedSizeVector ApplyInverseTo(const vctDynamicConstVectorBase<__vectorOwnerType, value_type> & input) const { CMN_ASSERT(input.size() == DIMENSION); vctFixedSizeVector result; this->ApplyInverseTo(input, result); return result; } /*! Apply the inverse of the rotation to another rotation. The result is stored into a vctQuaternionRotation3Base (ThisType) provided by the caller and passed by reference. \param input The input rotation \param output The output rotation */ inline void ApplyInverseTo(const ThisType & input, ThisType & output) const { CMN_ASSERT(input.Pointer() != output.Pointer()); output.ProductOf(this->Conjugate(), input); } /*! Apply the inverse of the rotation to another rotation. The result is returned by copy. This interface might be more convenient for some but one should note that it is less efficient since it requires a copy. \param input The input rotation \return The output rotation */ inline ThisType ApplyInverseTo(const ThisType & input) const { ThisType result; this->ApplyInverseTo(input, result); return result; } /*! Apply the the inverse of the rotation to a dynamic vector. The result is stored into another dynamic vector passed by reference by the caller. It is assumed that both are of size 3. */ template inline void ApplyInverseTo(const vctDynamicConstVectorBase<_vectorOwnerType1, value_type> & input, vctDynamicVectorBase<_vectorOwnerType2, value_type> & output) const { CMN_ASSERT(input.Pointer() != output.Pointer()); CMN_ASSERT(input.size() == DIMENSION); CMN_ASSERT(output.size() == DIMENSION); // see code for fixed size input/output value_type qR, qX, qY, qZ; const value_type x = this->X(); const value_type y = this->Y(); const value_type z = this->Z(); const value_type r = this->R(); qR = input.X() * x + input.Y() * y + input.Z() * z; qX = - input.X() * r - input.Y() * z + input.Z() * y; qY = - input.Y() * r - input.Z() * x + input.X() * z; qZ = - input.Z() * r - input.X() * y + input.Y() * x; output.X() = - r * qX + x * qR + y * qZ - z * qY; output.Y() = - r * qY + y * qR + z * qX - x * qZ; output.Z() = - r * qZ + z * qR + x * qY - y * qX; } /*! Return true if this quaternion rotation is equivalent to the other quaternion rotation, up to the given tolerance. Quaternion rotation may be effectively equivalent if one is elementwise equal to the other, or if one is the negation of the other. The tolerance factor is used to compare each of the elements of the difference vector. \sa AlmostEqual */ inline bool AlmostEquivalent(const ThisType & other, value_type tolerance = TypeTraits::Tolerance()) const { ThisType differenceVector; differenceVector.DifferenceOf(*this, other); differenceVector.AbsSelf(); if (differenceVector.Lesser(tolerance)) { return true; } differenceVector.SumOf(*this, other); differenceVector.AbsSelf(); if (differenceVector.Lesser(tolerance)) { return true; } return false; } /*! Multiply two rotation quaternions and return the result as a normalized rotation quaternion. \return (*this) * other \note this function overrides and shadows the operator * defined for basic quaternion type. The latter returns a vctQuaternion, while this version returns a specialized vctQuaternionRotation3Base */ ThisType operator * (const ThisType & input) const { CMN_ASSERT(input.Pointer() != this->Pointer()); return this->ApplyTo(input); } /*! \name Deprecated methods */ //@{ /*! Convert from a unit vector and an angle. It is important to note that this method doesn't normalize the axis vector but asserts that it is normalized (in debug mode only). \param axis A unit vector of size 3. \param angle The angle in radian \note This method is deprecated. Use From(vctAxisAngleRotation3) instead. */ template inline CISST_DEPRECATED const ThisType & From(const vctFixedSizeConstVectorBase<3, __stride, value_type, __dataPtrType> & axis, const AngleType & angle) { assert(vctUnaryOperations::AbsValue::Operate(value_type(axis.Norm() - 1)) < cmnTypeTraits::Tolerance()); const double halfAngle = angle * 0.5; const value_type s = value_type(sin(halfAngle)); const value_type c = value_type(cos(halfAngle)); this->ConcatenationOf(s * axis, c); return *this; } /*! Convert from ``Rodriguez rotation'' to rotation quaternion. A Rodriguez rotation is a 3-element vector whose direction is the rotation axis, and magnitude is the rotation angle in radians. \param rodriguezRotation the Rodriguez rotation \note For reasons of numerical stability, if the magnitude of the Rodriguez vector is less than cmnTypeTraits::Tolerance(), it is regarded as zero. \note This method is deprecated. Use From(vctRodriguezRotation3Base) instead. */ template inline CISST_DEPRECATED const ThisType & From(const vctFixedSizeConstVectorBase<3, __stride, value_type, __dataPtrType> & rodriguezRotation) { const value_type axisLength = rodriguezRotation.Norm(); const value_type axisTolerance = cmnTypeTraits::Tolerance(); const double angle = (fabs(axisLength) < axisTolerance) ? 0.0 : axisLength; const vctFixedSizeVector defaultAxis(value_type(1), value_type(0), value_type(0)); const vctFixedSizeVector axis( (angle == value_type(0)) ? defaultAxis : (rodriguezRotation / axisLength) ); return this->From(axis, angle); } /*! Convert to an axis and angle representation. It is important to note that this method doesn't check if the rotation quaternion is normalized or not. \param axis The axis of the rotation \param angle The angle around the axis to match the rotation \note This method is deprecated. Use vctAxisAngleRotation3.From() instead. */ template inline CISST_DEPRECATED void GetAxisAngle(vctFixedSizeVectorBase<3, __stride, value_type, __dataPtrType> & axis, value_type & angle) { angle = value_type(acos(this->R()) * 2); value_type sinAngle = value_type(sqrt(1.0 - this->R() * this->R())); if (vctUnaryOperations::AbsValue::Operate(sinAngle) > cmnTypeTraits::Tolerance()) { axis.X() = this->X() / sinAngle; axis.Y() = this->Y() / sinAngle; axis.Z() = this->Z() / sinAngle; } else { axis.X() = this->X(); axis.Y() = this->Y(); axis.Z() = this->Z(); } } //@} }; /* Specialization of Allocate for a dynamic matrix */ template<> inline void vctQuaternionRotation3Base >::Allocate(void) { this->SetSize(SIZE); } template<> inline void vctQuaternionRotation3Base >::Allocate(void) { this->SetSize(SIZE); } template inline vctFixedSizeVector<_elementType, 3> operator * (const vctQuaternionRotation3Base<_containerType> & rotationQuaternion, const vctFixedSizeConstVectorBase<3, _stride, _elementType, _dataPtrType> & vector) { vctFixedSizeVector<_elementType, 3> result; rotationQuaternion.ApplyTo(vector, result); return result; } #endif // _vctQuaternionRotation3Base_h // **************************************************************************** // Change History // **************************************************************************** // // $Log: vctQuaternionRotation3Base.h,v $ // Revision 1.14 2007/04/26 19:33:58 anton // All files in libraries: Applied new license text, separate copyright and // updated dates, added standard header where missing. // // Revision 1.13 2007/03/08 21:11:03 anton // cisstVector: Added missing explicit export for Visual Studio compiler for // templated functions. // // Revision 1.12 2007/01/29 03:29:37 anton // cisstVector: Update quaternion and quaternion based rotation to reflect new // class structure (see ticket #263). // // Revision 1.11 2007/01/23 20:59:27 anton // cisstVector: Updated transformations classes so that non const methods // returning a reference on "this" return a non const reference. This follows // ticket #259. // // Revision 1.10 2006/11/20 20:33:20 anton // Licensing: Applied new license to cisstCommon, cisstVector, cisstNumerical, // cisstInteractive, cisstImage and cisstOSAbstraction. // // Revision 1.9 2006/09/26 21:47:47 anton // cisstVector and cisstNumerical: Use #ifdef CISST_COMPILER_IS_MSVC instead // of #if (CISST_COMPILER == CISST_XYZ) for XYZ in Visual Studio 7~2000, 7.1~2003 // and 8~2005. // // Revision 1.8 2006/01/20 04:46:36 anton // cisstVector: Added methods AlmostEquivalent for all transformations as well // as some missing Equal (and ==). See ticket #204. // // Revision 1.7 2005/11/15 03:32:34 anton // vctQuaternionRotation3Base: // *: FromRaw(vctRodriguez) use correct ctor for temporary vctAxisAngle // *: Redefined Normalized method so that returned value is a // vctQuaternionRotation, not a base type (i.e. vctQuaternion) // // Revision 1.6 2005/10/08 20:18:58 anton // vctMatRot3Base and vctQuatRot3Base: Use assert, not throw, if size of dynamic // vector if not 3 (see #180). Updated vctQuatRot3Base to operate on dynamic // vectors. // // Revision 1.5 2005/10/06 16:56:37 anton // Doxygen: Corrected errors and some warnings detected by Doxygen 1.4.3. // // Revision 1.4 2005/09/28 20:31:06 anton // vctQuaternionRotation: Major update to implement ticket #110 re. initial // conditions for conversion methods (i.e. normalization of input). // // Revision 1.3 2005/09/26 15:41:47 anton // cisst: Added modelines for emacs and vi. // // Revision 1.2 2005/09/05 22:54:22 ofri // Added constructor for vctRodriguezRotation3Base, following ticket #169. Plus a // few cosmetic changes, such as replacing const_reference parameters with // value_type // // Revision 1.1 2005/09/01 06:44:47 anton // vctQuaternionRotation3Base: Omited new file in check-in [1381]. // // // ****************************************************************************