From 7a8d0d8bc2572707c9d35006f30ea835c86954b0 Mon Sep 17 00:00:00 2001 From: sotech117 Date: Tue, 9 Apr 2024 03:14:17 -0400 Subject: first draft to generate waves --- Eigen/src/Geometry/Rotation2D.h | 199 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 199 insertions(+) create mode 100644 Eigen/src/Geometry/Rotation2D.h (limited to 'Eigen/src/Geometry/Rotation2D.h') diff --git a/Eigen/src/Geometry/Rotation2D.h b/Eigen/src/Geometry/Rotation2D.h new file mode 100644 index 0000000..d0bd575 --- /dev/null +++ b/Eigen/src/Geometry/Rotation2D.h @@ -0,0 +1,199 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_ROTATION2D_H +#define EIGEN_ROTATION2D_H + +namespace Eigen { + +/** \geometry_module \ingroup Geometry_Module + * + * \class Rotation2D + * + * \brief Represents a rotation/orientation in a 2 dimensional space. + * + * \tparam _Scalar the scalar type, i.e., the type of the coefficients + * + * This class is equivalent to a single scalar representing a counter clock wise rotation + * as a single angle in radian. It provides some additional features such as the automatic + * conversion from/to a 2x2 rotation matrix. Moreover this class aims to provide a similar + * interface to Quaternion in order to facilitate the writing of generic algorithms + * dealing with rotations. + * + * \sa class Quaternion, class Transform + */ + +namespace internal { + +template struct traits > +{ + typedef _Scalar Scalar; +}; +} // end namespace internal + +template +class Rotation2D : public RotationBase,2> +{ + typedef RotationBase,2> Base; + +public: + + using Base::operator*; + + enum { Dim = 2 }; + /** the scalar type of the coefficients */ + typedef _Scalar Scalar; + typedef Matrix Vector2; + typedef Matrix Matrix2; + +protected: + + Scalar m_angle; + +public: + + /** Construct a 2D counter clock wise rotation from the angle \a a in radian. */ + EIGEN_DEVICE_FUNC explicit inline Rotation2D(const Scalar& a) : m_angle(a) {} + + /** Default constructor wihtout initialization. The represented rotation is undefined. */ + EIGEN_DEVICE_FUNC Rotation2D() {} + + /** Construct a 2D rotation from a 2x2 rotation matrix \a mat. + * + * \sa fromRotationMatrix() + */ + template + EIGEN_DEVICE_FUNC explicit Rotation2D(const MatrixBase& m) + { + fromRotationMatrix(m.derived()); + } + + /** \returns the rotation angle */ + EIGEN_DEVICE_FUNC inline Scalar angle() const { return m_angle; } + + /** \returns a read-write reference to the rotation angle */ + EIGEN_DEVICE_FUNC inline Scalar& angle() { return m_angle; } + + /** \returns the rotation angle in [0,2pi] */ + EIGEN_DEVICE_FUNC inline Scalar smallestPositiveAngle() const { + Scalar tmp = numext::fmod(m_angle,Scalar(2*EIGEN_PI)); + return tmpScalar(EIGEN_PI)) tmp -= Scalar(2*EIGEN_PI); + else if(tmp<-Scalar(EIGEN_PI)) tmp += Scalar(2*EIGEN_PI); + return tmp; + } + + /** \returns the inverse rotation */ + EIGEN_DEVICE_FUNC inline Rotation2D inverse() const { return Rotation2D(-m_angle); } + + /** Concatenates two rotations */ + EIGEN_DEVICE_FUNC inline Rotation2D operator*(const Rotation2D& other) const + { return Rotation2D(m_angle + other.m_angle); } + + /** Concatenates two rotations */ + EIGEN_DEVICE_FUNC inline Rotation2D& operator*=(const Rotation2D& other) + { m_angle += other.m_angle; return *this; } + + /** Applies the rotation to a 2D vector */ + EIGEN_DEVICE_FUNC Vector2 operator* (const Vector2& vec) const + { return toRotationMatrix() * vec; } + + template + EIGEN_DEVICE_FUNC Rotation2D& fromRotationMatrix(const MatrixBase& m); + EIGEN_DEVICE_FUNC Matrix2 toRotationMatrix() const; + + /** Set \c *this from a 2x2 rotation matrix \a mat. + * In other words, this function extract the rotation angle from the rotation matrix. + * + * This method is an alias for fromRotationMatrix() + * + * \sa fromRotationMatrix() + */ + template + EIGEN_DEVICE_FUNC Rotation2D& operator=(const MatrixBase& m) + { return fromRotationMatrix(m.derived()); } + + /** \returns the spherical interpolation between \c *this and \a other using + * parameter \a t. It is in fact equivalent to a linear interpolation. + */ + EIGEN_DEVICE_FUNC inline Rotation2D slerp(const Scalar& t, const Rotation2D& other) const + { + Scalar dist = Rotation2D(other.m_angle-m_angle).smallestAngle(); + return Rotation2D(m_angle + dist*t); + } + + /** \returns \c *this with scalar type casted to \a NewScalarType + * + * Note that if \a NewScalarType is equal to the current scalar type of \c *this + * then this function smartly returns a const reference to \c *this. + */ + template + EIGEN_DEVICE_FUNC inline typename internal::cast_return_type >::type cast() const + { return typename internal::cast_return_type >::type(*this); } + + /** Copy constructor with scalar type conversion */ + template + EIGEN_DEVICE_FUNC inline explicit Rotation2D(const Rotation2D& other) + { + m_angle = Scalar(other.angle()); + } + + EIGEN_DEVICE_FUNC static inline Rotation2D Identity() { return Rotation2D(0); } + + /** \returns \c true if \c *this is approximately equal to \a other, within the precision + * determined by \a prec. + * + * \sa MatrixBase::isApprox() */ + EIGEN_DEVICE_FUNC bool isApprox(const Rotation2D& other, const typename NumTraits::Real& prec = NumTraits::dummy_precision()) const + { return internal::isApprox(m_angle,other.m_angle, prec); } + +}; + +/** \ingroup Geometry_Module + * single precision 2D rotation type */ +typedef Rotation2D Rotation2Df; +/** \ingroup Geometry_Module + * double precision 2D rotation type */ +typedef Rotation2D Rotation2Dd; + +/** Set \c *this from a 2x2 rotation matrix \a mat. + * In other words, this function extract the rotation angle + * from the rotation matrix. + */ +template +template +EIGEN_DEVICE_FUNC Rotation2D& Rotation2D::fromRotationMatrix(const MatrixBase& mat) +{ + EIGEN_USING_STD(atan2) + EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==2 && Derived::ColsAtCompileTime==2,YOU_MADE_A_PROGRAMMING_MISTAKE) + m_angle = atan2(mat.coeff(1,0), mat.coeff(0,0)); + return *this; +} + +/** Constructs and \returns an equivalent 2x2 rotation matrix. + */ +template +typename Rotation2D::Matrix2 +EIGEN_DEVICE_FUNC Rotation2D::toRotationMatrix(void) const +{ + EIGEN_USING_STD(sin) + EIGEN_USING_STD(cos) + Scalar sinA = sin(m_angle); + Scalar cosA = cos(m_angle); + return (Matrix2() << cosA, -sinA, sinA, cosA).finished(); +} + +} // end namespace Eigen + +#endif // EIGEN_ROTATION2D_H -- cgit v1.2.3-70-g09d2