inline static typename get_matrix3X<Rotation_>::template Matrix3X<Cols> rotate(const RotationBase<Rotation_, Usage_>& rotation, const typename internal::get_matrix3X<Rotation_>::template Matrix3X<Cols>& m) { if(Usage_ == RotationUsage::ACTIVE) { return eigen_impl::RotationMatrix<typename Rotation_::Scalar, Usage_>(rotation.derived()).toImplementation()*m; } else { return eigen_impl::RotationMatrix<typename Rotation_::Scalar, Usage_>(rotation.derived().inverted()).toImplementation()*m; } }
inline static Vector_ rotate(const RotationBase<Rotation_, Usage_>& rotation, const Vector_& vector) { return static_cast<Vector_>(rotation.derived().rotate(vector.toImplementation())); // if(Usage_ == RotationUsage::ACTIVE){ // // } else { // return static_cast<Vector_>(eigen_impl::RotationMatrix<typename Rotation_::Scalar, Usage_>(rotation.derived().inverted()).toImplementation()*vector.toImplementation()); // } }
inline explicit EulerAnglesZyxDiff(const RotationBase<RotationDerived_, Usage_>& rotation, const RotationDiffBase<OtherDerived_, Usage_>& other) : zyxDiff_(internal::RotationDiffConversionTraits<EulerAnglesZyxDiff, OtherDerived_, RotationDerived_>::convert(rotation.derived(), other.derived()).toImplementation()){ }
OtherDerived_ cast(const RotationBase<RotationDerived_, Usage_>& rotation) const { return internal::RotationDiffConversionTraits<OtherDerived_, EulerAnglesZyxDiff, RotationDerived_>::convert(rotation.derived(), *this); }
//! Default multiplication of rotations converts the representations of the rotations to rotation quaternions and multiplies them inline static Left_ mult(const RotationBase<Left_, Usage_>& lhs, const RotationBase<Right_, Usage_>& rhs) { if(Usage_ == RotationUsage::ACTIVE) { return Left_(typename eigen_impl::RotationQuaternion<typename Left_::Scalar, Usage_>( (typename eigen_impl::RotationQuaternion<typename Left_::Scalar, Usage_>(lhs.derived())).toImplementation() * (typename eigen_impl::RotationQuaternion<typename Right_::Scalar, Usage_>(rhs.derived())).toImplementation() )); } else { return Left_(typename eigen_impl::RotationQuaternion<typename Left_::Scalar, Usage_>( (typename eigen_impl::RotationQuaternion<typename Right_::Scalar, Usage_>(rhs.derived())).toImplementation() * (typename eigen_impl::RotationQuaternion<typename Left_::Scalar, Usage_>(lhs.derived())).toImplementation() )); } }
inline static Left_ box_plus(const RotationBase<Left_, Usage_>& rotation, const typename internal::get_matrix3X<Left_>::template Matrix3X<1>& vector) { return Left_(MapTraits<RotationBase<Left_,Usage_>>::set_exponential_map(vector)*rotation.derived()); }
/*! \brief Gets the disparity angle between two rotations. * * The disparity angle is defined as the angle of the angle-axis representation of the concatenation of * the first rotation and the inverse of the second rotation. If the disparity angle is zero, * the rotations are equal. * \returns disparity angle in [-pi,pi) @todo: is this range correct? */ inline static typename Left_::Scalar get_disparity_angle(const RotationBase<Left_, Usage_>& left, const RotationBase<Right_, Usage_>& right) { return fabs(common::floatingPointModulo(eigen_impl::AngleAxis<typename Left_::Scalar, Usage_>(left.derived()*right.derived().inverted()).angle() + M_PI,2*M_PI)-M_PI); }
inline static typename internal::get_matrix3X<Left_>::template Matrix3X<1> box_minus(const RotationBase<Left_, Usage_>& lhs, const RotationBase<Right_, Usage_>& rhs) { return (lhs.derived()*rhs.derived().inverted()).logarithmicMap(); }
typename internal::get_matrix3X<Derived_>::template Matrix3X<1> boxMinus(const RotationBase<OtherDerived_>& other) const { return internal::BoxOperationTraits<RotationBase<Derived_>, RotationBase<OtherDerived_>>::box_minus(this->derived(), other.derived()); }
inline explicit EulerAnglesXyz(const RotationBase<OtherDerived_>& other) : xyz_(internal::ConversionTraits<EulerAnglesXyz, OtherDerived_>::convert(other.derived()).toImplementation()) { }
typename internal::get_scalar<Derived_>::Scalar getDisparityAngle(const RotationBase<OtherDerived_>& other) const { return internal::DisparityAngleTraits<RotationBase<Derived_>, RotationBase<OtherDerived_>>::compute(this->derived(), other.derived()); }
bool operator ==(const RotationBase<OtherDerived_>& other) const { // todo: may be optimized return internal::ComparisonTraits<RotationBase<Derived_>, RotationBase<OtherDerived_>>::isEqual(this->derived().getUnique(), other.derived().getUnique()); // the type conversion must already take place here to ensure the specialised isequal function is called more often }
Derived_ operator *(const RotationBase<OtherDerived_>& other) const { return internal::MultiplicationTraits<RotationBase<Derived_>,RotationBase<OtherDerived_>>::mult(this->derived(), other.derived()); // todo: 1. ok? 2. may be optimized }
inline static typename get_other_usage<Derived_>::OtherUsage getPassive(const RotationBase<Derived_,RotationUsage::ACTIVE>& in) { return typename get_other_usage<Derived_>::OtherUsage(in.derived().inverted().toImplementation()); }
typename internal::get_scalar<Derived_>::Scalar getDisparityAngle(const RotationBase<OtherDerived_,Usage_>& other) const { return internal::ComparisonTraits<RotationBase<Derived_, Usage_>, RotationBase<OtherDerived_, Usage_>>::get_disparity_angle(this->derived(), other.derived()); }