| /* | |
| 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 |