920 lines
22 KiB
C++
920 lines
22 KiB
C++
/**
|
|
* @file m4math.cpp
|
|
* @brief LLMatrix4 class implementation.
|
|
*
|
|
* $LicenseInfo:firstyear=2000&license=viewergpl$
|
|
*
|
|
* Copyright (c) 2000-2009, Linden Research, Inc.
|
|
*
|
|
* Second Life Viewer Source Code
|
|
* The source code in this file ("Source Code") is provided by Linden Lab
|
|
* to you under the terms of the GNU General Public License, version 2.0
|
|
* ("GPL"), unless you have obtained a separate licensing agreement
|
|
* ("Other License"), formally executed by you and Linden Lab. Terms of
|
|
* the GPL can be found in doc/GPL-license.txt in this distribution, or
|
|
* online at http://secondlifegrid.net/programs/open_source/licensing/gplv2
|
|
*
|
|
* There are special exceptions to the terms and conditions of the GPL as
|
|
* it is applied to this Source Code. View the full text of the exception
|
|
* in the file doc/FLOSS-exception.txt in this software distribution, or
|
|
* online at
|
|
* http://secondlifegrid.net/programs/open_source/licensing/flossexception
|
|
*
|
|
* By copying, modifying or distributing this software, you acknowledge
|
|
* that you have read and understood your obligations described above,
|
|
* and agree to abide by those obligations.
|
|
*
|
|
* ALL LINDEN LAB SOURCE CODE IS PROVIDED "AS IS." LINDEN LAB MAKES NO
|
|
* WARRANTIES, EXPRESS, IMPLIED OR OTHERWISE, REGARDING ITS ACCURACY,
|
|
* COMPLETENESS OR PERFORMANCE.
|
|
* $/LicenseInfo$
|
|
*/
|
|
|
|
#include "linden_common.h"
|
|
|
|
//#include "vmath.h"
|
|
#include "v3math.h"
|
|
#include "v4math.h"
|
|
#include "m4math.h"
|
|
#include "m3math.h"
|
|
#include "llquaternion.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 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;
|
|
}
|
|
|
|
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(const F32 angle, const F32 x, const F32 y, const F32 z)
|
|
{
|
|
LLMatrix3 mat(angle, x, y, z);
|
|
return initMatrix(mat);
|
|
}
|
|
|
|
|
|
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);
|
|
}
|
|
|
|
|
|
// Position and Rotation
|
|
const LLMatrix4& LLMatrix4::initRotTrans(const F32 angle, const F32 rx, const F32 ry, const F32 rz,
|
|
const F32 tx, const F32 ty, const F32 tz)
|
|
{
|
|
LLMatrix3 mat(angle, rx, ry, rz);
|
|
LLVector3 translation(tx, ty, tz);
|
|
initMatrix(mat);
|
|
setTranslation(translation);
|
|
return (*this);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
// Rotate exisitng mMatrix
|
|
const LLMatrix4& LLMatrix4::rotate(const F32 angle, const F32 x, const F32 y, const F32 z)
|
|
{
|
|
LLVector4 vec4(x, y, z);
|
|
LLMatrix4 mat(angle, vec4);
|
|
*this *= mat;
|
|
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
|
|
|
|
|
|
/* Not implemented to help enforce code consistency with the syntax of
|
|
row-major notation. This is a Good Thing.
|
|
LLVector4 operator*(const LLMatrix4 &a, const LLVector4 &b)
|
|
{
|
|
// Operate "to the right" on column-vector b
|
|
LLVector4 vec;
|
|
vec.mV[VX] = a.mMatrix[VX][VX] * b.mV[VX] +
|
|
a.mMatrix[VY][VX] * b.mV[VY] +
|
|
a.mMatrix[VZ][VX] * b.mV[VZ] +
|
|
a.mMatrix[VW][VX] * b.mV[VW];
|
|
|
|
vec.mV[VY] = a.mMatrix[VX][VY] * b.mV[VX] +
|
|
a.mMatrix[VY][VY] * b.mV[VY] +
|
|
a.mMatrix[VZ][VY] * b.mV[VZ] +
|
|
a.mMatrix[VW][VY] * b.mV[VW];
|
|
|
|
vec.mV[VZ] = a.mMatrix[VX][VZ] * b.mV[VX] +
|
|
a.mMatrix[VY][VZ] * b.mV[VY] +
|
|
a.mMatrix[VZ][VZ] * b.mV[VZ] +
|
|
a.mMatrix[VW][VZ] * b.mV[VW];
|
|
|
|
vec.mV[VW] = a.mMatrix[VX][VW] * b.mV[VX] +
|
|
a.mMatrix[VY][VW] * b.mV[VY] +
|
|
a.mMatrix[VZ][VW] * b.mV[VZ] +
|
|
a.mMatrix[VW][VW] * b.mV[VW];
|
|
return vec;
|
|
}
|
|
*/
|
|
|
|
|
|
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();
|
|
}
|
|
|
|
|