/** * @file m4math.cpp * @brief LLMatrix4 class implementation. * * $LicenseInfo:firstyear=2000&license=viewerlgpl$ * Second Life Viewer Source Code * Copyright (C) 2010, Linden Research, Inc. * * This library is free software; you can redistribute it and/or * modify it under the terms of the GNU Lesser General Public * License as published by the Free Software Foundation; * version 2.1 of the License only. * * This library is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * Lesser General Public License for more details. * * You should have received a copy of the GNU Lesser General Public * License along with this library; if not, write to the Free Software * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA * * Linden Research, Inc., 945 Battery Street, San Francisco, CA 94111 USA * $/LicenseInfo$ */ #include "linden_common.h" #include "v3math.h" #include "v4math.h" #include "m4math.h" #include "m3math.h" #include "llquaternion.h" #include "llmatrix4a.h" // LLMatrix4 // Constructors LLMatrix4::LLMatrix4(const F32 *mat) { mMatrix[0][0] = mat[0]; mMatrix[0][1] = mat[1]; mMatrix[0][2] = mat[2]; mMatrix[0][3] = mat[3]; mMatrix[1][0] = mat[4]; mMatrix[1][1] = mat[5]; mMatrix[1][2] = mat[6]; mMatrix[1][3] = mat[7]; mMatrix[2][0] = mat[8]; mMatrix[2][1] = mat[9]; mMatrix[2][2] = mat[10]; mMatrix[2][3] = mat[11]; mMatrix[3][0] = mat[12]; mMatrix[3][1] = mat[13]; mMatrix[3][2] = mat[14]; mMatrix[3][3] = mat[15]; } LLMatrix4::LLMatrix4(const LLMatrix3 &mat, const LLVector4 &vec) { mMatrix[0][0] = mat.mMatrix[0][0]; mMatrix[0][1] = mat.mMatrix[0][1]; mMatrix[0][2] = mat.mMatrix[0][2]; mMatrix[0][3] = 0.f; mMatrix[1][0] = mat.mMatrix[1][0]; mMatrix[1][1] = mat.mMatrix[1][1]; mMatrix[1][2] = mat.mMatrix[1][2]; mMatrix[1][3] = 0.f; mMatrix[2][0] = mat.mMatrix[2][0]; mMatrix[2][1] = mat.mMatrix[2][1]; mMatrix[2][2] = mat.mMatrix[2][2]; mMatrix[2][3] = 0.f; mMatrix[3][0] = vec.mV[0]; mMatrix[3][1] = vec.mV[1]; mMatrix[3][2] = vec.mV[2]; mMatrix[3][3] = 1.f; } LLMatrix4::LLMatrix4(const LLMatrix3 &mat) { mMatrix[0][0] = mat.mMatrix[0][0]; mMatrix[0][1] = mat.mMatrix[0][1]; mMatrix[0][2] = mat.mMatrix[0][2]; mMatrix[0][3] = 0.f; mMatrix[1][0] = mat.mMatrix[1][0]; mMatrix[1][1] = mat.mMatrix[1][1]; mMatrix[1][2] = mat.mMatrix[1][2]; mMatrix[1][3] = 0.f; mMatrix[2][0] = mat.mMatrix[2][0]; mMatrix[2][1] = mat.mMatrix[2][1]; mMatrix[2][2] = mat.mMatrix[2][2]; mMatrix[2][3] = 0.f; mMatrix[3][0] = 0.f; mMatrix[3][1] = 0.f; mMatrix[3][2] = 0.f; mMatrix[3][3] = 1.f; } LLMatrix4::LLMatrix4(const LLQuaternion &q) { *this = initRotation(q); } LLMatrix4::LLMatrix4(const LLMatrix4a& mat) : LLMatrix4(mat.getF32ptr()) { } LLMatrix4::LLMatrix4(const LLQuaternion &q, const LLVector4 &pos) { *this = initRotTrans(q, pos); } LLMatrix4::LLMatrix4(const F32 angle, const LLVector4 &vec, const LLVector4 &pos) { initRotTrans(LLQuaternion(angle, vec), pos); } LLMatrix4::LLMatrix4(const F32 angle, const LLVector4 &vec) { initRotation(LLQuaternion(angle, vec)); mMatrix[3][0] = 0.f; mMatrix[3][1] = 0.f; mMatrix[3][2] = 0.f; mMatrix[3][3] = 1.f; } LLMatrix4::LLMatrix4(const F32 roll, const F32 pitch, const F32 yaw, const LLVector4 &pos) { LLMatrix3 mat(roll, pitch, yaw); initRotTrans(LLQuaternion(mat), pos); } LLMatrix4::LLMatrix4(const F32 roll, const F32 pitch, const F32 yaw) { LLMatrix3 mat(roll, pitch, yaw); initRotation(LLQuaternion(mat)); mMatrix[3][0] = 0.f; mMatrix[3][1] = 0.f; mMatrix[3][2] = 0.f; mMatrix[3][3] = 1.f; } LLMatrix4::~LLMatrix4(void) { } // Clear and Assignment Functions const LLMatrix4& LLMatrix4::setZero() { mMatrix[0][0] = 0.f; mMatrix[0][1] = 0.f; mMatrix[0][2] = 0.f; mMatrix[0][3] = 0.f; mMatrix[1][0] = 0.f; mMatrix[1][1] = 0.f; mMatrix[1][2] = 0.f; mMatrix[1][3] = 0.f; mMatrix[2][0] = 0.f; mMatrix[2][1] = 0.f; mMatrix[2][2] = 0.f; mMatrix[2][3] = 0.f; mMatrix[3][0] = 0.f; mMatrix[3][1] = 0.f; mMatrix[3][2] = 0.f; mMatrix[3][3] = 0.f; return *this; } // various useful mMatrix functions const LLMatrix4& LLMatrix4::transpose() { LLMatrix4 mat; mat.mMatrix[0][0] = mMatrix[0][0]; mat.mMatrix[1][0] = mMatrix[0][1]; mat.mMatrix[2][0] = mMatrix[0][2]; mat.mMatrix[3][0] = mMatrix[0][3]; mat.mMatrix[0][1] = mMatrix[1][0]; mat.mMatrix[1][1] = mMatrix[1][1]; mat.mMatrix[2][1] = mMatrix[1][2]; mat.mMatrix[3][1] = mMatrix[1][3]; mat.mMatrix[0][2] = mMatrix[2][0]; mat.mMatrix[1][2] = mMatrix[2][1]; mat.mMatrix[2][2] = mMatrix[2][2]; mat.mMatrix[3][2] = mMatrix[2][3]; mat.mMatrix[0][3] = mMatrix[3][0]; mat.mMatrix[1][3] = mMatrix[3][1]; mat.mMatrix[2][3] = mMatrix[3][2]; mat.mMatrix[3][3] = mMatrix[3][3]; *this = mat; return *this; } F32 LLMatrix4::determinant() const { F32 value = mMatrix[0][3] * mMatrix[1][2] * mMatrix[2][1] * mMatrix[3][0] - mMatrix[0][2] * mMatrix[1][3] * mMatrix[2][1] * mMatrix[3][0] - mMatrix[0][3] * mMatrix[1][1] * mMatrix[2][2] * mMatrix[3][0] + mMatrix[0][1] * mMatrix[1][3] * mMatrix[2][2] * mMatrix[3][0] + mMatrix[0][2] * mMatrix[1][1] * mMatrix[2][3] * mMatrix[3][0] - mMatrix[0][1] * mMatrix[1][2] * mMatrix[2][3] * mMatrix[3][0] - mMatrix[0][3] * mMatrix[1][2] * mMatrix[2][0] * mMatrix[3][1] + mMatrix[0][2] * mMatrix[1][3] * mMatrix[2][0] * mMatrix[3][1] + mMatrix[0][3] * mMatrix[1][0] * mMatrix[2][2] * mMatrix[3][1] - mMatrix[0][0] * mMatrix[1][3] * mMatrix[2][2] * mMatrix[3][1] - mMatrix[0][2] * mMatrix[1][0] * mMatrix[2][3] * mMatrix[3][1] + mMatrix[0][0] * mMatrix[1][2] * mMatrix[2][3] * mMatrix[3][1] + mMatrix[0][3] * mMatrix[1][1] * mMatrix[2][0] * mMatrix[3][2] - mMatrix[0][1] * mMatrix[1][3] * mMatrix[2][0] * mMatrix[3][2] - mMatrix[0][3] * mMatrix[1][0] * mMatrix[2][1] * mMatrix[3][2] + mMatrix[0][0] * mMatrix[1][3] * mMatrix[2][1] * mMatrix[3][2] + mMatrix[0][1] * mMatrix[1][0] * mMatrix[2][3] * mMatrix[3][2] - mMatrix[0][0] * mMatrix[1][1] * mMatrix[2][3] * mMatrix[3][2] - mMatrix[0][2] * mMatrix[1][1] * mMatrix[2][0] * mMatrix[3][3] + mMatrix[0][1] * mMatrix[1][2] * mMatrix[2][0] * mMatrix[3][3] + mMatrix[0][2] * mMatrix[1][0] * mMatrix[2][1] * mMatrix[3][3] - mMatrix[0][0] * mMatrix[1][2] * mMatrix[2][1] * mMatrix[3][3] - mMatrix[0][1] * mMatrix[1][0] * mMatrix[2][2] * mMatrix[3][3] + mMatrix[0][0] * mMatrix[1][1] * mMatrix[2][2] * mMatrix[3][3]; return value; } // Only works for pure orthonormal, homogeneous transform matrices. const LLMatrix4& LLMatrix4::invert(void) { // transpose the rotation part F32 temp; temp = mMatrix[VX][VY]; mMatrix[VX][VY] = mMatrix[VY][VX]; mMatrix[VY][VX] = temp; temp = mMatrix[VX][VZ]; mMatrix[VX][VZ] = mMatrix[VZ][VX]; mMatrix[VZ][VX] = temp; temp = mMatrix[VY][VZ]; mMatrix[VY][VZ] = mMatrix[VZ][VY]; mMatrix[VZ][VY] = temp; // rotate the translation part by the new rotation // (temporarily store in empty column of matrix) U32 j; for (j=0; j<3; j++) { mMatrix[j][VW] = mMatrix[VW][VX] * mMatrix[VX][j] + mMatrix[VW][VY] * mMatrix[VY][j] + mMatrix[VW][VZ] * mMatrix[VZ][j]; } // negate and copy the temporary vector back to the tranlation row mMatrix[VW][VX] = -mMatrix[VX][VW]; mMatrix[VW][VY] = -mMatrix[VY][VW]; mMatrix[VW][VZ] = -mMatrix[VZ][VW]; // zero the empty column again mMatrix[VX][VW] = mMatrix[VY][VW] = mMatrix[VZ][VW] = 0.0f; return *this; } // Convenience func for simplifying comparison-heavy code by // intentionally stomping values in [-FLT_EPS,FLT_EPS] to 0.0f // void LLMatrix4::condition(void) { U32 i; U32 j; for (i = 0; i < 3;i++) for (j = 0; j < 3;j++) mMatrix[i][j] = ((mMatrix[i][j] > -FLT_EPSILON) && (mMatrix[i][j] < FLT_EPSILON)) ? 0.0f : mMatrix[i][j]; } LLVector4 LLMatrix4::getFwdRow4() const { return LLVector4(mMatrix[VX][VX], mMatrix[VX][VY], mMatrix[VX][VZ], mMatrix[VX][VW]); } LLVector4 LLMatrix4::getLeftRow4() const { return LLVector4(mMatrix[VY][VX], mMatrix[VY][VY], mMatrix[VY][VZ], mMatrix[VY][VW]); } LLVector4 LLMatrix4::getUpRow4() const { return LLVector4(mMatrix[VZ][VX], mMatrix[VZ][VY], mMatrix[VZ][VZ], mMatrix[VZ][VW]); } // SJB: This code is correct for a logicly stored (non-transposed) matrix; // Our matrices are stored transposed, OpenGL style, so this generates the // INVERSE quaternion (-x, -y, -z, w)! // Because we use similar logic in LLQuaternion::getMatrix3, // we are internally consistant so everything works OK :) LLQuaternion LLMatrix4::quaternion() const { LLQuaternion quat; F32 tr, s, q[4]; U32 i, j, k; U32 nxt[3] = {1, 2, 0}; tr = mMatrix[0][0] + mMatrix[1][1] + mMatrix[2][2]; // check the diagonal if (tr > 0.f) { s = (F32)sqrt (tr + 1.f); quat.mQ[VS] = s / 2.f; s = 0.5f / s; quat.mQ[VX] = (mMatrix[1][2] - mMatrix[2][1]) * s; quat.mQ[VY] = (mMatrix[2][0] - mMatrix[0][2]) * s; quat.mQ[VZ] = (mMatrix[0][1] - mMatrix[1][0]) * s; } else { // diagonal is negative i = 0; if (mMatrix[1][1] > mMatrix[0][0]) i = 1; if (mMatrix[2][2] > mMatrix[i][i]) i = 2; j = nxt[i]; k = nxt[j]; s = (F32)sqrt ((mMatrix[i][i] - (mMatrix[j][j] + mMatrix[k][k])) + 1.f); q[i] = s * 0.5f; if (s != 0.f) s = 0.5f / s; q[3] = (mMatrix[j][k] - mMatrix[k][j]) * s; q[j] = (mMatrix[i][j] + mMatrix[j][i]) * s; q[k] = (mMatrix[i][k] + mMatrix[k][i]) * s; quat.setQuat(q); } return quat; } void LLMatrix4::initRows(const LLVector4 &row0, const LLVector4 &row1, const LLVector4 &row2, const LLVector4 &row3) { mMatrix[0][0] = row0.mV[0]; mMatrix[0][1] = row0.mV[1]; mMatrix[0][2] = row0.mV[2]; mMatrix[0][3] = row0.mV[3]; mMatrix[1][0] = row1.mV[0]; mMatrix[1][1] = row1.mV[1]; mMatrix[1][2] = row1.mV[2]; mMatrix[1][3] = row1.mV[3]; mMatrix[2][0] = row2.mV[0]; mMatrix[2][1] = row2.mV[1]; mMatrix[2][2] = row2.mV[2]; mMatrix[2][3] = row2.mV[3]; mMatrix[3][0] = row3.mV[0]; mMatrix[3][1] = row3.mV[1]; mMatrix[3][2] = row3.mV[2]; mMatrix[3][3] = row3.mV[3]; } const LLMatrix4& LLMatrix4::initRotation(F32 angle, const LLVector4 &vec) { LLMatrix3 mat(angle, vec); return initMatrix(mat); } const LLMatrix4& LLMatrix4::initRotation(const F32 roll, const F32 pitch, const F32 yaw) { LLMatrix3 mat(roll, pitch, yaw); return initMatrix(mat); } const LLMatrix4& LLMatrix4::initRotation(const LLQuaternion &q) { LLMatrix3 mat(q); return initMatrix(mat); } const LLMatrix4& LLMatrix4::initRotTrans(const F32 angle, const LLVector3 &axis, const LLVector3&translation) { LLMatrix3 mat(angle, axis); initMatrix(mat); setTranslation(translation); return (*this); } const LLMatrix4& LLMatrix4::initRotTrans(const F32 roll, const F32 pitch, const F32 yaw, const LLVector4 &translation) { LLMatrix3 mat(roll, pitch, yaw); initMatrix(mat); setTranslation(translation); return (*this); } /* const LLMatrix4& LLMatrix4::initRotTrans(const LLVector4 &fwd, const LLVector4 &left, const LLVector4 &up, const LLVector4 &translation) { LLMatrix3 mat(fwd, left, up); initMatrix(mat); setTranslation(translation); return (*this); } */ const LLMatrix4& LLMatrix4::initRotTrans(const LLQuaternion &q, const LLVector4 &translation) { LLMatrix3 mat(q); initMatrix(mat); setTranslation(translation); return (*this); } const LLMatrix4& LLMatrix4::initScale(const LLVector3 &scale) { setIdentity(); mMatrix[VX][VX] = scale.mV[VX]; mMatrix[VY][VY] = scale.mV[VY]; mMatrix[VZ][VZ] = scale.mV[VZ]; return (*this); } const LLMatrix4& LLMatrix4::initAll(const LLVector3 &scale, const LLQuaternion &q, const LLVector3 &pos) { F32 sx, sy, sz; F32 xx, xy, xz, xw, yy, yz, yw, zz, zw; sx = scale.mV[0]; sy = scale.mV[1]; sz = scale.mV[2]; xx = q.mQ[VX] * q.mQ[VX]; xy = q.mQ[VX] * q.mQ[VY]; xz = q.mQ[VX] * q.mQ[VZ]; xw = q.mQ[VX] * q.mQ[VW]; yy = q.mQ[VY] * q.mQ[VY]; yz = q.mQ[VY] * q.mQ[VZ]; yw = q.mQ[VY] * q.mQ[VW]; zz = q.mQ[VZ] * q.mQ[VZ]; zw = q.mQ[VZ] * q.mQ[VW]; mMatrix[0][0] = (1.f - 2.f * ( yy + zz )) *sx; mMatrix[0][1] = ( 2.f * ( xy + zw )) *sx; mMatrix[0][2] = ( 2.f * ( xz - yw )) *sx; mMatrix[1][0] = ( 2.f * ( xy - zw )) *sy; mMatrix[1][1] = (1.f - 2.f * ( xx + zz )) *sy; mMatrix[1][2] = ( 2.f * ( yz + xw )) *sy; mMatrix[2][0] = ( 2.f * ( xz + yw )) *sz; mMatrix[2][1] = ( 2.f * ( yz - xw )) *sz; mMatrix[2][2] = (1.f - 2.f * ( xx + yy )) *sz; mMatrix[3][0] = pos.mV[0]; mMatrix[3][1] = pos.mV[1]; mMatrix[3][2] = pos.mV[2]; mMatrix[3][3] = 1.0; // TODO -- should we set the translation portion to zero? return (*this); } const LLMatrix4& LLMatrix4::rotate(const F32 angle, const LLVector4 &vec) { LLMatrix4 mat(angle, vec); *this *= mat; return *this; } const LLMatrix4& LLMatrix4::rotate(const F32 roll, const F32 pitch, const F32 yaw) { LLMatrix4 mat(roll, pitch, yaw); *this *= mat; return *this; } const LLMatrix4& LLMatrix4::rotate(const LLQuaternion &q) { LLMatrix4 mat(q); *this *= mat; return *this; } const LLMatrix4& LLMatrix4::translate(const LLVector3 &vec) { mMatrix[3][0] += vec.mV[0]; mMatrix[3][1] += vec.mV[1]; mMatrix[3][2] += vec.mV[2]; return (*this); } void LLMatrix4::setFwdRow(const LLVector3 &row) { mMatrix[VX][VX] = row.mV[VX]; mMatrix[VX][VY] = row.mV[VY]; mMatrix[VX][VZ] = row.mV[VZ]; } void LLMatrix4::setLeftRow(const LLVector3 &row) { mMatrix[VY][VX] = row.mV[VX]; mMatrix[VY][VY] = row.mV[VY]; mMatrix[VY][VZ] = row.mV[VZ]; } void LLMatrix4::setUpRow(const LLVector3 &row) { mMatrix[VZ][VX] = row.mV[VX]; mMatrix[VZ][VY] = row.mV[VY]; mMatrix[VZ][VZ] = row.mV[VZ]; } void LLMatrix4::setFwdCol(const LLVector3 &col) { mMatrix[VX][VX] = col.mV[VX]; mMatrix[VY][VX] = col.mV[VY]; mMatrix[VZ][VX] = col.mV[VZ]; } void LLMatrix4::setLeftCol(const LLVector3 &col) { mMatrix[VX][VY] = col.mV[VX]; mMatrix[VY][VY] = col.mV[VY]; mMatrix[VZ][VY] = col.mV[VZ]; } void LLMatrix4::setUpCol(const LLVector3 &col) { mMatrix[VX][VZ] = col.mV[VX]; mMatrix[VY][VZ] = col.mV[VY]; mMatrix[VZ][VZ] = col.mV[VZ]; } const LLMatrix4& LLMatrix4::setTranslation(const F32 tx, const F32 ty, const F32 tz) { mMatrix[VW][VX] = tx; mMatrix[VW][VY] = ty; mMatrix[VW][VZ] = tz; return (*this); } const LLMatrix4& LLMatrix4::setTranslation(const LLVector3 &translation) { mMatrix[VW][VX] = translation.mV[VX]; mMatrix[VW][VY] = translation.mV[VY]; mMatrix[VW][VZ] = translation.mV[VZ]; return (*this); } const LLMatrix4& LLMatrix4::setTranslation(const LLVector4 &translation) { mMatrix[VW][VX] = translation.mV[VX]; mMatrix[VW][VY] = translation.mV[VY]; mMatrix[VW][VZ] = translation.mV[VZ]; return (*this); } // LLMatrix3 Extraction and Setting LLMatrix3 LLMatrix4::getMat3() const { LLMatrix3 retmat; retmat.mMatrix[0][0] = mMatrix[0][0]; retmat.mMatrix[0][1] = mMatrix[0][1]; retmat.mMatrix[0][2] = mMatrix[0][2]; retmat.mMatrix[1][0] = mMatrix[1][0]; retmat.mMatrix[1][1] = mMatrix[1][1]; retmat.mMatrix[1][2] = mMatrix[1][2]; retmat.mMatrix[2][0] = mMatrix[2][0]; retmat.mMatrix[2][1] = mMatrix[2][1]; retmat.mMatrix[2][2] = mMatrix[2][2]; return retmat; } const LLMatrix4& LLMatrix4::initMatrix(const LLMatrix3 &mat) { mMatrix[0][0] = mat.mMatrix[0][0]; mMatrix[0][1] = mat.mMatrix[0][1]; mMatrix[0][2] = mat.mMatrix[0][2]; mMatrix[0][3] = 0.f; mMatrix[1][0] = mat.mMatrix[1][0]; mMatrix[1][1] = mat.mMatrix[1][1]; mMatrix[1][2] = mat.mMatrix[1][2]; mMatrix[1][3] = 0.f; mMatrix[2][0] = mat.mMatrix[2][0]; mMatrix[2][1] = mat.mMatrix[2][1]; mMatrix[2][2] = mat.mMatrix[2][2]; mMatrix[2][3] = 0.f; mMatrix[3][0] = 0.f; mMatrix[3][1] = 0.f; mMatrix[3][2] = 0.f; mMatrix[3][3] = 1.f; return (*this); } const LLMatrix4& LLMatrix4::initMatrix(const LLMatrix3 &mat, const LLVector4 &translation) { mMatrix[0][0] = mat.mMatrix[0][0]; mMatrix[0][1] = mat.mMatrix[0][1]; mMatrix[0][2] = mat.mMatrix[0][2]; mMatrix[0][3] = 0.f; mMatrix[1][0] = mat.mMatrix[1][0]; mMatrix[1][1] = mat.mMatrix[1][1]; mMatrix[1][2] = mat.mMatrix[1][2]; mMatrix[1][3] = 0.f; mMatrix[2][0] = mat.mMatrix[2][0]; mMatrix[2][1] = mat.mMatrix[2][1]; mMatrix[2][2] = mat.mMatrix[2][2]; mMatrix[2][3] = 0.f; mMatrix[3][0] = translation.mV[0]; mMatrix[3][1] = translation.mV[1]; mMatrix[3][2] = translation.mV[2]; mMatrix[3][3] = 1.f; return (*this); } // LLMatrix4 Operators LLVector4 operator*(const LLVector4 &a, const LLMatrix4 &b) { // Operate "to the left" on row-vector a return LLVector4(a.mV[VX] * b.mMatrix[VX][VX] + a.mV[VY] * b.mMatrix[VY][VX] + a.mV[VZ] * b.mMatrix[VZ][VX] + a.mV[VW] * b.mMatrix[VW][VX], a.mV[VX] * b.mMatrix[VX][VY] + a.mV[VY] * b.mMatrix[VY][VY] + a.mV[VZ] * b.mMatrix[VZ][VY] + a.mV[VW] * b.mMatrix[VW][VY], a.mV[VX] * b.mMatrix[VX][VZ] + a.mV[VY] * b.mMatrix[VY][VZ] + a.mV[VZ] * b.mMatrix[VZ][VZ] + a.mV[VW] * b.mMatrix[VW][VZ], a.mV[VX] * b.mMatrix[VX][VW] + a.mV[VY] * b.mMatrix[VY][VW] + a.mV[VZ] * b.mMatrix[VZ][VW] + a.mV[VW] * b.mMatrix[VW][VW]); } LLVector4 rotate_vector(const LLVector4 &a, const LLMatrix4 &b) { // Rotates but does not translate // Operate "to the left" on row-vector a LLVector4 vec; vec.mV[VX] = a.mV[VX] * b.mMatrix[VX][VX] + a.mV[VY] * b.mMatrix[VY][VX] + a.mV[VZ] * b.mMatrix[VZ][VX]; vec.mV[VY] = a.mV[VX] * b.mMatrix[VX][VY] + a.mV[VY] * b.mMatrix[VY][VY] + a.mV[VZ] * b.mMatrix[VZ][VY]; vec.mV[VZ] = a.mV[VX] * b.mMatrix[VX][VZ] + a.mV[VY] * b.mMatrix[VY][VZ] + a.mV[VZ] * b.mMatrix[VZ][VZ]; // vec.mV[VW] = a.mV[VX] * b.mMatrix[VX][VW] + // a.mV[VY] * b.mMatrix[VY][VW] + // a.mV[VZ] * b.mMatrix[VZ][VW] + vec.mV[VW] = a.mV[VW]; return vec; } LLVector3 rotate_vector(const LLVector3 &a, const LLMatrix4 &b) { // Rotates but does not translate // Operate "to the left" on row-vector a LLVector3 vec; vec.mV[VX] = a.mV[VX] * b.mMatrix[VX][VX] + a.mV[VY] * b.mMatrix[VY][VX] + a.mV[VZ] * b.mMatrix[VZ][VX]; vec.mV[VY] = a.mV[VX] * b.mMatrix[VX][VY] + a.mV[VY] * b.mMatrix[VY][VY] + a.mV[VZ] * b.mMatrix[VZ][VY]; vec.mV[VZ] = a.mV[VX] * b.mMatrix[VX][VZ] + a.mV[VY] * b.mMatrix[VY][VZ] + a.mV[VZ] * b.mMatrix[VZ][VZ]; return vec; } bool operator==(const LLMatrix4 &a, const LLMatrix4 &b) { U32 i, j; for (i = 0; i < NUM_VALUES_IN_MAT4; i++) { for (j = 0; j < NUM_VALUES_IN_MAT4; j++) { if (a.mMatrix[j][i] != b.mMatrix[j][i]) return false; } } return true; } bool operator!=(const LLMatrix4 &a, const LLMatrix4 &b) { U32 i, j; for (i = 0; i < NUM_VALUES_IN_MAT4; i++) { for (j = 0; j < NUM_VALUES_IN_MAT4; j++) { if (a.mMatrix[j][i] != b.mMatrix[j][i]) return true; } } return false; } bool operator<(const LLMatrix4& a, const LLMatrix4 &b) { U32 i, j; for (i = 0; i < NUM_VALUES_IN_MAT4; i++) { for (j = 0; j < NUM_VALUES_IN_MAT4; j++) { if (a.mMatrix[i][j] != b.mMatrix[i][j]) { return a.mMatrix[i][j] < b.mMatrix[i][j]; } } } return false; } const LLMatrix4& operator*=(LLMatrix4 &a, F32 k) { U32 i, j; for (i = 0; i < NUM_VALUES_IN_MAT4; i++) { for (j = 0; j < NUM_VALUES_IN_MAT4; j++) { a.mMatrix[j][i] *= k; } } return a; } std::ostream& operator<<(std::ostream& s, const LLMatrix4 &a) { s << "{ " << a.mMatrix[VX][VX] << ", " << a.mMatrix[VX][VY] << ", " << a.mMatrix[VX][VZ] << ", " << a.mMatrix[VX][VW] << "; " << a.mMatrix[VY][VX] << ", " << a.mMatrix[VY][VY] << ", " << a.mMatrix[VY][VZ] << ", " << a.mMatrix[VY][VW] << "; " << a.mMatrix[VZ][VX] << ", " << a.mMatrix[VZ][VY] << ", " << a.mMatrix[VZ][VZ] << ", " << a.mMatrix[VZ][VW] << "; " << a.mMatrix[VW][VX] << ", " << a.mMatrix[VW][VY] << ", " << a.mMatrix[VW][VZ] << ", " << a.mMatrix[VW][VW] << " }"; return s; } LLSD LLMatrix4::getValue() const { LLSD ret; ret[0] = mMatrix[0][0]; ret[1] = mMatrix[0][1]; ret[2] = mMatrix[0][2]; ret[3] = mMatrix[0][3]; ret[4] = mMatrix[1][0]; ret[5] = mMatrix[1][1]; ret[6] = mMatrix[1][2]; ret[7] = mMatrix[1][3]; ret[8] = mMatrix[2][0]; ret[9] = mMatrix[2][1]; ret[10] = mMatrix[2][2]; ret[11] = mMatrix[2][3]; ret[12] = mMatrix[3][0]; ret[13] = mMatrix[3][1]; ret[14] = mMatrix[3][2]; ret[15] = mMatrix[3][3]; return ret; } void LLMatrix4::setValue(const LLSD& data) { mMatrix[0][0] = (F32)data[0].asReal(); mMatrix[0][1] = (F32)data[1].asReal(); mMatrix[0][2] = (F32)data[2].asReal(); mMatrix[0][3] = (F32)data[3].asReal(); mMatrix[1][0] = (F32)data[4].asReal(); mMatrix[1][1] = (F32)data[5].asReal(); mMatrix[1][2] = (F32)data[6].asReal(); mMatrix[1][3] = (F32)data[7].asReal(); mMatrix[2][0] = (F32)data[8].asReal(); mMatrix[2][1] = (F32)data[9].asReal(); mMatrix[2][2] = (F32)data[10].asReal(); mMatrix[2][3] = (F32)data[11].asReal(); mMatrix[3][0] = (F32)data[12].asReal(); mMatrix[3][1] = (F32)data[13].asReal(); mMatrix[3][2] = (F32)data[14].asReal(); mMatrix[3][3] = (F32)data[15].asReal(); }