--- /dev/null
+// Created on: 2013-05-30
+// Created by: Anton POLETAEV
+// Copyright (c) 2013 OPEN CASCADE SAS
+//
+// The content of this file is subject to the Open CASCADE Technology Public
+// License Version 65 (the "License") You may not use the content of this file
+// except in compliance with the License Please obtain a copy of the License
+// at http://www.opencascade.org and read it completely before using this file
+//
+// The Initial Developer of the Original Code is Open CASCADE SAS, having its
+// main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France
+//
+// The Original Code and all software distributed under the License is
+// distributed on an "AS IS" basis, without warranty of any kind, and the
+// Initial Developer hereby disclaims all such warranties, including without
+// limitation, any warranties of merchantability, fitness for a particular
+// purpose or non-infringement Please see the License for the specific terms
+// and conditions governing the rights and limitations under the License
+//
+// The code was inspired by "sView" project by
+// Kirill GAVRILOV: https://github.com/gkv311/sview.
+// Files: StGLMatrix.hxx, StGLMatrix.cxx.
+// Copyright (c) 2010-2013 Kirill Gavrilov <kirill@sview.ru>
+
+#ifndef _NCollection_Mat4_HeaderFile
+#define _NCollection_Mat4_HeaderFile
+
+#include <NCollection_Vec4.hxx>
+
+//! Generic matrix of 4 x 4 elements.
+//! To be used in conjunction with NCollection_Vec4 entities.
+//! Originally introduced for 3D space projection and orientation
+//! operations.
+template<typename Element_t>
+class NCollection_Mat4
+{
+
+public:
+
+ //! Get number of rows.
+ //! @return number of rows.
+ static size_t Rows()
+ {
+ return 4;
+ }
+
+ //! Get number of columns.
+ //! @retur number of columns.
+ static size_t Cols()
+ {
+ return 4;
+ }
+
+ //! Empty constructor.
+ //! Construct the zero matrix.
+ NCollection_Mat4()
+ {
+ InitIdentity();
+ }
+
+ //! Copy constructor.
+ //! @param theOther [in] the matrix to copy values from.
+ NCollection_Mat4 (const NCollection_Mat4& theOther)
+ {
+ std::memcpy (this, &theOther, sizeof (NCollection_Mat4));
+ }
+
+ //! Assignment operator.
+ //! @param theOther [in] the matrix to copy values from.
+ const NCollection_Mat4& operator= (const NCollection_Mat4& theOther)
+ {
+ std::memcpy (this, &theOther, sizeof (NCollection_Mat4));
+ return *this;
+ }
+
+ //! Get element at the specified row and column.
+ //! @param theRow [in] the row.to address.
+ //! @param theCol [in] the column to address.
+ //! @return the value of the addressed element.
+ Element_t GetValue (const size_t theRow, const size_t theCol) const
+ {
+ return myMat[theCol * 4 + theRow];
+ }
+
+ //! Access element at the specified row and column.
+ //! @param theRow [in] the row.to access.
+ //! @param theCol [in] the column to access.
+ //! @return reference on the matrix element.
+ Element_t& ChangeValue (const size_t theRow, const size_t theCol)
+ {
+ return myMat[theCol * 4 + theRow];
+ }
+
+ //! Set value for the element specified by row and columns.
+ //! @param theRow [in] the row to change.
+ //! @param theCol [in] the column to change.
+ //! @param theValue [in] the value to set.s
+ void SetValue (const size_t theRow,
+ const size_t theCol,
+ const Element_t theValue)
+ {
+ myMat[theCol * 4 + theRow] = theValue;
+ }
+
+ //! Get vector of elements for the specified row.
+ //! @param theRow [in] the row to access.
+ //! @return vector of elements.
+ NCollection_Vec4<Element_t> GetRow (const size_t theRow) const
+ {
+ return NCollection_Vec4<Element_t> (GetValue (theRow, 0),
+ GetValue (theRow, 1),
+ GetValue (theRow, 2),
+ GetValue (theRow, 3));
+ }
+
+ //! Change first 3 row values by the passed vector.
+ //! @param theRow [in] the row to change.
+ //! @param theVec [in] the vector of values.
+ void SetRow (const size_t theRow, const NCollection_Vec3<Element_t>& theVec)
+ {
+ SetValue (theRow, 0, theVec.x());
+ SetValue (theRow, 1, theVec.y());
+ SetValue (theRow, 2, theVec.z());
+ }
+
+ //! Set row values by the passed 4 element vector.
+ //! @param theRow [in] the row to change.
+ //! @param theVec [in] the vector of values.
+ void SetRow (const size_t theRow, const NCollection_Vec4<Element_t>& theVec)
+ {
+ SetValue (theRow, 0, theVec.x());
+ SetValue (theRow, 1, theVec.y());
+ SetValue (theRow, 2, theVec.z());
+ SetValue (theRow, 3, theVec.w());
+ }
+
+ //! Get vector of elements for the specified column.
+ //! @param theCol [in] the column to access.
+ //! @return vector of elements.
+ NCollection_Vec4<Element_t> GetColumn (const size_t theCol) const
+ {
+ return NCollection_Vec4<Element_t> (GetValue (0, theCol),
+ GetValue (1, theCol),
+ GetValue (2, theCol),
+ GetValue (3, theCol));
+ }
+
+ //! Change first 3 column values by the passed vector.
+ //! @param theCol [in] the column to change.
+ //! @param theVec [in] the vector of values.
+ void SetColumn (const size_t theCol,
+ const NCollection_Vec3<Element_t>& theVec)
+ {
+ SetValue (0, theCol, theVec.x());
+ SetValue (1, theCol, theVec.y());
+ SetValue (2, theCol, theVec.z());
+ }
+
+ //! Set column values by the passed 4 element vector.
+ //! @param theCol [in] the column to change.
+ //! @param theVec [in] the vector of values.
+ void SetColumn (const size_t theCol,
+ const NCollection_Vec4<Element_t>& theVec)
+ {
+ SetValue (0, theCol, theVec.x());
+ SetValue (1, theCol, theVec.y());
+ SetValue (2, theCol, theVec.z());
+ SetValue (3, theCol, theVec.w());
+ }
+
+ //! Get vector of diagonal elements.
+ //! \return vector of diagonal elements.
+ NCollection_Vec4<Element_t> GetDiagonal() const
+ {
+ return NCollection_Vec4<Element_t> (GetValue (0, 0),
+ GetValue (1, 1),
+ GetValue (2, 2),
+ GetValue (3, 3));
+ }
+
+ //! Change first 3 elements of the diagonal matrix.
+ //! @param theVec the vector of values.
+ void SetDiagonal (const NCollection_Vec3<Element_t>& theVec)
+ {
+ SetValue (0, 0, theVec.x());
+ SetValue (1, 1, theVec.y());
+ SetValue (2, 2, theVec.z());
+ }
+
+ //! Set diagonal elements of the matrix by the passed vector.
+ //! @param theVec [in] the vector of values.
+ void SetDiagonal (const NCollection_Vec4<Element_t>& theVec)
+ {
+ SetValue (0, 0, theVec.x());
+ SetValue (1, 1, theVec.y());
+ SetValue (2, 2, theVec.z());
+ SetValue (3, 3, theVec.w());
+ }
+
+ //! Initialize the identity matrix.
+ void InitIdentity()
+ {
+ static const Element_t anIdentity[] =
+ {1, 0, 0, 0,
+ 0, 1, 0, 0,
+ 0, 0, 1, 0,
+ 0, 0, 0, 1};
+
+ std::memcpy (this, anIdentity, sizeof (NCollection_Mat4));
+ }
+
+ //! Raw access to the data (for OpenGL exchange).
+ const Element_t* GetData() const { return myMat; }
+ operator const Element_t*() const { return myMat; }
+ operator Element_t*() { return myMat; }
+
+ //! Multiply by the vector (M * V).
+ //! @param theVec [in] the vector to multiply.
+ NCollection_Vec4<Element_t> operator* (const NCollection_Vec4<Element_t>& theVec) const
+ {
+ return NCollection_Vec4<Element_t> (
+ GetValue (0, 0) * theVec.x() + GetValue (0, 1) * theVec.y() + GetValue (0, 2) * theVec.z() + GetValue (0, 3) * theVec.w(),
+ GetValue (1, 0) * theVec.x() + GetValue (1, 1) * theVec.y() + GetValue (1, 2) * theVec.z() + GetValue (1, 3) * theVec.w(),
+ GetValue (2, 0) * theVec.x() + GetValue (2, 1) * theVec.y() + GetValue (2, 2) * theVec.z() + GetValue (2, 3) * theVec.w(),
+ GetValue (3, 0) * theVec.x() + GetValue (3, 1) * theVec.y() + GetValue (3, 2) * theVec.z() + GetValue (3, 3) * theVec.w());
+ }
+
+ //! Compute matrix multiplication product: A * B.
+ //! @param theMatA [in] the matrix "A".
+ //! @param theMatB [in] the matrix "B".
+ NCollection_Mat4 Multiply (const NCollection_Mat4& theMatA,
+ const NCollection_Mat4& theMatB)
+ {
+ NCollection_Mat4 aMatRes;
+
+ size_t aInputElem;
+ for (size_t aResElem = 0; aResElem < 16; ++aResElem)
+ {
+ aMatRes[aResElem] = (Element_t )0;
+ for (aInputElem = 0; aInputElem < 4; ++aInputElem)
+ {
+ aMatRes[aResElem] += theMatA.GetValue(aResElem % 4, aInputElem)
+ * theMatB.GetValue(aInputElem, aResElem / 4);
+ }
+ }
+
+ return aMatRes;
+ }
+
+ //! Compute matrix multiplication.
+ //! @param theMat [in] the matrix to multiply.
+ void Multiply (const NCollection_Mat4& theMat)
+ {
+ *this = Multiply(*this, theMat);
+ }
+
+ //! Multiply by the another matrix.
+ //! @param theMat [in] the other matrix.
+ NCollection_Mat4& operator*= (const NCollection_Mat4& theMat)
+ {
+ Multiply (theMat);
+ return *this;
+ }
+
+ //! Compute matrix multiplication product.
+ //! @param theMat [in] the other matrix.
+ //! @return result of multiplication.
+ NCollection_Mat4 operator* (const NCollection_Mat4& theMat) const
+ {
+ return Mutiplied (theMat);
+ }
+
+ //! Compute matrix multiplication product.
+ //! @param theMat [in] the other matrix.
+ //! @return result of multiplication.
+ NCollection_Mat4 Multiplied (const NCollection_Mat4& theMat) const
+ {
+ NCollection_Mat4 aTempMat (*this);
+ aTempMat *= theMat;
+ return aTempMat;
+ }
+
+ //! Compute per-component multiplication.
+ //! @param theFactor [in] the scale factor.
+ void Multiply (const Element_t theFactor)
+ {
+ for (size_t i = 0; i < 16; ++i)
+ {
+ myMat[i] *= theFactor;
+ }
+ }
+
+ //! Compute per-element multiplication.
+ //! @param theFactor [in] the scale factor.
+ NCollection_Mat4& operator*=(const Element_t theFactor)
+ {
+ Multiply (theFactor);
+ return *this;
+ }
+
+ //! Compute per-element multiplication.
+ //! @param theFactor [in] the scale factor.
+ //! @return the result of multiplicaton.
+ NCollection_Mat4 operator* (const Element_t theFactor) const
+ {
+ return Multiplied (theFactor);
+ }
+
+ //! Compute per-element multiplication.
+ //! @param theFactor [in] the scale factor.
+ //! @return the result of multiplicaton.
+ NCollection_Mat4 Multiplied (const Element_t theFactor) const
+ {
+ NCollection_Mat4 aTempMat (*this);
+ aTempMat *= theFactor;
+ return aTempMat;
+ }
+
+ //! Translate the matrix on the passed vector.
+ //! @param theVec [in] the translation vector.
+ void Translate (const NCollection_Vec3<Element_t>& theVec)
+ {
+ NCollection_Mat4 aTempMat;
+ aTempMat.SetColumn (3, theVec);
+ this->Multiply (aTempMat);
+ }
+
+ //! Compute inverted matrix.
+ //! @param theOutMx [out] the inverted matrix.
+ //! @return true if reversion success.
+ bool Inverted (NCollection_Mat4<Element_t>& theOutMx) const
+ {
+ Element_t* inv = theOutMx.myMat;
+
+ // use short-cut for better readability
+ const Element_t* m = myMat;
+
+ inv[ 0] = m[ 5] * (m[10] * m[15] - m[11] * m[14]) -
+ m[ 9] * (m[ 6] * m[15] - m[ 7] * m[14]) -
+ m[13] * (m[ 7] * m[10] - m[ 6] * m[11]);
+
+ inv[ 1] = m[ 1] * (m[11] * m[14] - m[10] * m[15]) -
+ m[ 9] * (m[ 3] * m[14] - m[ 2] * m[15]) -
+ m[13] * (m[ 2] * m[11] - m[ 3] * m[10]);
+
+ inv[ 2] = m[ 1] * (m[ 6] * m[15] - m[ 7] * m[14]) -
+ m[ 5] * (m[ 2] * m[15] - m[ 3] * m[14]) -
+ m[13] * (m[ 3] * m[ 6] - m[ 2] * m[ 7]);
+
+ inv[ 3] = m[ 1] * (m[ 7] * m[10] - m[ 6] * m[11]) -
+ m[ 5] * (m[ 3] * m[10] - m[ 2] * m[11]) -
+ m[ 9] * (m[ 2] * m[ 7] - m[ 3] * m[ 6]);
+
+ inv[ 4] = m[ 4] * (m[11] * m[14] - m[10] * m[15]) -
+ m[ 8] * (m[ 7] * m[14] - m[ 6] * m[15]) -
+ m[12] * (m[ 6] * m[11] - m[ 7] * m[10]);
+
+ inv[ 5] = m[ 0] * (m[10] * m[15] - m[11] * m[14]) -
+ m[ 8] * (m[ 2] * m[15] - m[ 3] * m[14]) -
+ m[12] * (m[ 3] * m[10] - m[ 2] * m[11]);
+
+ inv[ 6] = m[ 0] * (m[ 7] * m[14] - m[ 6] * m[15]) -
+ m[ 4] * (m[ 3] * m[14] - m[ 2] * m[15]) -
+ m[12] * (m[ 2] * m[ 7] - m[ 3] * m[ 6]);
+
+ inv[ 7] = m[ 0] * (m[ 6] * m[11] - m[ 7] * m[10]) -
+ m[ 4] * (m[ 2] * m[11] - m[ 3] * m[10]) -
+ m[ 8] * (m[ 3] * m[ 6] - m[ 2] * m[ 7]);
+
+ inv[ 8] = m[ 4] * (m[ 9] * m[15] - m[11] * m[13]) -
+ m[ 8] * (m[ 5] * m[15] - m[ 7] * m[13]) -
+ m[12] * (m[ 7] * m[ 9] - m[ 5] * m[11]);
+
+ inv[ 9] = m[ 0] * (m[11] * m[13] - m[ 9] * m[15]) -
+ m[ 8] * (m[ 3] * m[13] - m[ 1] * m[15]) -
+ m[12] * (m[ 1] * m[11] - m[ 3] * m[ 9]);
+
+ inv[10] = m[ 0] * (m[ 5] * m[15] - m[ 7] * m[13]) -
+ m[ 4] * (m[ 1] * m[15] - m[ 3] * m[13]) -
+ m[12] * (m[ 3] * m[ 5] - m[ 1] * m[ 7]);
+
+ inv[11] = m[ 0] * (m[ 7] * m[ 9] - m[ 5] * m[11]) -
+ m[ 4] * (m[ 3] * m[ 9] - m[ 1] * m[11]) -
+ m[ 8] * (m[ 1] * m[ 7] - m[ 3] * m[ 5]);
+
+ inv[12] = m[ 4] * (m[10] * m[13] - m[ 9] * m[14]) -
+ m[ 8] * (m[ 6] * m[13] - m[ 5] * m[14]) -
+ m[12] * (m[ 5] * m[10] - m[ 6] * m[ 9]);
+
+ inv[13] = m[ 0] * (m[ 9] * m[14] - m[10] * m[13]) -
+ m[ 8] * (m[ 1] * m[14] - m[ 2] * m[13]) -
+ m[12] * (m[ 2] * m[ 9] - m[ 1] * m[10]);
+
+ inv[14] = m[ 0] * (m[ 6] * m[13] - m[ 5] * m[14]) -
+ m[ 4] * (m[ 2] * m[13] - m[ 1] * m[14]) -
+ m[12] * (m[ 1] * m[ 6] - m[ 2] * m[ 5]);
+
+ inv[15] = m[ 0] * (m[ 5] * m[10] - m[ 6] * m[ 9]) -
+ m[ 4] * (m[ 1] * m[10] - m[ 2] * m[ 9]) -
+ m[ 8] * (m[ 2] * m[ 5] - m[ 1] * m[ 6]);
+
+ Element_t aDet = m[0] * inv[ 0] +
+ m[1] * inv[ 4] +
+ m[2] * inv[ 8] +
+ m[3] * inv[12];
+
+ if (aDet == 0)
+ return false;
+
+ aDet = (Element_t) 1. / aDet;
+
+ for (int i = 0; i < 16; ++i)
+ inv[i] *= aDet;
+
+ return true;
+ }
+
+private:
+
+ Element_t myMat[16];
+
+};
+
+#endif // _NCollection_Matrix_H__
projects / occt.git / blobdiff