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());
}
