| /* |
| Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ |
| |
| This software is provided 'as-is', without any express or implied warranty. |
| In no event will the authors be held liable for any damages arising from the use of this software. |
| Permission is granted to anyone to use this software for any purpose, |
| including commercial applications, and to alter it and redistribute it freely, |
| subject to the following restrictions: |
| |
| 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
| 3. This notice may not be removed or altered from any source distribution. |
| */ |
| |
| |
| #ifndef btMatrix3x3_H |
| #define btMatrix3x3_H |
| |
| #include "btScalar.h" |
| |
| #include "btVector3.h" |
| #include "btQuaternion.h" |
| |
| |
| |
| /**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3. |
| * Make sure to only include a pure orthogonal matrix without scaling. */ |
| class btMatrix3x3 { |
| public: |
| /** @brief No initializaion constructor */ |
| btMatrix3x3 () {} |
| |
| // explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); } |
| |
| /**@brief Constructor from Quaternion */ |
| explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); } |
| /* |
| template <typename btScalar> |
| Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) |
| { |
| setEulerYPR(yaw, pitch, roll); |
| } |
| */ |
| /** @brief Constructor with row major formatting */ |
| btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz, |
| const btScalar& yx, const btScalar& yy, const btScalar& yz, |
| const btScalar& zx, const btScalar& zy, const btScalar& zz) |
| { |
| setValue(xx, xy, xz, |
| yx, yy, yz, |
| zx, zy, zz); |
| } |
| /** @brief Copy constructor */ |
| SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other) |
| { |
| m_el[0] = other.m_el[0]; |
| m_el[1] = other.m_el[1]; |
| m_el[2] = other.m_el[2]; |
| } |
| /** @brief Assignment Operator */ |
| SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other) |
| { |
| m_el[0] = other.m_el[0]; |
| m_el[1] = other.m_el[1]; |
| m_el[2] = other.m_el[2]; |
| return *this; |
| } |
| |
| /** @brief Get a column of the matrix as a vector |
| * @param i Column number 0 indexed */ |
| SIMD_FORCE_INLINE btVector3 getColumn(int i) const |
| { |
| return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]); |
| } |
| |
| |
| /** @brief Get a row of the matrix as a vector |
| * @param i Row number 0 indexed */ |
| SIMD_FORCE_INLINE const btVector3& getRow(int i) const |
| { |
| btFullAssert(0 <= i && i < 3); |
| return m_el[i]; |
| } |
| |
| /** @brief Get a mutable reference to a row of the matrix as a vector |
| * @param i Row number 0 indexed */ |
| SIMD_FORCE_INLINE btVector3& operator[](int i) |
| { |
| btFullAssert(0 <= i && i < 3); |
| return m_el[i]; |
| } |
| |
| /** @brief Get a const reference to a row of the matrix as a vector |
| * @param i Row number 0 indexed */ |
| SIMD_FORCE_INLINE const btVector3& operator[](int i) const |
| { |
| btFullAssert(0 <= i && i < 3); |
| return m_el[i]; |
| } |
| |
| /** @brief Multiply by the target matrix on the right |
| * @param m Rotation matrix to be applied |
| * Equivilant to this = this * m */ |
| btMatrix3x3& operator*=(const btMatrix3x3& m); |
| |
| /** @brief Set from a carray of btScalars |
| * @param m A pointer to the beginning of an array of 9 btScalars */ |
| void setFromOpenGLSubMatrix(const btScalar *m) |
| { |
| m_el[0].setValue(m[0],m[4],m[8]); |
| m_el[1].setValue(m[1],m[5],m[9]); |
| m_el[2].setValue(m[2],m[6],m[10]); |
| |
| } |
| /** @brief Set the values of the matrix explicitly (row major) |
| * @param xx Top left |
| * @param xy Top Middle |
| * @param xz Top Right |
| * @param yx Middle Left |
| * @param yy Middle Middle |
| * @param yz Middle Right |
| * @param zx Bottom Left |
| * @param zy Bottom Middle |
| * @param zz Bottom Right*/ |
| void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, |
| const btScalar& yx, const btScalar& yy, const btScalar& yz, |
| const btScalar& zx, const btScalar& zy, const btScalar& zz) |
| { |
| m_el[0].setValue(xx,xy,xz); |
| m_el[1].setValue(yx,yy,yz); |
| m_el[2].setValue(zx,zy,zz); |
| } |
| |
| /** @brief Set the matrix from a quaternion |
| * @param q The Quaternion to match */ |
| void setRotation(const btQuaternion& q) |
| { |
| btScalar d = q.length2(); |
| btFullAssert(d != btScalar(0.0)); |
| btScalar s = btScalar(2.0) / d; |
| btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s; |
| btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs; |
| btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs; |
| btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs; |
| setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy, |
| xy + wz, btScalar(1.0) - (xx + zz), yz - wx, |
| xz - wy, yz + wx, btScalar(1.0) - (xx + yy)); |
| } |
| |
| |
| /** @brief Set the matrix from euler angles using YPR around YXZ respectively |
| * @param yaw Yaw about Y axis |
| * @param pitch Pitch about X axis |
| * @param roll Roll about Z axis |
| */ |
| void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) |
| { |
| setEulerZYX(roll, pitch, yaw); |
| } |
| |
| /** @brief Set the matrix from euler angles YPR around ZYX axes |
| * @param eulerX Roll about X axis |
| * @param eulerY Pitch around Y axis |
| * @param eulerZ Yaw aboud Z axis |
| * |
| * These angles are used to produce a rotation matrix. The euler |
| * angles are applied in ZYX order. I.e a vector is first rotated |
| * about X then Y and then Z |
| **/ |
| void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) { |
| ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code |
| btScalar ci ( btCos(eulerX)); |
| btScalar cj ( btCos(eulerY)); |
| btScalar ch ( btCos(eulerZ)); |
| btScalar si ( btSin(eulerX)); |
| btScalar sj ( btSin(eulerY)); |
| btScalar sh ( btSin(eulerZ)); |
| btScalar cc = ci * ch; |
| btScalar cs = ci * sh; |
| btScalar sc = si * ch; |
| btScalar ss = si * sh; |
| |
| setValue(cj * ch, sj * sc - cs, sj * cc + ss, |
| cj * sh, sj * ss + cc, sj * cs - sc, |
| -sj, cj * si, cj * ci); |
| } |
| |
| /**@brief Set the matrix to the identity */ |
| void setIdentity() |
| { |
| setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), |
| btScalar(0.0), btScalar(1.0), btScalar(0.0), |
| btScalar(0.0), btScalar(0.0), btScalar(1.0)); |
| } |
| |
| static const btMatrix3x3& getIdentity() |
| { |
| static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0), |
| btScalar(0.0), btScalar(1.0), btScalar(0.0), |
| btScalar(0.0), btScalar(0.0), btScalar(1.0)); |
| return identityMatrix; |
| } |
| |
| /**@brief Fill the values of the matrix into a 9 element array |
| * @param m The array to be filled */ |
| void getOpenGLSubMatrix(btScalar *m) const |
| { |
| m[0] = btScalar(m_el[0].x()); |
| m[1] = btScalar(m_el[1].x()); |
| m[2] = btScalar(m_el[2].x()); |
| m[3] = btScalar(0.0); |
| m[4] = btScalar(m_el[0].y()); |
| m[5] = btScalar(m_el[1].y()); |
| m[6] = btScalar(m_el[2].y()); |
| m[7] = btScalar(0.0); |
| m[8] = btScalar(m_el[0].z()); |
| m[9] = btScalar(m_el[1].z()); |
| m[10] = btScalar(m_el[2].z()); |
| m[11] = btScalar(0.0); |
| } |
| |
| /**@brief Get the matrix represented as a quaternion |
| * @param q The quaternion which will be set */ |
| void getRotation(btQuaternion& q) const |
| { |
| btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); |
| btScalar temp[4]; |
| |
| if (trace > btScalar(0.0)) |
| { |
| btScalar s = btSqrt(trace + btScalar(1.0)); |
| temp[3]=(s * btScalar(0.5)); |
| s = btScalar(0.5) / s; |
| |
| temp[0]=((m_el[2].y() - m_el[1].z()) * s); |
| temp[1]=((m_el[0].z() - m_el[2].x()) * s); |
| temp[2]=((m_el[1].x() - m_el[0].y()) * s); |
| } |
| else |
| { |
| int i = m_el[0].x() < m_el[1].y() ? |
| (m_el[1].y() < m_el[2].z() ? 2 : 1) : |
| (m_el[0].x() < m_el[2].z() ? 2 : 0); |
| int j = (i + 1) % 3; |
| int k = (i + 2) % 3; |
| |
| btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0)); |
| temp[i] = s * btScalar(0.5); |
| s = btScalar(0.5) / s; |
| |
| temp[3] = (m_el[k][j] - m_el[j][k]) * s; |
| temp[j] = (m_el[j][i] + m_el[i][j]) * s; |
| temp[k] = (m_el[k][i] + m_el[i][k]) * s; |
| } |
| q.setValue(temp[0],temp[1],temp[2],temp[3]); |
| } |
| |
| /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR |
| * @param yaw Yaw around Y axis |
| * @param pitch Pitch around X axis |
| * @param roll around Z axis */ |
| void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const |
| { |
| |
| // first use the normal calculus |
| yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x())); |
| pitch = btScalar(btAsin(-m_el[2].x())); |
| roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z())); |
| |
| // on pitch = +/-HalfPI |
| if (btFabs(pitch)==SIMD_HALF_PI) |
| { |
| if (yaw>0) |
| yaw-=SIMD_PI; |
| else |
| yaw+=SIMD_PI; |
| |
| if (roll>0) |
| roll-=SIMD_PI; |
| else |
| roll+=SIMD_PI; |
| } |
| }; |
| |
| |
| /**@brief Get the matrix represented as euler angles around ZYX |
| * @param yaw Yaw around X axis |
| * @param pitch Pitch around Y axis |
| * @param roll around X axis |
| * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ |
| void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const |
| { |
| struct Euler{btScalar yaw, pitch, roll;}; |
| Euler euler_out; |
| Euler euler_out2; //second solution |
| //get the pointer to the raw data |
| |
| // Check that pitch is not at a singularity |
| if (btFabs(m_el[2].x()) >= 1) |
| { |
| euler_out.yaw = 0; |
| euler_out2.yaw = 0; |
| |
| // From difference of angles formula |
| btScalar delta = btAtan2(m_el[0].x(),m_el[0].z()); |
| if (m_el[2].x() > 0) //gimbal locked up |
| { |
| euler_out.pitch = SIMD_PI / btScalar(2.0); |
| euler_out2.pitch = SIMD_PI / btScalar(2.0); |
| euler_out.roll = euler_out.pitch + delta; |
| euler_out2.roll = euler_out.pitch + delta; |
| } |
| else // gimbal locked down |
| { |
| euler_out.pitch = -SIMD_PI / btScalar(2.0); |
| euler_out2.pitch = -SIMD_PI / btScalar(2.0); |
| euler_out.roll = -euler_out.pitch + delta; |
| euler_out2.roll = -euler_out.pitch + delta; |
| } |
| } |
| else |
| { |
| euler_out.pitch = - btAsin(m_el[2].x()); |
| euler_out2.pitch = SIMD_PI - euler_out.pitch; |
| |
| euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), |
| m_el[2].z()/btCos(euler_out.pitch)); |
| euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), |
| m_el[2].z()/btCos(euler_out2.pitch)); |
| |
| euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), |
| m_el[0].x()/btCos(euler_out.pitch)); |
| euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), |
| m_el[0].x()/btCos(euler_out2.pitch)); |
| } |
| |
| if (solution_number == 1) |
| { |
| yaw = euler_out.yaw; |
| pitch = euler_out.pitch; |
| roll = euler_out.roll; |
| } |
| else |
| { |
| yaw = euler_out2.yaw; |
| pitch = euler_out2.pitch; |
| roll = euler_out2.roll; |
| } |
| } |
| |
| /**@brief Create a scaled copy of the matrix |
| * @param s Scaling vector The elements of the vector will scale each column */ |
| |
| btMatrix3x3 scaled(const btVector3& s) const |
| { |
| return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(), |
| m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(), |
| m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z()); |
| } |
| |
| /**@brief Return the determinant of the matrix */ |
| btScalar determinant() const; |
| /**@brief Return the adjoint of the matrix */ |
| btMatrix3x3 adjoint() const; |
| /**@brief Return the matrix with all values non negative */ |
| btMatrix3x3 absolute() const; |
| /**@brief Return the transpose of the matrix */ |
| btMatrix3x3 transpose() const; |
| /**@brief Return the inverse of the matrix */ |
| btMatrix3x3 inverse() const; |
| |
| btMatrix3x3 transposeTimes(const btMatrix3x3& m) const; |
| btMatrix3x3 timesTranspose(const btMatrix3x3& m) const; |
| |
| SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const |
| { |
| return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z(); |
| } |
| SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const |
| { |
| return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z(); |
| } |
| SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const |
| { |
| return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); |
| } |
| |
| |
| /**@brief diagonalizes this matrix by the Jacobi method. |
| * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original |
| * coordinate system, i.e., old_this = rot * new_this * rot^T. |
| * @param threshold See iteration |
| * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied |
| * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. |
| * |
| * Note that this matrix is assumed to be symmetric. |
| */ |
| void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps) |
| { |
| rot.setIdentity(); |
| for (int step = maxSteps; step > 0; step--) |
| { |
| // find off-diagonal element [p][q] with largest magnitude |
| int p = 0; |
| int q = 1; |
| int r = 2; |
| btScalar max = btFabs(m_el[0][1]); |
| btScalar v = btFabs(m_el[0][2]); |
| if (v > max) |
| { |
| q = 2; |
| r = 1; |
| max = v; |
| } |
| v = btFabs(m_el[1][2]); |
| if (v > max) |
| { |
| p = 1; |
| q = 2; |
| r = 0; |
| max = v; |
| } |
| |
| btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2])); |
| if (max <= t) |
| { |
| if (max <= SIMD_EPSILON * t) |
| { |
| return; |
| } |
| step = 1; |
| } |
| |
| // compute Jacobi rotation J which leads to a zero for element [p][q] |
| btScalar mpq = m_el[p][q]; |
| btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq); |
| btScalar theta2 = theta * theta; |
| btScalar cos; |
| btScalar sin; |
| if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON)) |
| { |
| t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2)) |
| : 1 / (theta - btSqrt(1 + theta2)); |
| cos = 1 / btSqrt(1 + t * t); |
| sin = cos * t; |
| } |
| else |
| { |
| // approximation for large theta-value, i.e., a nearly diagonal matrix |
| t = 1 / (theta * (2 + btScalar(0.5) / theta2)); |
| cos = 1 - btScalar(0.5) * t * t; |
| sin = cos * t; |
| } |
| |
| // apply rotation to matrix (this = J^T * this * J) |
| m_el[p][q] = m_el[q][p] = 0; |
| m_el[p][p] -= t * mpq; |
| m_el[q][q] += t * mpq; |
| btScalar mrp = m_el[r][p]; |
| btScalar mrq = m_el[r][q]; |
| m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq; |
| m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp; |
| |
| // apply rotation to rot (rot = rot * J) |
| for (int i = 0; i < 3; i++) |
| { |
| btVector3& row = rot[i]; |
| mrp = row[p]; |
| mrq = row[q]; |
| row[p] = cos * mrp - sin * mrq; |
| row[q] = cos * mrq + sin * mrp; |
| } |
| } |
| } |
| |
| |
| |
| protected: |
| /**@brief Calculate the matrix cofactor |
| * @param r1 The first row to use for calculating the cofactor |
| * @param c1 The first column to use for calculating the cofactor |
| * @param r1 The second row to use for calculating the cofactor |
| * @param c1 The second column to use for calculating the cofactor |
| * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details |
| */ |
| btScalar cofac(int r1, int c1, int r2, int c2) const |
| { |
| return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; |
| } |
| ///Data storage for the matrix, each vector is a row of the matrix |
| btVector3 m_el[3]; |
| }; |
| |
| SIMD_FORCE_INLINE btMatrix3x3& |
| btMatrix3x3::operator*=(const btMatrix3x3& m) |
| { |
| setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]), |
| m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]), |
| m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2])); |
| return *this; |
| } |
| |
| SIMD_FORCE_INLINE btScalar |
| btMatrix3x3::determinant() const |
| { |
| return btTriple((*this)[0], (*this)[1], (*this)[2]); |
| } |
| |
| |
| SIMD_FORCE_INLINE btMatrix3x3 |
| btMatrix3x3::absolute() const |
| { |
| return btMatrix3x3( |
| btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()), |
| btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()), |
| btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z())); |
| } |
| |
| SIMD_FORCE_INLINE btMatrix3x3 |
| btMatrix3x3::transpose() const |
| { |
| return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(), |
| m_el[0].y(), m_el[1].y(), m_el[2].y(), |
| m_el[0].z(), m_el[1].z(), m_el[2].z()); |
| } |
| |
| SIMD_FORCE_INLINE btMatrix3x3 |
| btMatrix3x3::adjoint() const |
| { |
| return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2), |
| cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0), |
| cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1)); |
| } |
| |
| SIMD_FORCE_INLINE btMatrix3x3 |
| btMatrix3x3::inverse() const |
| { |
| btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); |
| btScalar det = (*this)[0].dot(co); |
| btFullAssert(det != btScalar(0.0)); |
| btScalar s = btScalar(1.0) / det; |
| return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, |
| co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, |
| co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); |
| } |
| |
| SIMD_FORCE_INLINE btMatrix3x3 |
| btMatrix3x3::transposeTimes(const btMatrix3x3& m) const |
| { |
| return btMatrix3x3( |
| m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(), |
| m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(), |
| m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(), |
| m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(), |
| m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(), |
| m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(), |
| m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(), |
| m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(), |
| m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z()); |
| } |
| |
| SIMD_FORCE_INLINE btMatrix3x3 |
| btMatrix3x3::timesTranspose(const btMatrix3x3& m) const |
| { |
| return btMatrix3x3( |
| m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]), |
| m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]), |
| m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2])); |
| |
| } |
| |
| SIMD_FORCE_INLINE btVector3 |
| operator*(const btMatrix3x3& m, const btVector3& v) |
| { |
| return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v)); |
| } |
| |
| |
| SIMD_FORCE_INLINE btVector3 |
| operator*(const btVector3& v, const btMatrix3x3& m) |
| { |
| return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v)); |
| } |
| |
| SIMD_FORCE_INLINE btMatrix3x3 |
| operator*(const btMatrix3x3& m1, const btMatrix3x3& m2) |
| { |
| return btMatrix3x3( |
| m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]), |
| m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]), |
| m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2])); |
| } |
| |
| /* |
| SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) { |
| return btMatrix3x3( |
| m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0], |
| m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1], |
| m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2], |
| m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0], |
| m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1], |
| m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2], |
| m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0], |
| m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1], |
| m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]); |
| } |
| */ |
| |
| /**@brief Equality operator between two matrices |
| * It will test all elements are equal. */ |
| SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2) |
| { |
| return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && |
| m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] && |
| m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] ); |
| } |
| |
| #endif |