From c3e6f7b1643076df35a6960f735024078ddbb353 Mon Sep 17 00:00:00 2001 From: Ansariel Date: Mon, 19 Feb 2024 21:33:38 +0100 Subject: Convert remaining cases of BOOL to bool in llmath and llprimitive Changed the return values for LLPrimitive::packTEMessage methods from FALSE to true - these seemed to be strange and wrong, especially considering the following statement in LLVOAvatarSelf: bool success = packTEMessage(mesgsys); --- indra/llmath/m3math.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'indra/llmath/m3math.cpp') diff --git a/indra/llmath/m3math.cpp b/indra/llmath/m3math.cpp index 65eb3348de..c7777164c0 100644 --- a/indra/llmath/m3math.cpp +++ b/indra/llmath/m3math.cpp @@ -528,10 +528,10 @@ bool operator==(const LLMatrix3 &a, const LLMatrix3 &b) for (j = 0; j < NUM_VALUES_IN_MAT3; j++) { if (a.mMatrix[j][i] != b.mMatrix[j][i]) - return FALSE; + return false; } } - return TRUE; + return true; } bool operator!=(const LLMatrix3 &a, const LLMatrix3 &b) @@ -542,10 +542,10 @@ bool operator!=(const LLMatrix3 &a, const LLMatrix3 &b) for (j = 0; j < NUM_VALUES_IN_MAT3; j++) { if (a.mMatrix[j][i] != b.mMatrix[j][i]) - return TRUE; + return true; } } - return FALSE; + return false; } const LLMatrix3& operator*=(LLMatrix3 &a, const LLMatrix3 &b) -- cgit v1.2.3 From e2e37cced861b98de8c1a7c9c0d3a50d2d90e433 Mon Sep 17 00:00:00 2001 From: Ansariel Date: Wed, 22 May 2024 21:25:21 +0200 Subject: Fix line endlings --- indra/llmath/m3math.cpp | 1180 +++++++++++++++++++++++------------------------ 1 file changed, 590 insertions(+), 590 deletions(-) (limited to 'indra/llmath/m3math.cpp') diff --git a/indra/llmath/m3math.cpp b/indra/llmath/m3math.cpp index e48c47d1ef..472d340af5 100644 --- a/indra/llmath/m3math.cpp +++ b/indra/llmath/m3math.cpp @@ -1,590 +1,590 @@ -/** - * @file m3math.cpp - * @brief LLMatrix3 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 "vmath.h" -#include "v3math.h" -#include "v3dmath.h" -#include "v4math.h" -#include "m4math.h" -#include "m3math.h" -#include "llquaternion.h" - -// LLMatrix3 - -// ji -// LLMatrix3 = |00 01 02 | -// |10 11 12 | -// |20 21 22 | - -// LLMatrix3 = |fx fy fz | forward-axis -// |lx ly lz | left-axis -// |ux uy uz | up-axis - - -// Constructors - - -LLMatrix3::LLMatrix3(const LLQuaternion &q) -{ - setRot(q); -} - - -LLMatrix3::LLMatrix3(const F32 angle, const LLVector3 &vec) -{ - LLQuaternion quat(angle, vec); - setRot(quat); -} - -LLMatrix3::LLMatrix3(const F32 angle, const LLVector3d &vec) -{ - LLVector3 vec_f; - vec_f.setVec(vec); - LLQuaternion quat(angle, vec_f); - setRot(quat); -} - -LLMatrix3::LLMatrix3(const F32 angle, const LLVector4 &vec) -{ - LLQuaternion quat(angle, vec); - setRot(quat); -} - -LLMatrix3::LLMatrix3(const F32 roll, const F32 pitch, const F32 yaw) -{ - setRot(roll,pitch,yaw); -} - -// From Matrix and Quaternion FAQ -void LLMatrix3::getEulerAngles(F32 *roll, F32 *pitch, F32 *yaw) const -{ - F64 angle_x, angle_y, angle_z; - F64 cx, cy, cz; // cosine of angle_x, angle_y, angle_z - F64 sx, sz; // sine of angle_x, angle_y, angle_z - - angle_y = asin(llclamp(mMatrix[2][0], -1.f, 1.f)); - cy = cos(angle_y); - - if (fabs(cy) > 0.005) // non-zero - { - // no gimbal lock - cx = mMatrix[2][2] / cy; - sx = - mMatrix[2][1] / cy; - - angle_x = (F32) atan2(sx, cx); - - cz = mMatrix[0][0] / cy; - sz = - mMatrix[1][0] / cy; - - angle_z = (F32) atan2(sz, cz); - } - else - { - // yup, gimbal lock - angle_x = 0; - - // some tricky math thereby avoided, see article - - cz = mMatrix[1][1]; - sz = mMatrix[0][1]; - - angle_z = atan2(sz, cz); - } - - *roll = (F32)angle_x; - *pitch = (F32)angle_y; - *yaw = (F32)angle_z; -} - - -// Clear and Assignment Functions - -const LLMatrix3& LLMatrix3::setIdentity() -{ - mMatrix[0][0] = 1.f; - mMatrix[0][1] = 0.f; - mMatrix[0][2] = 0.f; - - mMatrix[1][0] = 0.f; - mMatrix[1][1] = 1.f; - mMatrix[1][2] = 0.f; - - mMatrix[2][0] = 0.f; - mMatrix[2][1] = 0.f; - mMatrix[2][2] = 1.f; - return (*this); -} - -const LLMatrix3& LLMatrix3::clear() -{ - mMatrix[0][0] = 0.f; - mMatrix[0][1] = 0.f; - mMatrix[0][2] = 0.f; - - mMatrix[1][0] = 0.f; - mMatrix[1][1] = 0.f; - mMatrix[1][2] = 0.f; - - mMatrix[2][0] = 0.f; - mMatrix[2][1] = 0.f; - mMatrix[2][2] = 0.f; - return (*this); -} - -const LLMatrix3& LLMatrix3::setZero() -{ - mMatrix[0][0] = 0.f; - mMatrix[0][1] = 0.f; - mMatrix[0][2] = 0.f; - - mMatrix[1][0] = 0.f; - mMatrix[1][1] = 0.f; - mMatrix[1][2] = 0.f; - - mMatrix[2][0] = 0.f; - mMatrix[2][1] = 0.f; - mMatrix[2][2] = 0.f; - return (*this); -} - -// various useful mMatrix functions - -const LLMatrix3& LLMatrix3::transpose() -{ - // transpose the matrix - 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; - return *this; -} - - -F32 LLMatrix3::determinant() const -{ - // Is this a useful method when we assume the matrices are valid rotation - // matrices throughout this implementation? - return mMatrix[0][0] * (mMatrix[1][1] * mMatrix[2][2] - mMatrix[1][2] * mMatrix[2][1]) + - mMatrix[0][1] * (mMatrix[1][2] * mMatrix[2][0] - mMatrix[1][0] * mMatrix[2][2]) + - mMatrix[0][2] * (mMatrix[1][0] * mMatrix[2][1] - mMatrix[1][1] * mMatrix[2][0]); -} - -// inverts this matrix -void LLMatrix3::invert() -{ - // fails silently if determinant is zero too small - F32 det = determinant(); - const F32 VERY_SMALL_DETERMINANT = 0.000001f; - if (fabs(det) > VERY_SMALL_DETERMINANT) - { - // invertiable - LLMatrix3 t(*this); - mMatrix[VX][VX] = ( t.mMatrix[VY][VY] * t.mMatrix[VZ][VZ] - t.mMatrix[VY][VZ] * t.mMatrix[VZ][VY] ) / det; - mMatrix[VY][VX] = ( t.mMatrix[VY][VZ] * t.mMatrix[VZ][VX] - t.mMatrix[VY][VX] * t.mMatrix[VZ][VZ] ) / det; - mMatrix[VZ][VX] = ( t.mMatrix[VY][VX] * t.mMatrix[VZ][VY] - t.mMatrix[VY][VY] * t.mMatrix[VZ][VX] ) / det; - mMatrix[VX][VY] = ( t.mMatrix[VZ][VY] * t.mMatrix[VX][VZ] - t.mMatrix[VZ][VZ] * t.mMatrix[VX][VY] ) / det; - mMatrix[VY][VY] = ( t.mMatrix[VZ][VZ] * t.mMatrix[VX][VX] - t.mMatrix[VZ][VX] * t.mMatrix[VX][VZ] ) / det; - mMatrix[VZ][VY] = ( t.mMatrix[VZ][VX] * t.mMatrix[VX][VY] - t.mMatrix[VZ][VY] * t.mMatrix[VX][VX] ) / det; - mMatrix[VX][VZ] = ( t.mMatrix[VX][VY] * t.mMatrix[VY][VZ] - t.mMatrix[VX][VZ] * t.mMatrix[VY][VY] ) / det; - mMatrix[VY][VZ] = ( t.mMatrix[VX][VZ] * t.mMatrix[VY][VX] - t.mMatrix[VX][VX] * t.mMatrix[VY][VZ] ) / det; - mMatrix[VZ][VZ] = ( t.mMatrix[VX][VX] * t.mMatrix[VY][VY] - t.mMatrix[VX][VY] * t.mMatrix[VY][VX] ) / det; - } -} - -// does not assume a rotation matrix, and does not divide by determinant, assuming results will be renormalized -const LLMatrix3& LLMatrix3::adjointTranspose() -{ - LLMatrix3 adjoint_transpose; - adjoint_transpose.mMatrix[VX][VX] = mMatrix[VY][VY] * mMatrix[VZ][VZ] - mMatrix[VY][VZ] * mMatrix[VZ][VY] ; - adjoint_transpose.mMatrix[VY][VX] = mMatrix[VY][VZ] * mMatrix[VZ][VX] - mMatrix[VY][VX] * mMatrix[VZ][VZ] ; - adjoint_transpose.mMatrix[VZ][VX] = mMatrix[VY][VX] * mMatrix[VZ][VY] - mMatrix[VY][VY] * mMatrix[VZ][VX] ; - adjoint_transpose.mMatrix[VX][VY] = mMatrix[VZ][VY] * mMatrix[VX][VZ] - mMatrix[VZ][VZ] * mMatrix[VX][VY] ; - adjoint_transpose.mMatrix[VY][VY] = mMatrix[VZ][VZ] * mMatrix[VX][VX] - mMatrix[VZ][VX] * mMatrix[VX][VZ] ; - adjoint_transpose.mMatrix[VZ][VY] = mMatrix[VZ][VX] * mMatrix[VX][VY] - mMatrix[VZ][VY] * mMatrix[VX][VX] ; - adjoint_transpose.mMatrix[VX][VZ] = mMatrix[VX][VY] * mMatrix[VY][VZ] - mMatrix[VX][VZ] * mMatrix[VY][VY] ; - adjoint_transpose.mMatrix[VY][VZ] = mMatrix[VX][VZ] * mMatrix[VY][VX] - mMatrix[VX][VX] * mMatrix[VY][VZ] ; - adjoint_transpose.mMatrix[VZ][VZ] = mMatrix[VX][VX] * mMatrix[VY][VY] - mMatrix[VX][VY] * mMatrix[VY][VX] ; - - *this = adjoint_transpose; - return *this; -} - -// 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 LLMatrix3::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; -} - -const LLMatrix3& LLMatrix3::setRot(const F32 angle, const LLVector3 &vec) -{ - setRot(LLQuaternion(angle, vec)); - return *this; -} - -const LLMatrix3& LLMatrix3::setRot(const F32 roll, const F32 pitch, const F32 yaw) -{ - // Rotates RH about x-axis by 'roll' then - // rotates RH about the old y-axis by 'pitch' then - // rotates RH about the original z-axis by 'yaw'. - // . - // /|\ yaw axis - // | __. - // ._ ___| /| pitch axis - // _||\ \\ |-. / - // \|| \_______\_|__\_/_______ - // | _ _ o o o_o_o_o o /_\_ ________\ roll axis - // // /_______/ /__________> / - // /_,-' // / - // /__,-' - - F32 cx, sx, cy, sy, cz, sz; - F32 cxsy, sxsy; - - cx = (F32)cos(roll); //A - sx = (F32)sin(roll); //B - cy = (F32)cos(pitch); //C - sy = (F32)sin(pitch); //D - cz = (F32)cos(yaw); //E - sz = (F32)sin(yaw); //F - - cxsy = cx * sy; //AD - sxsy = sx * sy; //BD - - mMatrix[0][0] = cy * cz; - mMatrix[1][0] = -cy * sz; - mMatrix[2][0] = sy; - mMatrix[0][1] = sxsy * cz + cx * sz; - mMatrix[1][1] = -sxsy * sz + cx * cz; - mMatrix[2][1] = -sx * cy; - mMatrix[0][2] = -cxsy * cz + sx * sz; - mMatrix[1][2] = cxsy * sz + sx * cz; - mMatrix[2][2] = cx * cy; - return *this; -} - - -const LLMatrix3& LLMatrix3::setRot(const LLQuaternion &q) -{ - *this = q.getMatrix3(); - return *this; -} - -const LLMatrix3& LLMatrix3::setRows(const LLVector3 &fwd, const LLVector3 &left, const LLVector3 &up) -{ - mMatrix[0][0] = fwd.mV[0]; - mMatrix[0][1] = fwd.mV[1]; - mMatrix[0][2] = fwd.mV[2]; - - mMatrix[1][0] = left.mV[0]; - mMatrix[1][1] = left.mV[1]; - mMatrix[1][2] = left.mV[2]; - - mMatrix[2][0] = up.mV[0]; - mMatrix[2][1] = up.mV[1]; - mMatrix[2][2] = up.mV[2]; - - return *this; -} - -const LLMatrix3& LLMatrix3::setRow( U32 rowIndex, const LLVector3& row ) -{ - llassert( rowIndex >= 0 && rowIndex < NUM_VALUES_IN_MAT3 ); - - mMatrix[rowIndex][0] = row[0]; - mMatrix[rowIndex][1] = row[1]; - mMatrix[rowIndex][2] = row[2]; - - return *this; -} - -const LLMatrix3& LLMatrix3::setCol( U32 colIndex, const LLVector3& col ) -{ - llassert( colIndex >= 0 && colIndex < NUM_VALUES_IN_MAT3 ); - - mMatrix[0][colIndex] = col[0]; - mMatrix[1][colIndex] = col[1]; - mMatrix[2][colIndex] = col[2]; - - return *this; -} - -const LLMatrix3& LLMatrix3::rotate(const F32 angle, const LLVector3 &vec) -{ - LLMatrix3 mat(angle, vec); - *this *= mat; - return *this; -} - - -const LLMatrix3& LLMatrix3::rotate(const F32 roll, const F32 pitch, const F32 yaw) -{ - LLMatrix3 mat(roll, pitch, yaw); - *this *= mat; - return *this; -} - - -const LLMatrix3& LLMatrix3::rotate(const LLQuaternion &q) -{ - LLMatrix3 mat(q); - *this *= mat; - return *this; -} - -void LLMatrix3::add(const LLMatrix3& other_matrix) -{ - for (S32 i = 0; i < 3; ++i) - { - for (S32 j = 0; j < 3; ++j) - { - mMatrix[i][j] += other_matrix.mMatrix[i][j]; - } - } -} - -LLVector3 LLMatrix3::getFwdRow() const -{ - return LLVector3(mMatrix[VX]); -} - -LLVector3 LLMatrix3::getLeftRow() const -{ - return LLVector3(mMatrix[VY]); -} - -LLVector3 LLMatrix3::getUpRow() const -{ - return LLVector3(mMatrix[VZ]); -} - - - -const LLMatrix3& LLMatrix3::orthogonalize() -{ - LLVector3 x_axis(mMatrix[VX]); - LLVector3 y_axis(mMatrix[VY]); - LLVector3 z_axis(mMatrix[VZ]); - - x_axis.normVec(); - y_axis -= x_axis * (x_axis * y_axis); - y_axis.normVec(); - z_axis = x_axis % y_axis; - setRows(x_axis, y_axis, z_axis); - return (*this); -} - - -// LLMatrix3 Operators - -LLMatrix3 operator*(const LLMatrix3 &a, const LLMatrix3 &b) -{ - U32 i, j; - LLMatrix3 mat; - for (i = 0; i < NUM_VALUES_IN_MAT3; i++) - { - for (j = 0; j < NUM_VALUES_IN_MAT3; j++) - { - mat.mMatrix[j][i] = a.mMatrix[j][0] * b.mMatrix[0][i] + - a.mMatrix[j][1] * b.mMatrix[1][i] + - a.mMatrix[j][2] * b.mMatrix[2][i]; - } - } - return mat; -} - -/* Not implemented to help enforce code consistency with the syntax of - row-major notation. This is a Good Thing. -LLVector3 operator*(const LLMatrix3 &a, const LLVector3 &b) -{ - LLVector3 vec; - // matrix operates "from the left" on column vector - vec.mV[VX] = a.mMatrix[VX][VX] * b.mV[VX] + - a.mMatrix[VX][VY] * b.mV[VY] + - a.mMatrix[VX][VZ] * b.mV[VZ]; - - vec.mV[VY] = a.mMatrix[VY][VX] * b.mV[VX] + - a.mMatrix[VY][VY] * b.mV[VY] + - a.mMatrix[VY][VZ] * b.mV[VZ]; - - vec.mV[VZ] = a.mMatrix[VZ][VX] * b.mV[VX] + - a.mMatrix[VZ][VY] * b.mV[VY] + - a.mMatrix[VZ][VZ] * b.mV[VZ]; - return vec; -} -*/ - - -LLVector3 operator*(const LLVector3 &a, const LLMatrix3 &b) -{ - // matrix operates "from the right" on row vector - return LLVector3( - a.mV[VX] * b.mMatrix[VX][VX] + - a.mV[VY] * b.mMatrix[VY][VX] + - a.mV[VZ] * b.mMatrix[VZ][VX], - - a.mV[VX] * b.mMatrix[VX][VY] + - a.mV[VY] * b.mMatrix[VY][VY] + - a.mV[VZ] * b.mMatrix[VZ][VY], - - a.mV[VX] * b.mMatrix[VX][VZ] + - a.mV[VY] * b.mMatrix[VY][VZ] + - a.mV[VZ] * b.mMatrix[VZ][VZ] ); -} - -LLVector3d operator*(const LLVector3d &a, const LLMatrix3 &b) -{ - // matrix operates "from the right" on row vector - return LLVector3d( - a.mdV[VX] * b.mMatrix[VX][VX] + - a.mdV[VY] * b.mMatrix[VY][VX] + - a.mdV[VZ] * b.mMatrix[VZ][VX], - - a.mdV[VX] * b.mMatrix[VX][VY] + - a.mdV[VY] * b.mMatrix[VY][VY] + - a.mdV[VZ] * b.mMatrix[VZ][VY], - - a.mdV[VX] * b.mMatrix[VX][VZ] + - a.mdV[VY] * b.mMatrix[VY][VZ] + - a.mdV[VZ] * b.mMatrix[VZ][VZ] ); -} - -bool operator==(const LLMatrix3 &a, const LLMatrix3 &b) -{ - U32 i, j; - for (i = 0; i < NUM_VALUES_IN_MAT3; i++) - { - for (j = 0; j < NUM_VALUES_IN_MAT3; j++) - { - if (a.mMatrix[j][i] != b.mMatrix[j][i]) - return false; - } - } - return true; -} - -bool operator!=(const LLMatrix3 &a, const LLMatrix3 &b) -{ - U32 i, j; - for (i = 0; i < NUM_VALUES_IN_MAT3; i++) - { - for (j = 0; j < NUM_VALUES_IN_MAT3; j++) - { - if (a.mMatrix[j][i] != b.mMatrix[j][i]) - return true; - } - } - return false; -} - -const LLMatrix3& operator*=(LLMatrix3 &a, const LLMatrix3 &b) -{ - U32 i, j; - LLMatrix3 mat; - for (i = 0; i < NUM_VALUES_IN_MAT3; i++) - { - for (j = 0; j < NUM_VALUES_IN_MAT3; j++) - { - mat.mMatrix[j][i] = a.mMatrix[j][0] * b.mMatrix[0][i] + - a.mMatrix[j][1] * b.mMatrix[1][i] + - a.mMatrix[j][2] * b.mMatrix[2][i]; - } - } - a = mat; - return a; -} - -const LLMatrix3& operator*=(LLMatrix3 &a, F32 scalar ) -{ - for( U32 i = 0; i < NUM_VALUES_IN_MAT3; ++i ) - { - for( U32 j = 0; j < NUM_VALUES_IN_MAT3; ++j ) - { - a.mMatrix[i][j] *= scalar; - } - } - - return a; -} - -std::ostream& operator<<(std::ostream& s, const LLMatrix3 &a) -{ - s << "{ " - << a.mMatrix[VX][VX] << ", " << a.mMatrix[VX][VY] << ", " << a.mMatrix[VX][VZ] << "; " - << a.mMatrix[VY][VX] << ", " << a.mMatrix[VY][VY] << ", " << a.mMatrix[VY][VZ] << "; " - << a.mMatrix[VZ][VX] << ", " << a.mMatrix[VZ][VY] << ", " << a.mMatrix[VZ][VZ] - << " }"; - return s; -} - +/** + * @file m3math.cpp + * @brief LLMatrix3 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 "vmath.h" +#include "v3math.h" +#include "v3dmath.h" +#include "v4math.h" +#include "m4math.h" +#include "m3math.h" +#include "llquaternion.h" + +// LLMatrix3 + +// ji +// LLMatrix3 = |00 01 02 | +// |10 11 12 | +// |20 21 22 | + +// LLMatrix3 = |fx fy fz | forward-axis +// |lx ly lz | left-axis +// |ux uy uz | up-axis + + +// Constructors + + +LLMatrix3::LLMatrix3(const LLQuaternion &q) +{ + setRot(q); +} + + +LLMatrix3::LLMatrix3(const F32 angle, const LLVector3 &vec) +{ + LLQuaternion quat(angle, vec); + setRot(quat); +} + +LLMatrix3::LLMatrix3(const F32 angle, const LLVector3d &vec) +{ + LLVector3 vec_f; + vec_f.setVec(vec); + LLQuaternion quat(angle, vec_f); + setRot(quat); +} + +LLMatrix3::LLMatrix3(const F32 angle, const LLVector4 &vec) +{ + LLQuaternion quat(angle, vec); + setRot(quat); +} + +LLMatrix3::LLMatrix3(const F32 roll, const F32 pitch, const F32 yaw) +{ + setRot(roll,pitch,yaw); +} + +// From Matrix and Quaternion FAQ +void LLMatrix3::getEulerAngles(F32 *roll, F32 *pitch, F32 *yaw) const +{ + F64 angle_x, angle_y, angle_z; + F64 cx, cy, cz; // cosine of angle_x, angle_y, angle_z + F64 sx, sz; // sine of angle_x, angle_y, angle_z + + angle_y = asin(llclamp(mMatrix[2][0], -1.f, 1.f)); + cy = cos(angle_y); + + if (fabs(cy) > 0.005) // non-zero + { + // no gimbal lock + cx = mMatrix[2][2] / cy; + sx = - mMatrix[2][1] / cy; + + angle_x = (F32) atan2(sx, cx); + + cz = mMatrix[0][0] / cy; + sz = - mMatrix[1][0] / cy; + + angle_z = (F32) atan2(sz, cz); + } + else + { + // yup, gimbal lock + angle_x = 0; + + // some tricky math thereby avoided, see article + + cz = mMatrix[1][1]; + sz = mMatrix[0][1]; + + angle_z = atan2(sz, cz); + } + + *roll = (F32)angle_x; + *pitch = (F32)angle_y; + *yaw = (F32)angle_z; +} + + +// Clear and Assignment Functions + +const LLMatrix3& LLMatrix3::setIdentity() +{ + mMatrix[0][0] = 1.f; + mMatrix[0][1] = 0.f; + mMatrix[0][2] = 0.f; + + mMatrix[1][0] = 0.f; + mMatrix[1][1] = 1.f; + mMatrix[1][2] = 0.f; + + mMatrix[2][0] = 0.f; + mMatrix[2][1] = 0.f; + mMatrix[2][2] = 1.f; + return (*this); +} + +const LLMatrix3& LLMatrix3::clear() +{ + mMatrix[0][0] = 0.f; + mMatrix[0][1] = 0.f; + mMatrix[0][2] = 0.f; + + mMatrix[1][0] = 0.f; + mMatrix[1][1] = 0.f; + mMatrix[1][2] = 0.f; + + mMatrix[2][0] = 0.f; + mMatrix[2][1] = 0.f; + mMatrix[2][2] = 0.f; + return (*this); +} + +const LLMatrix3& LLMatrix3::setZero() +{ + mMatrix[0][0] = 0.f; + mMatrix[0][1] = 0.f; + mMatrix[0][2] = 0.f; + + mMatrix[1][0] = 0.f; + mMatrix[1][1] = 0.f; + mMatrix[1][2] = 0.f; + + mMatrix[2][0] = 0.f; + mMatrix[2][1] = 0.f; + mMatrix[2][2] = 0.f; + return (*this); +} + +// various useful mMatrix functions + +const LLMatrix3& LLMatrix3::transpose() +{ + // transpose the matrix + 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; + return *this; +} + + +F32 LLMatrix3::determinant() const +{ + // Is this a useful method when we assume the matrices are valid rotation + // matrices throughout this implementation? + return mMatrix[0][0] * (mMatrix[1][1] * mMatrix[2][2] - mMatrix[1][2] * mMatrix[2][1]) + + mMatrix[0][1] * (mMatrix[1][2] * mMatrix[2][0] - mMatrix[1][0] * mMatrix[2][2]) + + mMatrix[0][2] * (mMatrix[1][0] * mMatrix[2][1] - mMatrix[1][1] * mMatrix[2][0]); +} + +// inverts this matrix +void LLMatrix3::invert() +{ + // fails silently if determinant is zero too small + F32 det = determinant(); + const F32 VERY_SMALL_DETERMINANT = 0.000001f; + if (fabs(det) > VERY_SMALL_DETERMINANT) + { + // invertiable + LLMatrix3 t(*this); + mMatrix[VX][VX] = ( t.mMatrix[VY][VY] * t.mMatrix[VZ][VZ] - t.mMatrix[VY][VZ] * t.mMatrix[VZ][VY] ) / det; + mMatrix[VY][VX] = ( t.mMatrix[VY][VZ] * t.mMatrix[VZ][VX] - t.mMatrix[VY][VX] * t.mMatrix[VZ][VZ] ) / det; + mMatrix[VZ][VX] = ( t.mMatrix[VY][VX] * t.mMatrix[VZ][VY] - t.mMatrix[VY][VY] * t.mMatrix[VZ][VX] ) / det; + mMatrix[VX][VY] = ( t.mMatrix[VZ][VY] * t.mMatrix[VX][VZ] - t.mMatrix[VZ][VZ] * t.mMatrix[VX][VY] ) / det; + mMatrix[VY][VY] = ( t.mMatrix[VZ][VZ] * t.mMatrix[VX][VX] - t.mMatrix[VZ][VX] * t.mMatrix[VX][VZ] ) / det; + mMatrix[VZ][VY] = ( t.mMatrix[VZ][VX] * t.mMatrix[VX][VY] - t.mMatrix[VZ][VY] * t.mMatrix[VX][VX] ) / det; + mMatrix[VX][VZ] = ( t.mMatrix[VX][VY] * t.mMatrix[VY][VZ] - t.mMatrix[VX][VZ] * t.mMatrix[VY][VY] ) / det; + mMatrix[VY][VZ] = ( t.mMatrix[VX][VZ] * t.mMatrix[VY][VX] - t.mMatrix[VX][VX] * t.mMatrix[VY][VZ] ) / det; + mMatrix[VZ][VZ] = ( t.mMatrix[VX][VX] * t.mMatrix[VY][VY] - t.mMatrix[VX][VY] * t.mMatrix[VY][VX] ) / det; + } +} + +// does not assume a rotation matrix, and does not divide by determinant, assuming results will be renormalized +const LLMatrix3& LLMatrix3::adjointTranspose() +{ + LLMatrix3 adjoint_transpose; + adjoint_transpose.mMatrix[VX][VX] = mMatrix[VY][VY] * mMatrix[VZ][VZ] - mMatrix[VY][VZ] * mMatrix[VZ][VY] ; + adjoint_transpose.mMatrix[VY][VX] = mMatrix[VY][VZ] * mMatrix[VZ][VX] - mMatrix[VY][VX] * mMatrix[VZ][VZ] ; + adjoint_transpose.mMatrix[VZ][VX] = mMatrix[VY][VX] * mMatrix[VZ][VY] - mMatrix[VY][VY] * mMatrix[VZ][VX] ; + adjoint_transpose.mMatrix[VX][VY] = mMatrix[VZ][VY] * mMatrix[VX][VZ] - mMatrix[VZ][VZ] * mMatrix[VX][VY] ; + adjoint_transpose.mMatrix[VY][VY] = mMatrix[VZ][VZ] * mMatrix[VX][VX] - mMatrix[VZ][VX] * mMatrix[VX][VZ] ; + adjoint_transpose.mMatrix[VZ][VY] = mMatrix[VZ][VX] * mMatrix[VX][VY] - mMatrix[VZ][VY] * mMatrix[VX][VX] ; + adjoint_transpose.mMatrix[VX][VZ] = mMatrix[VX][VY] * mMatrix[VY][VZ] - mMatrix[VX][VZ] * mMatrix[VY][VY] ; + adjoint_transpose.mMatrix[VY][VZ] = mMatrix[VX][VZ] * mMatrix[VY][VX] - mMatrix[VX][VX] * mMatrix[VY][VZ] ; + adjoint_transpose.mMatrix[VZ][VZ] = mMatrix[VX][VX] * mMatrix[VY][VY] - mMatrix[VX][VY] * mMatrix[VY][VX] ; + + *this = adjoint_transpose; + return *this; +} + +// 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 LLMatrix3::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; +} + +const LLMatrix3& LLMatrix3::setRot(const F32 angle, const LLVector3 &vec) +{ + setRot(LLQuaternion(angle, vec)); + return *this; +} + +const LLMatrix3& LLMatrix3::setRot(const F32 roll, const F32 pitch, const F32 yaw) +{ + // Rotates RH about x-axis by 'roll' then + // rotates RH about the old y-axis by 'pitch' then + // rotates RH about the original z-axis by 'yaw'. + // . + // /|\ yaw axis + // | __. + // ._ ___| /| pitch axis + // _||\ \\ |-. / + // \|| \_______\_|__\_/_______ + // | _ _ o o o_o_o_o o /_\_ ________\ roll axis + // // /_______/ /__________> / + // /_,-' // / + // /__,-' + + F32 cx, sx, cy, sy, cz, sz; + F32 cxsy, sxsy; + + cx = (F32)cos(roll); //A + sx = (F32)sin(roll); //B + cy = (F32)cos(pitch); //C + sy = (F32)sin(pitch); //D + cz = (F32)cos(yaw); //E + sz = (F32)sin(yaw); //F + + cxsy = cx * sy; //AD + sxsy = sx * sy; //BD + + mMatrix[0][0] = cy * cz; + mMatrix[1][0] = -cy * sz; + mMatrix[2][0] = sy; + mMatrix[0][1] = sxsy * cz + cx * sz; + mMatrix[1][1] = -sxsy * sz + cx * cz; + mMatrix[2][1] = -sx * cy; + mMatrix[0][2] = -cxsy * cz + sx * sz; + mMatrix[1][2] = cxsy * sz + sx * cz; + mMatrix[2][2] = cx * cy; + return *this; +} + + +const LLMatrix3& LLMatrix3::setRot(const LLQuaternion &q) +{ + *this = q.getMatrix3(); + return *this; +} + +const LLMatrix3& LLMatrix3::setRows(const LLVector3 &fwd, const LLVector3 &left, const LLVector3 &up) +{ + mMatrix[0][0] = fwd.mV[0]; + mMatrix[0][1] = fwd.mV[1]; + mMatrix[0][2] = fwd.mV[2]; + + mMatrix[1][0] = left.mV[0]; + mMatrix[1][1] = left.mV[1]; + mMatrix[1][2] = left.mV[2]; + + mMatrix[2][0] = up.mV[0]; + mMatrix[2][1] = up.mV[1]; + mMatrix[2][2] = up.mV[2]; + + return *this; +} + +const LLMatrix3& LLMatrix3::setRow( U32 rowIndex, const LLVector3& row ) +{ + llassert( rowIndex >= 0 && rowIndex < NUM_VALUES_IN_MAT3 ); + + mMatrix[rowIndex][0] = row[0]; + mMatrix[rowIndex][1] = row[1]; + mMatrix[rowIndex][2] = row[2]; + + return *this; +} + +const LLMatrix3& LLMatrix3::setCol( U32 colIndex, const LLVector3& col ) +{ + llassert( colIndex >= 0 && colIndex < NUM_VALUES_IN_MAT3 ); + + mMatrix[0][colIndex] = col[0]; + mMatrix[1][colIndex] = col[1]; + mMatrix[2][colIndex] = col[2]; + + return *this; +} + +const LLMatrix3& LLMatrix3::rotate(const F32 angle, const LLVector3 &vec) +{ + LLMatrix3 mat(angle, vec); + *this *= mat; + return *this; +} + + +const LLMatrix3& LLMatrix3::rotate(const F32 roll, const F32 pitch, const F32 yaw) +{ + LLMatrix3 mat(roll, pitch, yaw); + *this *= mat; + return *this; +} + + +const LLMatrix3& LLMatrix3::rotate(const LLQuaternion &q) +{ + LLMatrix3 mat(q); + *this *= mat; + return *this; +} + +void LLMatrix3::add(const LLMatrix3& other_matrix) +{ + for (S32 i = 0; i < 3; ++i) + { + for (S32 j = 0; j < 3; ++j) + { + mMatrix[i][j] += other_matrix.mMatrix[i][j]; + } + } +} + +LLVector3 LLMatrix3::getFwdRow() const +{ + return LLVector3(mMatrix[VX]); +} + +LLVector3 LLMatrix3::getLeftRow() const +{ + return LLVector3(mMatrix[VY]); +} + +LLVector3 LLMatrix3::getUpRow() const +{ + return LLVector3(mMatrix[VZ]); +} + + + +const LLMatrix3& LLMatrix3::orthogonalize() +{ + LLVector3 x_axis(mMatrix[VX]); + LLVector3 y_axis(mMatrix[VY]); + LLVector3 z_axis(mMatrix[VZ]); + + x_axis.normVec(); + y_axis -= x_axis * (x_axis * y_axis); + y_axis.normVec(); + z_axis = x_axis % y_axis; + setRows(x_axis, y_axis, z_axis); + return (*this); +} + + +// LLMatrix3 Operators + +LLMatrix3 operator*(const LLMatrix3 &a, const LLMatrix3 &b) +{ + U32 i, j; + LLMatrix3 mat; + for (i = 0; i < NUM_VALUES_IN_MAT3; i++) + { + for (j = 0; j < NUM_VALUES_IN_MAT3; j++) + { + mat.mMatrix[j][i] = a.mMatrix[j][0] * b.mMatrix[0][i] + + a.mMatrix[j][1] * b.mMatrix[1][i] + + a.mMatrix[j][2] * b.mMatrix[2][i]; + } + } + return mat; +} + +/* Not implemented to help enforce code consistency with the syntax of + row-major notation. This is a Good Thing. +LLVector3 operator*(const LLMatrix3 &a, const LLVector3 &b) +{ + LLVector3 vec; + // matrix operates "from the left" on column vector + vec.mV[VX] = a.mMatrix[VX][VX] * b.mV[VX] + + a.mMatrix[VX][VY] * b.mV[VY] + + a.mMatrix[VX][VZ] * b.mV[VZ]; + + vec.mV[VY] = a.mMatrix[VY][VX] * b.mV[VX] + + a.mMatrix[VY][VY] * b.mV[VY] + + a.mMatrix[VY][VZ] * b.mV[VZ]; + + vec.mV[VZ] = a.mMatrix[VZ][VX] * b.mV[VX] + + a.mMatrix[VZ][VY] * b.mV[VY] + + a.mMatrix[VZ][VZ] * b.mV[VZ]; + return vec; +} +*/ + + +LLVector3 operator*(const LLVector3 &a, const LLMatrix3 &b) +{ + // matrix operates "from the right" on row vector + return LLVector3( + a.mV[VX] * b.mMatrix[VX][VX] + + a.mV[VY] * b.mMatrix[VY][VX] + + a.mV[VZ] * b.mMatrix[VZ][VX], + + a.mV[VX] * b.mMatrix[VX][VY] + + a.mV[VY] * b.mMatrix[VY][VY] + + a.mV[VZ] * b.mMatrix[VZ][VY], + + a.mV[VX] * b.mMatrix[VX][VZ] + + a.mV[VY] * b.mMatrix[VY][VZ] + + a.mV[VZ] * b.mMatrix[VZ][VZ] ); +} + +LLVector3d operator*(const LLVector3d &a, const LLMatrix3 &b) +{ + // matrix operates "from the right" on row vector + return LLVector3d( + a.mdV[VX] * b.mMatrix[VX][VX] + + a.mdV[VY] * b.mMatrix[VY][VX] + + a.mdV[VZ] * b.mMatrix[VZ][VX], + + a.mdV[VX] * b.mMatrix[VX][VY] + + a.mdV[VY] * b.mMatrix[VY][VY] + + a.mdV[VZ] * b.mMatrix[VZ][VY], + + a.mdV[VX] * b.mMatrix[VX][VZ] + + a.mdV[VY] * b.mMatrix[VY][VZ] + + a.mdV[VZ] * b.mMatrix[VZ][VZ] ); +} + +bool operator==(const LLMatrix3 &a, const LLMatrix3 &b) +{ + U32 i, j; + for (i = 0; i < NUM_VALUES_IN_MAT3; i++) + { + for (j = 0; j < NUM_VALUES_IN_MAT3; j++) + { + if (a.mMatrix[j][i] != b.mMatrix[j][i]) + return false; + } + } + return true; +} + +bool operator!=(const LLMatrix3 &a, const LLMatrix3 &b) +{ + U32 i, j; + for (i = 0; i < NUM_VALUES_IN_MAT3; i++) + { + for (j = 0; j < NUM_VALUES_IN_MAT3; j++) + { + if (a.mMatrix[j][i] != b.mMatrix[j][i]) + return true; + } + } + return false; +} + +const LLMatrix3& operator*=(LLMatrix3 &a, const LLMatrix3 &b) +{ + U32 i, j; + LLMatrix3 mat; + for (i = 0; i < NUM_VALUES_IN_MAT3; i++) + { + for (j = 0; j < NUM_VALUES_IN_MAT3; j++) + { + mat.mMatrix[j][i] = a.mMatrix[j][0] * b.mMatrix[0][i] + + a.mMatrix[j][1] * b.mMatrix[1][i] + + a.mMatrix[j][2] * b.mMatrix[2][i]; + } + } + a = mat; + return a; +} + +const LLMatrix3& operator*=(LLMatrix3 &a, F32 scalar ) +{ + for( U32 i = 0; i < NUM_VALUES_IN_MAT3; ++i ) + { + for( U32 j = 0; j < NUM_VALUES_IN_MAT3; ++j ) + { + a.mMatrix[i][j] *= scalar; + } + } + + return a; +} + +std::ostream& operator<<(std::ostream& s, const LLMatrix3 &a) +{ + s << "{ " + << a.mMatrix[VX][VX] << ", " << a.mMatrix[VX][VY] << ", " << a.mMatrix[VX][VZ] << "; " + << a.mMatrix[VY][VX] << ", " << a.mMatrix[VY][VY] << ", " << a.mMatrix[VY][VZ] << "; " + << a.mMatrix[VZ][VX] << ", " << a.mMatrix[VZ][VY] << ", " << a.mMatrix[VZ][VZ] + << " }"; + return s; +} + -- cgit v1.2.3