//---------------------------------------------------------------------------- //Description: The Mat4T class is a utility class for reference frame / matrix //rotations, scalings, and translations. This matrix class is column major and //assumes all in passed Vectors are also column major. As a result, this class //performs post multiplication against other Mat4s and Vectors. // //Useful operations are also provided to manipulate a Matrices and verticies. //Additionally, the BIG 3 are implemented. // //This class is defined as a templated class that has a user-specified //storage type. The most expected user specified types are float, double, //and other floating point types (perhaps long double)?. Either way, //since float and double are considered the "most common" usages, this //Mat4T class declares two aliases as defined in MatrixFwd.h. In this file //Matrix is an alias to Mat4T and MatrixD is an alias to //Mat4T. // //To PREDECLARE this class in a header file, simply #include "MatrixFwd.h" //Otherwise, the user will have to retype the alias definitions made //within MatrixFwd.h. // //Author: Scott Nykl //---------------------------------------------------------------------------- #pragma once #include "AftrConfig.h" #include "AftrGlobals.h" #include "Mat4Fwd.h" #include "VectorFwd.h" #include "Vector_Mat4_Common.h" #include "QuatFwd.h" #include "Vector.h" #include "Mat4Impl.h" #include "Quat.h" #include #include #include #include #include namespace Aftr { template< typename T > class Mat4T { public: using value_type = T; static constexpr size_t NUM_ELEMS = 16; static void setToIdentity4x4( std::array& m ) noexcept; ///------------------------------------------------------------------------ /// Column-major 4x4 matrix default constructor; initializes to identity. ///------------------------------------------------------------------------ Mat4T() noexcept; ///------------------------------------------------------------------------ /// Copy constructor creates a copy of the passed in Matrix. /// const Mat4T& m: the Matrix to be copied. ///------------------------------------------------------------------------ Mat4T( const Mat4T& m ) noexcept; ///------------------------------------------------------------------------- /// Constructor which creates a Matrix from the first 16 elements from the /// in passed T* array. T* must contain at least 16 elements or the behavior /// is undefined. The in passed T* is only copied so ownership is /// NOT passed to this instance. ///------------------------------------------------------------------------- explicit Mat4T( const T* Mat4ToBeCopied ) noexcept; ///------------------------------------------------------------------------- /// Constructor which creates a Rotation Matrix (DCM / SO(3)) from the in /// passed AxisAngle instance. Note this does NOT create a skew symmetric /// matrix, but a normal Rotation Matrix. ///------------------------------------------------------------------------- Mat4T( AxisAngleT const& a ) noexcept; ///------------------------------------------------------------------------- /// Constructor which creates a Rotation Matrix (DCM / SO(3)) from the in /// passed Quaternion instance. Note this does NOT create a skew symmetric /// matrix, but a normal Rotation Matrix. Same as calling /// q.toRotationMatrix(). ///------------------------------------------------------------------------- Mat4T( QuatT const& q ) noexcept; ///------------------------------------------------------------------------ /// Constructor which creates a Mat4T from the passed in string. /// String str: The str which is parsed to create the Mat4T. /// The input format is a string containing at least 16 numbers. A number may /// look like: 1.2 0.2 2 .3 1.2e+3 -.3 45.334E-32, etc. /// The numbers can be separated by any separator or set of separators that /// do not include a format matched by above. /// Specifically, given a string, the first 16 matches found by the regular /// expression: "[-+]?[0-9]*\\.?[0-9]+([eE][-+]?[0-9]+)?" /// will be used as input. /// /// If the input string contains less than 2 non-starting, non-terminating, /// non-adjacent NEWLINES ('\n') then the input string is considering one /// contiguous list of numbers AND: /// -If the second parameter is true (default), the INPUT STRING is /// interpreted as COLUMN major. Since the Mat4 is also column major, /// the **columns** of the input string will be placed in the corresponding /// columns of the Mat4. See examples below. /// /// -If the second parameter is false, the INPUT STRING is interpreted as /// ROW major. Since Mat4 is column major, the constructed Mat4 will /// place each input **row** into the corresponding column of the Mat4. /// /// If the input string contains more than 2 non-starting, non-terminating, /// non-adjacent NEWLINES ('\n') then: /// -If the second parameter is true, the each \n-terminated substring /// will be interpreted a one COLUMN vector, See m_b in example below. /// -If the second parameter is false, the each \n terminated substring /// will be interpreted as one ROW vector. /// /// However, the user must determine if the input string is specified in /// ROW major or COLUMN major form. Naturally, this isn't as simple as it /// may sound -- consider the following example: /// /// std::string m_a = "1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16\n"; /// std::string m_b = R"( 1 5 9 13\n // notice more than 2 interleaved /// 2 6 10 14\n // \n. Each \n denotes one vector /// 3 7 11 15\n // that will be come a column (2nd param == true) /// 4 8 12 16 )"; // or become a row (2nd param == false) /// Both m_a and m_b will produce the same Mat4 with following values: /// Mat4 a{ m_a, true }; //a and b have identical values /// Mat4 b{ m_b, true }; //a and b have identical values /// a.toString() -> "1 5 9 13 //X col is 1 2 3 4, Y col is 5 6 7 8 /// 2 6 10 14 //Z col is 9 10 11 12, pos is 13 14 15. /// 3 7 11 15 /// 4 8 12 16"; /// /// Now consider passing false as the second argument: /// Mat4 a{ m_a, false }; //a and b have identical values /// Mat4 b{ m_b, false }; //a and b have identical values /// a.toString() -> "1 2 3 4 //X col is 1 5 9 13, Y col is 2 6 10 14 /// 5 6 7 8 //Z col is 3 7 11 15, pos is 4 8 12. /// 9 10 11 12 /// 13 14 15 16"; /// /// Another example, one can also use ; instead of newlines to quickly /// create a Mat4: /// Mat4 a{ "1 2 3 4; 5 6 7 8; 9 10 11 12; 13 14 15 16", true }; //default /// a.toString() -> " 1 2 3 4 //X col is 1 5 9 13, Y col is 2 6 10 14 /// 5 6 7 8 //Z col is 3 7 11 15, pos is 4 8 12 /// 9 10 11 12 /// 13 14 15 16" //cols of input string become cols of Mat4 /// /// Mat4 b{ "1 2 3 4; 5 6 7 8; 9 10 11 12; 13 14 15 16", false }; /// b.toString() -> " 1 5 9 13 //X col is 1 2 3 4, Y col is 5 6 7 8 /// 2 6 10 14 //Z col is 9 10 11 12, pos is 13 14 15 /// 3 7 11 15 /// 4 8 12 16" //rows of input string become cols of Mat4 ///------------------------------------------------------------------------ Mat4T( const std::string& str, bool inputStringIsColumnMajor = true ) noexcept; Mat4T( const VectorT& x, const VectorT& y, const VectorT& z ) noexcept; Mat4T( const VectorT& x, const VectorT& y, const VectorT& z, const VectorT& translation ) noexcept; /// Performs pair-wise index assignment. That is, this->data[i] = m[i] for /// all elements of m. Mat4T are column major, so element 0, 1, 2 should /// be interpreted as the first 3 elements of the x column vector. Mat4T( std::array const& m ) noexcept; ///------------------------------------------------------------------------ /// Destructor ///------------------------------------------------------------------------ ~Mat4T() noexcept {}; ///------------------------------------------------------------------------ /// Copies 16 values from the input into this Mat4's internal matrix. ///------------------------------------------------------------------------ void set( const T* mat4x4 ); void set( Mat4T const& m ) noexcept; ///------------------------------------------------------------------------ /// Resets the X,Y,Z values to 0. ///------------------------------------------------------------------------ void setMeToIdentity() noexcept; ///------------------------------------------------------------------------ /// Multiplies the upper 3x3 portion of this matrix by the in passed vector /// and returns the resultant Vector. The translation column of this matrix /// is NOT added to the result; that is, a Vector (direction only) is /// returned. If you would like to transform a vertex, either add the /// fourth column of this matrix to the result, or use a Vector4 instance. /// Optionally, you may also use the operator * which takes an /// std::pair< Vector, T >, where the second item is the 4th coordinate /// (ie, the 'w' in the x,y,z,w in passed values). /// Returns: Vector: the column vector transformed by the upper 3x3 of this /// submatrix of this matrix; the translation column (4th column) of this /// matrix is NOT added to the result. ///------------------------------------------------------------------------ [[nodiscard]] VectorT operator *( const VectorT& v ) const noexcept; ///------------------------------------------------------------------------ /// Multiplies a 4 element column major vector by this matrix and returns /// a 4 element column major vector. The 4 element vector is broken into /// two parts: A) the first part of the pair, which defines the X,Y,Z /// components, and B) the second part of the pair, which defines the X /// component. /// The resultant 4 element vector is return as an std::pair in the same /// format as the input. ///------------------------------------------------------------------------ [[nodiscard]] std::pair< VectorT, T > operator *( const std::pair< const VectorT&, T >& homogeniusCoord ) const noexcept; ///------------------------------------------------------------------------ /// Multiplies a 4 element column major vector by this matrix and returns /// a 3 ///------------------------------------------------------------------------ ///VectorT multiply4ElementVec( const VectorT& v, T h ) const; //Scott: contemplate equivalent "translateMe" functions which manipulate the existing object in equivalent ways //not sure if this class is implicitly using move semantics, so unsure if this will improve performance in //cases where old values are not needed, wasn't immediately obvious as a user that these wouldn't //translate the object they were called on [[nodiscard]] Mat4T translate( const VectorT& t ) const noexcept; static Mat4T translateIdentityMat( const VectorT& t ) noexcept; ///Scale scales the x, y, and z components of a copy of this by s.x, s.y, and s.z and returns it, to assign the diagonal use setScale() [[nodiscard]] Mat4T scale( const VectorT& s ) const noexcept; [[nodiscard]] static Mat4T scaleIdentityMat( const VectorT& s ) noexcept; [[nodiscard]] Mat4T transposeUpperLeft3x3() const noexcept; [[nodiscard]] Mat4T transpose4x4() const noexcept; /// Identical to calling transpose4x4(), just shorter notation. [[nodiscard]] Mat4T tp() const noexcept { return this->transpose4x4(); } ///------------------------------------------------------------------------ /// Returns this Matrix rotated about axisNormalied by angleRads radians. /// Internally, an identity matrix is rotated by the in passed params and /// then is post multiplied by this matrix. /// Return: this matrix rotated by the in passed parameters. /// returnMat := rotatedIdentity * (*this); (poaftrultiplication) ///------------------------------------------------------------------------ [[nodiscard]] Mat4T rotate( const VectorT& axisNormalized, T const angleRads ) const noexcept; ///------------------------------------------------------------------------ /// Returns this Matrix rotated about its current relative X axis. Mat4 is /// always column major and post multiplied from right to left ( L <- R ). /// Equiv to: /// returnMat := rotatedIdentity_by_relX * (this_Mat) //Postmultiplied ///------------------------------------------------------------------------ [[nodiscard]] Mat4T rotateAboutRelX( T const angleRads ) const noexcept; ///------------------------------------------------------------------------ /// Returns this Matrix rotated about its current relative Y axis. Mat4 is /// always column major and post multiplied from right to left ( L <- R ). /// Equiv to: /// returnMat := rotatedIdentity_by_relY * (this_Mat) //Postmultiplied ///------------------------------------------------------------------------ [[nodiscard]] Mat4T rotateAboutRelY( T const angleRads ) const noexcept; ///------------------------------------------------------------------------ /// Returns this Matrix rotated about its current relative Z axis. Mat4 is /// always column major and post multiplied from right to left ( L <- R ). /// Equiv to: /// returnMat := rotatedIdentity_by_relZ * (this_Mat) //Postmultiplied ///------------------------------------------------------------------------ [[nodiscard]] Mat4T rotateAboutRelZ( T const angleRads ) const noexcept; ///------------------------------------------------------------------------ /// A static method that always rotates the identity matrix about the in /// passed axis by the specified radian amount. This is identical to /// Mat4T::rotate when ::rotate is called on a matrix set to identity. /// This method is faster than ::rotate since no internal accumulation /// has to take place; ie, this requires one less matrix multiplication /// than rotate. /// Returns an identity reference frame rotated about axisNormalized by /// angleRad radians. ///------------------------------------------------------------------------ [[nodiscard]] static Mat4T rotateIdentityMat( const VectorT& axisNormalized, T const angleRads ) noexcept; ///------------------------------------------------------------------------ /// Post Multiplies this Matrix by m and returns the result. /// Result := thisMatrix * m (column-major using post multiplication). /// Returns: Mat4T: the product of this matrix and Matrix m. ///------------------------------------------------------------------------ [[nodiscard]] Mat4T operator*( Mat4T const& m ) const noexcept; ///------------------------------------------------------------------------ /// Scales each element in this Matrix by Scalar m and returns the result. /// Result := thisMatrix * m (column-major using post multiplication). /// Returns: Mat4T: the product of this matrix and Matrix m. ///------------------------------------------------------------------------ [[nodiscard]] Mat4T operator*( T const scalar ) const noexcept; ///------------------------------------------------------------------------ /// Divides each element in this Matrix by Scalar m and returns the result. /// Mat4 containing element-wise division by scalar. /// Returns: Mat4T: the product of this matrix and Matrix m. ///------------------------------------------------------------------------ [[nodiscard]] Mat4T operator/( T const scalar ) const noexcept; ///------------------------------------------------------------------------ /// Post Multiplies this Matrix by m and returns the result. /// thisMatrix := m * thisMatrix (column-major using post multiplication). /// Returns: Mat4T: the product of this Matrix m and this matrix. /// This enables rotations like: /// /// thisMat *= Mat4::rotateIdentityMat( {0,0,1}, 75.0f*Aftr::DEGtoRAD ); /// Will rotate thisMat about the global +Z axis 75 degrees. /// /// thisMat *= Mat4::rotateIdentityMat( m.getZ(), 75.0f*Aftr::DEGtoRAD ); /// Will rotate thisMat about its local +Z axis 75 degrees. ///------------------------------------------------------------------------ Mat4T& operator *=( const Mat4T& m ) noexcept; ///------------------------------------------------------------------------ /// Component wise addition of the two matrices. /// Result := thisMatrix + m /// Returns: Mat4T: the product of this matrix and Matrix m. ///------------------------------------------------------------------------ [[nodiscard]] Mat4T operator +(const Mat4T& m) const noexcept; ///------------------------------------------------------------------------ /// Component wise subtraction of the two matrices. /// Result := thisMatrix - m /// Returns: Mat4T: the difference between this matrix and Matrix m. ///------------------------------------------------------------------------ [[nodiscard]] Mat4T operator -( const Mat4T& m ) const noexcept; ///------------------------------------------------------------------------ /// Insertion operator enabling a Mat4T to be read into std::cout << myVec. ///------------------------------------------------------------------------ friend std::ostream& operator <<( std::ostream& out, const Mat4T& m ) { out << m.toString(); return out; } ///------------------------------------------------------------------------ ///Assignment operator assigns the in passed Mat4T's data equal to this ///Mat4T. ///m: the Matrix who's data will be used to fill this Mat4T. ///------------------------------------------------------------------------ Mat4T& operator =( const Mat4T& mat ) noexcept; ///------------------------------------------------------------------------ /// Returns true if each component falls within a specified epsilon /// to each other. /// PARAM const Mat4T& m: the Matrix who's values will be compared to /// the values of this Mat4T. /// RETURN: True if the X,Y,Z values lie within an epsilon to each other; /// false, otherwise. ///------------------------------------------------------------------------ bool operator ==( const Mat4T& m ) const noexcept; bool operator !=( const Mat4T& m ) const noexcept; T& operator[]( size_t i ) noexcept; T operator[]( size_t i ) const noexcept; T& at( size_t i ) noexcept; T at( size_t i ) const noexcept; T& at( int row, int col ) noexcept; T at( int row, int col ) const noexcept; bool isEqual( const Mat4T& m, T tolerance = static_cast( 0.00001 ) ) const noexcept; static bool is_T_Equal( T x, T y, T epsilon ) noexcept; ///------------------------------------------------------------------------ /// Returns true if each element in this 4x4 matrix is not infinite or not /// NaN. ///------------------------------------------------------------------------ bool isValidForEachComponent() const noexcept; ///------------------------------------------------------------------------ /// Returns the sum of the diagonal elements of the upper 3x3 matrix. This /// is also known as the trace of the matrix. This is elements /// [0] + [5] + [10], which is identical to elements /// at(0,0) + at(1,1) + at(2,2). /// | 0 4 8 12 | //single idx | 0,0 0,1 0,2 0,3 | //row,col idx /// | 1 5 9 13 | | 1,0 1,1 1,2 1,3 | /// | 2 6 10 14 | | 2,0 2,1 2,2 2,3 | /// | 3 7 11 15 | | 3,0 3,1 3,2 3,3 | ///------------------------------------------------------------------------ T trace3x3() const noexcept; ///------------------------------------------------------------------------ /// Returns the sum of the diagonal elements of the upper 3x3 matrix. This /// is also known as the trace of the matrix. This is elements /// [0] + [5] + [10] + [15], which is identical to elements /// at(0,0) + at(1,1) + at(2,2) + at(3,3). /// | 0 4 8 12 | //single idx | 0,0 0,1 0,2 0,3 | //row,col idx /// | 1 5 9 13 | | 1,0 1,1 1,2 1,3 | /// | 2 6 10 14 | | 2,0 2,1 2,2 2,3 | /// | 3 7 11 15 | | 3,0 3,1 3,2 3,3 | ///------------------------------------------------------------------------ T trace4x4() const noexcept; ///------------------------------------------------------------------------ /// Returns a string representation of the current Mat4T. /// Return: string: "(X, Y, Z)" representing the current Mat4T. ///------------------------------------------------------------------------ std::string toString() const; std::string toString( size_t length ) const; std::string toStringWithVecMagnitudes( size_t length ) const; ///------------------------------------------------------------------------ /// Follows exact same semantics as constructor with same input parameters. ///------------------------------------------------------------------------ static Mat4T fromString( const std::string& str, bool inputStringIsColumnMajor = true ) noexcept; static std::optional< Mat4T > fromString_opt( const std::string& str, bool inputStringIsColumnMajor = true ) noexcept; //Returns a double precision version of this single precision Mat4T. Mat4T< double > toMatD() const noexcept; Mat4T< float > toMatS() const noexcept; [[nodiscard]] VectorT getX() const noexcept; [[nodiscard]] VectorT getY() const noexcept; [[nodiscard]] VectorT getZ() const noexcept; [[nodiscard]] VectorT getPosition() const noexcept; ///< Returns the position column stored in this matrix. That is, the X,Y,Z values in the 4th column (elements 12,13,14) [[nodiscard]] VectorT getVecCol( int i ) const noexcept; ///< Returns the ith column vector in this 4x4 matrix (0=X, 1=Y, 2=Z, 3=Pos) [[nodiscard]] Mat4T getUpperLeft3x3AsMat4() const noexcept; void setX( const VectorT& xVec ) noexcept; void setY( const VectorT& yVec ) noexcept; void setZ( const VectorT& zVec ) noexcept; void setXYZ( const VectorT& xCol, const VectorT& yCol, const VectorT& zCol ) noexcept; void setPosition( const VectorT& transVec ) noexcept; ///< Sets the X,Y,Z values of the 4th column to in passed value, does not transform through upper 3x3 void setVecCol( int i, const VectorT& vec ) noexcept; ///< Sets the ith column vector in this 4x4 matrix (0=X, 1=Y, 2=Z, 3=Pos) void setScale( const VectorT& scaleVec ) noexcept; ///< Sets the Diagonal values (matrix's trace) of the first 3 columns to in passed value. It overwrites the x,y,z scales. void multScale(const VectorT& scaleVec) noexcept;///< Multiplies the diagonal values of the first 3 columns to in passed value. Effectively the operation of now depricated glScale*() ///------------------------------------------------------------------------ // Assuming the upper 3x3 of this MatT is a DCM representing an // orthogonal basis set spanning 3 dimensions, this method will normalize // each vector and ensure they are perpendicular to each other. This is // helpful when many accumulations occur on the same matrix and numerical // errors lead to de-orthogonalization. Interally, this method converts the // Upper 3x3 into a Quaternion, normalizes the quaternion, and then // converts the quaternion to a Mat4T. The 4th column (position) is // simply passed through to the return value -- it does not affect // orthogonalization. ///------------------------------------------------------------------------ [[nodiscard]] Mat4T orthogonalize_3x3() const noexcept; ///------------------------------------------------------------------------ // Assuming the upper 3x3 of this MatT is a skew symmetric matrix // representing a infintesimal rotation about an axis of rotation, this // method returns the corresponding rotation matrix. // The magnitude of the 3 values // defines the radian counter clockwise (right handed) rotation of the // resultant rotation matrix. The 4th row and column are ignored and the // returned rotation matrix has identity 4th row/col. // // | 0 -z y 0 | This is the expected form of the skew symmetric matrix. // | z 0 -x 0 | If the matrix does *not* have this form, this // | -y x 0 0 | function ***WILL NOT RETURN A VALID VALUE***. // | 0 0 0 1 | // // This skew symmetric matrix is *also* the derivative of a rotation matrix. // Thus exponentiating the skew symmetric matrix is the same as computing // its integral which results in a rotation matrix. // // This is the same as exponentiating a SO3 skew matrix matrix. which is the // same as taking the "derivating" // // This is the same a OpenCV's rodrigues ( cv::rodrigues ) function. // https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac ///------------------------------------------------------------------------ [[nodiscard]] Mat4T toRotationMat_fromSkewSymmetricMat() const noexcept; [[nodiscard]] Mat4T toSkewSymmetricMat_fromRotationMat() const noexcept; [[nodiscard]] Mat4T exponentiate_toSkewSymmetric_fromRotMat() const noexcept { return this->toSkewSymmetric_fromRotationMat(); } ///------------------------------------------------------------------------ // Returns true iff this is a valid skew symmetric (also known as // antisymmetric matrix). Specifically a skew symmetric corresponding to // the log of an SO(3) Rotation matrix. The exponentiation of a skew // symmetric matrix is an SO(3) Rotation Matrix (a 3D DCM) / Rotation // matrix. ///------------------------------------------------------------------------ [[nodiscard]] bool isSkewSymmetric() const noexcept; ///------------------------------------------------------------------------ // Assuming the upper 3x3 of this MatT is a proper ROTATION matrix, // this method returns the corresponding rotation vector as an axis with // length corresponding to the radian counter-clockwise rotation about // that axis. // // This is the same a OpenCV's rodrigues ( cv::rodrigues ) function. // https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac ///------------------------------------------------------------------------ [[nodiscard]] VectorT toRodriguesVec_fromSkewSymmetricMat() const noexcept; [[nodiscard]] VectorT toRodriguesVec_fromRotationMat() const noexcept; ///------------------------------------------------------------------------ // Assuming the upper 3x3 of this MatT is a proper ROTATION matrix, // this method returns the Radian angle this DCM is rotated about its // current axis of rotation. This is the same as getting the Rodrigues // vector and inspecting the length of the returned X,Y,Z. This method, // however, performs less work that creating a full rodrigues vector. // // Returns radian rotation distance about the single rotation axis // contained in this rotation matrix. // // https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac ///------------------------------------------------------------------------ [[nodiscard]] T getAngleOfRotationAboutAxisOfRotationRads() const noexcept; [[nodiscard]] VectorT getAxisOfRotation_fromRotationMat() const noexcept; ///------------------------------------------------------------------------ /// Assumes this is a ROTATION Matrix (NOT A SKEW SYMMETRIC Matrix). /// If this is called on a skew symmetric matrix, an improper result will /// be returned. /// Returns an Axis/Angle corresponding to the orientation defined within /// the current SO(3) rotation matrix (DCM, Display Matrix). The last /// column / bottom row of the 4x4 are ignored, only the upper 3x3 is /// used. ///------------------------------------------------------------------------ [[nodiscard]] AxisAngleT toAxisAngle_fromRotationMat() const noexcept; [[nodiscard]] AxisAngleT toAxisAngle_fromSkewSymmetricMat() const noexcept; ///------------------------------------------------------------------------ /// Assumes this is a ROTATION Matrix (NOT A SKEW SYMMETRIC Matrix). /// If this is called on a skew symmetric matrix, an improper result will /// be returned. /// Returns a Quaternion corresponding to the orientation defined within /// the current SO(3) rotation matrix (DCM, Display Matrix). The last /// column / bottom row of the 4x4 are ignored, only the upper 3x3 is /// used. ///------------------------------------------------------------------------ [[nodiscard]] QuatT toQuat_fromRotationMat() const noexcept; //sln do me [[nodiscard]] QuatT toQuat_fromSkewSymmetricMat() const noexcept; //sln do me ///------------------------------------------------------------------------ /// Returns the Euler angles represented by the column major X,Y,Z axes, /// respectively. Assumes the YAW rotation is clockwise (mapping to compass /// heading). /// YAW of 0 is parallel to +x axis, /// YAW of 90 is parallel to -y axis (EAST) /// /// The returned vector's .x -> roll, .y -> pitch, and .z -> yaw (clockwise) /// /// RETURN VALUE of each component of the VectorT: /// -YAW degrees(.z): Compass heading from due north. /// +90 is east (parallel to -Y axis). /// -90 / 270 is west (parallel to +Y Axis). /// +x is assumed to point due north. /// -PITCH degrees(.y): How far above horizon the nose is pointing. /// +30 deg is thirty degrees above the horizon. /// 0 deg is level and -24 would be twenty four degrees /// below horizon. /// -ROLL degrees(.x): How far clockwise have the wingtips rolled? /// 0 is flat, 180 is upside down. /// If flying due north w/ a pitch of zero (flying towards /// horizon), at 90 deg roll would have to cockpit's /// normal vector pointing east. /// /// The Givens order of the rotation is Z -> Y -> X. In an /// aircraft's frame that is YAW, PITCH, ROLL. HOWEVER!!! Should Yaw be /// mapped to the compass heading (clockwise / left handed) or should /// Yaw map to a right handed coordinate frame (counter-clockwise / /// right handed)? To clarify, two methods are provided. ///------------------------------------------------------------------------ [[nodiscard]] VectorD getRPY_deg_clockwise() const noexcept; ///------------------------------------------------------------------------ /// Returns the Euler angle rotations about the column major X,Y,Z axes, /// respectively. Assumes the YAW rotation is counter-clockwise /// YAW of 0 is parallel to +x axis, YAW of 90 is parallel to +y axis /// /// The returned vector's .x -> roll, .y -> pitch, and /// .z -> yaw (counter-clockwise) /// /// /// RETURN VALUE of each component of the VectorT: /// -YAW degrees(.z): Right handed counter clockwise rotation from +x axis. /// +90 is parallel to +Y axis (0,1,0). /// -90 / 270 is /// aligned w/ -Y axis (0,-1,0). /// -PITCH degrees(.y): How far above horizon the nose is pointing. /// +30 deg is thirty degrees above the horizon. /// 0 deg is level and -24 would be twenty four degrees /// below horizon. /// -ROLL degrees(.x): How far clockwise have the wingtips rolled? /// 0 is flat, 180 is upside down. /// If flying due north w/ a pitch of zero (flying towards /// horizon), at 90 deg roll would have to cockpit's /// normal vector pointing east. /// /// The Givens order of the rotation is Z -> Y -> X. In an /// aircraft's frame that is YAW, PITCH, ROLL. HOWEVER!!! Should Yaw be /// mapped to the compass heading (clockwise / left handed) or should /// Yaw map to a right handed coordinate frame (counter-clockwise / /// right handed)? To clarify, two methods are provided. Use /// counter_clockwise for right handed yaw. Use clockwise for left handed / /// compass heading yaw. ///------------------------------------------------------------------------ [[nodiscard]] VectorD getRPY_deg_counter_clockwise() const noexcept; ///------------------------------------------------------------------------ /// This methods takes as input a Vector who's X,Y, and Z components hold /// the roll, pitch, and yaw, respectively, expressed in degrees. This /// method sets the outDispMat to a 4x4 orthonormal display matrix /// matching that orientation.The outDispMat is a typical Mat4 DCM that /// specifies the orientation of a Model. /// /// So passing in VectorT{ 20, 45, 90 }. Would be a DCM that has it's /// +x column pointing due east (along global -Y). It is pitched up 45 /// degrees above the horizon (the global XY plane). And it is rolled /// 20 degrees rightward (ie, a flat wing like this - would tilt /// like this \ ( - -> \). /// /// The order of evaluation for these euler angles is yaw, then pitch, and /// finally roll. /// /// Returns the a DCM built by Euler angle rotation of Yaw (clockwise / /// compass heading), then Pitch (above horizon is positive, below is neg), /// then roll (clockwise roll is positive where left wing moves up and /// right wing moves down). /// /// Assumes the YAW rotation is clockwise (mapping to compass /// heading). BUT the INPUT Vector is x->roll, y->pitch, z->yaw. /// YAW of 0 is parallel to +x axis, /// YAW of 90 is parallel to -y axis (EAST) /// /// The returned Mat4 is a DCM that first applied the .z->yaw, then /// .y->pitch, and finally .x -> roll (clockwise / COMPASS heading). /// /// Internally, this simply invokes /// AftrCoordinateTransforms::transformFromRollPitchYawToDisplayMatrix( rpy ); /// /// The Givens order of the rotation is Z -> Y -> X. In an /// aircraft's frame that is YAW, PITCH, ROLL. HOWEVER!!! Should Yaw be /// mapped to the compass heading (clockwise / left handed) or should /// Yaw map to a right handed coordinate frame (counter-clockwise / /// right handed)? To clarify, two methods are provided. ///------------------------------------------------------------------------ [[nodiscard]] static Mat4T fromRPY_deg_clockwise( const VectorT& rpyDeg ) noexcept; ///------------------------------------------------------------------------ /// Swizzle creates a new Mat4 containing the same column vectors as the /// original Mat4, but moved to new column locations specified by the /// input parameter. For example passing calling: /// Mat4 m; m.swizzle_to( yzx ), will return a new mat4 with the following /// values: /// | 0 0 1 U | U denotes UNchanged from original. /// | 1 0 0 U | /// | 0 1 0 U | /// | U U U U | /// That is, /// The original y col vec is moved to the first column in the new mat. /// The original z col vec is moved to the second column in the new mat. /// The original x col vec is moved to the third column in the new mat. /// The last column and bottom row of the 4x4 are unchanged from the /// original mat4. /// So, using xyz returns a copy of the original. ///------------------------------------------------------------------------ [[nodiscard]] Mat4T swizzle_upper_3x3( Aftr::swizzle_to const swiz ) const noexcept; ///------------------------------------------------------------------------ ///Returns a pointer to an array of 16 values, corresponding to this Mat4 ///internal matrix. Column Major layout as follows: /// 0 4 8 12 x0 x1 x2 0 /// 1 5 9 13 ===> y0 y1 y2 0 /// 2 6 10 14 ===> z0 z1 z2 0 /// 3 7 11 15 0 0 0 1 /// /// Where Column 0 is the X vector, Column 1 is the Y vector, and /// Column 2 is the Z vector in this Mat4. ///------------------------------------------------------------------------ T* getPtr() noexcept; const T* getPtr() const noexcept; T* data() noexcept; const T* data() const noexcept; std::array m = { 1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1 }; //The 4x4 matrix constexpr T ZERO_THRESHOLD() const noexcept { return static_cast( 0.001 ); } using iterator = typename decltype(m)::iterator; using const_iterator = typename decltype(m)::const_iterator; iterator begin() noexcept { return this->m.begin(); } const_iterator cbegin() const noexcept { return this->m.begin(); } iterator end() noexcept { return this->m.end(); } const_iterator cend() const noexcept { return this->m.end(); } }; //template< typename T > //typename Mat4T::iterator begin( Mat4T& m ) { return m.begin(); } // //template< typename T > //typename Mat4T::const_iterator begin( const Mat4T& m ) { return m.begin(); } // //template< typename T > //typename Mat4T::const_iterator end( Mat4T& m ) { return m.end(); } // //template< typename T > //typename Mat4T::iterator end( const Mat4T& m ) { return m.end(); } // //template< typename T > //typename Mat4T::const_iterator cbegin( const Mat4T& m ) { return m.cbegin(); } // //template< typename T > //typename Mat4T::const_iterator cend( const Mat4T& m ) { return m.cend(); } using Mat4 = Mat4T< float >; using Mat4D = Mat4T< double >; } //namespace Aftr #include "Mat4.cpptemplate.h"