/*==LICENSE==* CyanWorlds.com Engine - MMOG client, server and tools Copyright (C) 2011 Cyan Worlds, Inc. This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program 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 General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . You can contact Cyan Worlds, Inc. by email legal@cyan.com or by snail mail at: Cyan Worlds, Inc. 14617 N Newport Hwy Mead, WA 99021 *==LICENSE==*/ #include "HeadSpin.h" #include "hsGeometry3.h" #include "hsQuat.h" #include "hsMatrix44.h" #include "hsStream.h" static hsMatrix44 myIdent = hsMatrix44().Reset(); const hsMatrix44& hsMatrix44::IdentityMatrix() { return myIdent; } /* For the rotation: ¦ 2 2 ¦ ¦ 1 - (2Y + 2Z ) 2XY + 2ZW 2XZ - 2YW ¦ ¦ ¦ ¦ 2 2 ¦ M = ¦ 2XY - 2ZW 1 - (2X + 2Z ) 2YZ + 2XW ¦ ¦ ¦ ¦ 2 2 ¦ ¦ 2XZ + 2YW 2YZ - 2XW 1 - (2X + 2Y ) ¦ ¦ ¦ The translation is far too complex to discuss here. ;^) */ hsMatrix44::hsMatrix44(const hsScalarTriple &translate, const hsQuat &rotate) { float xx = rotate.fX * rotate.fX; float xy = rotate.fX * rotate.fY; float xz = rotate.fX * rotate.fZ; float xw = rotate.fX * rotate.fW; float yy = rotate.fY * rotate.fY; float yz = rotate.fY * rotate.fZ; float yw = rotate.fY * rotate.fW; float zz = rotate.fZ * rotate.fZ; float zw = rotate.fZ * rotate.fW; fMap[0][0] = 1 - 2 * ( yy + zz ); fMap[0][1] = 2 * ( xy - zw ); fMap[0][2] = 2 * ( xz + yw ); fMap[0][3] = translate.fX; fMap[1][0] = 2 * ( xy + zw ); fMap[1][1] = 1 - 2 * ( xx + zz ); fMap[1][2] = 2 * ( yz - xw ); fMap[1][3] = translate.fY; fMap[2][0] = 2 * ( xz - yw ); fMap[2][1] = 2 * ( yz + xw ); fMap[2][2] = 1 - 2 * ( xx + yy ); fMap[2][3] = translate.fZ; fMap[3][0] = fMap[3][1] = fMap[3][2] = 0.0f; fMap[3][3] = 1.0f; NotIdentity(); } void hsMatrix44::DecompRigid(hsScalarTriple &translate, hsQuat &rotate) const { translate = GetTranslate(); rotate.QuatFromMatrix44(*this); } #if 0 hsMatrix44& hsMatrix44::Reset() { for(int i = 0; i < 4; i++) { for(int j = 0; j < 4; j++) { fMap[i][j] = (i==j) ? hsScalar1 : 0; } } return *this; } #endif #if 0 // Havok reeks hsMatrix44 operator*(const hsMatrix44& a, const hsMatrix44& b) { hsMatrix44 c; if( a.fFlags & b.fFlags & hsMatrix44::kIsIdent ) { c.Reset(); return c; } if( a.fFlags & hsMatrix44::kIsIdent ) return b; if( b.fFlags & hsMatrix44::kIsIdent ) return a; #if HS_BUILD_FOR_PS2 MulMatrixVU0(a.fMap, b.fMap, c.fMap); #else c.fMap[0][0] = hsScalarMul(a.fMap[0][0], b.fMap[0][0]) + hsScalarMul(a.fMap[0][1], b.fMap[1][0]) + hsScalarMul(a.fMap[0][2], b.fMap[2][0]) + hsScalarMul(a.fMap[0][3], b.fMap[3][0]); c.fMap[0][1] = hsScalarMul(a.fMap[0][0], b.fMap[0][1]) + hsScalarMul(a.fMap[0][1], b.fMap[1][1]) + hsScalarMul(a.fMap[0][2], b.fMap[2][1]) + hsScalarMul(a.fMap[0][3], b.fMap[3][1]); c.fMap[0][2] = hsScalarMul(a.fMap[0][0], b.fMap[0][2]) + hsScalarMul(a.fMap[0][1], b.fMap[1][2]) + hsScalarMul(a.fMap[0][2], b.fMap[2][2]) + hsScalarMul(a.fMap[0][3], b.fMap[3][2]); c.fMap[0][3] = hsScalarMul(a.fMap[0][0], b.fMap[0][3]) + hsScalarMul(a.fMap[0][1], b.fMap[1][3]) + hsScalarMul(a.fMap[0][2], b.fMap[2][3]) + hsScalarMul(a.fMap[0][3], b.fMap[3][3]); c.fMap[1][0] = hsScalarMul(a.fMap[1][0], b.fMap[0][0]) + hsScalarMul(a.fMap[1][1], b.fMap[1][0]) + hsScalarMul(a.fMap[1][2], b.fMap[2][0]) + hsScalarMul(a.fMap[1][3], b.fMap[3][0]); c.fMap[1][1] = hsScalarMul(a.fMap[1][0], b.fMap[0][1]) + hsScalarMul(a.fMap[1][1], b.fMap[1][1]) + hsScalarMul(a.fMap[1][2], b.fMap[2][1]) + hsScalarMul(a.fMap[1][3], b.fMap[3][1]); c.fMap[1][2] = hsScalarMul(a.fMap[1][0], b.fMap[0][2]) + hsScalarMul(a.fMap[1][1], b.fMap[1][2]) + hsScalarMul(a.fMap[1][2], b.fMap[2][2]) + hsScalarMul(a.fMap[1][3], b.fMap[3][2]); c.fMap[1][3] = hsScalarMul(a.fMap[1][0], b.fMap[0][3]) + hsScalarMul(a.fMap[1][1], b.fMap[1][3]) + hsScalarMul(a.fMap[1][2], b.fMap[2][3]) + hsScalarMul(a.fMap[1][3], b.fMap[3][3]); c.fMap[2][0] = hsScalarMul(a.fMap[2][0], b.fMap[0][0]) + hsScalarMul(a.fMap[2][1], b.fMap[1][0]) + hsScalarMul(a.fMap[2][2], b.fMap[2][0]) + hsScalarMul(a.fMap[2][3], b.fMap[3][0]); c.fMap[2][1] = hsScalarMul(a.fMap[2][0], b.fMap[0][1]) + hsScalarMul(a.fMap[2][1], b.fMap[1][1]) + hsScalarMul(a.fMap[2][2], b.fMap[2][1]) + hsScalarMul(a.fMap[2][3], b.fMap[3][1]); c.fMap[2][2] = hsScalarMul(a.fMap[2][0], b.fMap[0][2]) + hsScalarMul(a.fMap[2][1], b.fMap[1][2]) + hsScalarMul(a.fMap[2][2], b.fMap[2][2]) + hsScalarMul(a.fMap[2][3], b.fMap[3][2]); c.fMap[2][3] = hsScalarMul(a.fMap[2][0], b.fMap[0][3]) + hsScalarMul(a.fMap[2][1], b.fMap[1][3]) + hsScalarMul(a.fMap[2][2], b.fMap[2][3]) + hsScalarMul(a.fMap[2][3], b.fMap[3][3]); c.fMap[3][0] = hsScalarMul(a.fMap[3][0], b.fMap[0][0]) + hsScalarMul(a.fMap[3][1], b.fMap[1][0]) + hsScalarMul(a.fMap[3][2], b.fMap[2][0]) + hsScalarMul(a.fMap[3][3], b.fMap[3][0]); c.fMap[3][1] = hsScalarMul(a.fMap[3][0], b.fMap[0][1]) + hsScalarMul(a.fMap[3][1], b.fMap[1][1]) + hsScalarMul(a.fMap[3][2], b.fMap[2][1]) + hsScalarMul(a.fMap[3][3], b.fMap[3][1]); c.fMap[3][2] = hsScalarMul(a.fMap[3][0], b.fMap[0][2]) + hsScalarMul(a.fMap[3][1], b.fMap[1][2]) + hsScalarMul(a.fMap[3][2], b.fMap[2][2]) + hsScalarMul(a.fMap[3][3], b.fMap[3][2]); c.fMap[3][3] = hsScalarMul(a.fMap[3][0], b.fMap[0][3]) + hsScalarMul(a.fMap[3][1], b.fMap[1][3]) + hsScalarMul(a.fMap[3][2], b.fMap[2][3]) + hsScalarMul(a.fMap[3][3], b.fMap[3][3]); #endif return c; } hsVector3 operator*(const hsMatrix44& m, const hsVector3& p) { if( m.fFlags & hsMatrix44::kIsIdent ) return p; hsVector3 rVal; #if HS_BUILD_FOR_PS2 MulVectorVU0(m.fMap, (MATRIX4) &p, (MATRIX4) &rVal); #else rVal.fX = hsScalarMul(p.fX, m.fMap[0][0]) + hsScalarMul(p.fY, m.fMap[0][1]) + hsScalarMul(p.fZ, m.fMap[0][2]); rVal.fY = hsScalarMul(p.fX, m.fMap[1][0]) + hsScalarMul(p.fY, m.fMap[1][1]) + hsScalarMul(p.fZ, m.fMap[1][2]); rVal.fZ = hsScalarMul(p.fX, m.fMap[2][0]) + hsScalarMul(p.fY, m.fMap[2][1]) + hsScalarMul(p.fZ, m.fMap[2][2]); #endif return rVal; } #else // Havok reeks hsMatrix44 hsMatrix44::operator*(const hsMatrix44& b) const { hsMatrix44 c; if( fFlags & b.fFlags & hsMatrix44::kIsIdent ) { c.Reset(); return c; } if( fFlags & hsMatrix44::kIsIdent ) return b; if( b.fFlags & hsMatrix44::kIsIdent ) return *this; #if HS_BUILD_FOR_PS2 MulMatrixVU0(fMap, b.fMap, c.fMap); #else c.fMap[0][0] = hsScalarMul(fMap[0][0], b.fMap[0][0]) + hsScalarMul(fMap[0][1], b.fMap[1][0]) + hsScalarMul(fMap[0][2], b.fMap[2][0]) + hsScalarMul(fMap[0][3], b.fMap[3][0]); c.fMap[0][1] = hsScalarMul(fMap[0][0], b.fMap[0][1]) + hsScalarMul(fMap[0][1], b.fMap[1][1]) + hsScalarMul(fMap[0][2], b.fMap[2][1]) + hsScalarMul(fMap[0][3], b.fMap[3][1]); c.fMap[0][2] = hsScalarMul(fMap[0][0], b.fMap[0][2]) + hsScalarMul(fMap[0][1], b.fMap[1][2]) + hsScalarMul(fMap[0][2], b.fMap[2][2]) + hsScalarMul(fMap[0][3], b.fMap[3][2]); c.fMap[0][3] = hsScalarMul(fMap[0][0], b.fMap[0][3]) + hsScalarMul(fMap[0][1], b.fMap[1][3]) + hsScalarMul(fMap[0][2], b.fMap[2][3]) + hsScalarMul(fMap[0][3], b.fMap[3][3]); c.fMap[1][0] = hsScalarMul(fMap[1][0], b.fMap[0][0]) + hsScalarMul(fMap[1][1], b.fMap[1][0]) + hsScalarMul(fMap[1][2], b.fMap[2][0]) + hsScalarMul(fMap[1][3], b.fMap[3][0]); c.fMap[1][1] = hsScalarMul(fMap[1][0], b.fMap[0][1]) + hsScalarMul(fMap[1][1], b.fMap[1][1]) + hsScalarMul(fMap[1][2], b.fMap[2][1]) + hsScalarMul(fMap[1][3], b.fMap[3][1]); c.fMap[1][2] = hsScalarMul(fMap[1][0], b.fMap[0][2]) + hsScalarMul(fMap[1][1], b.fMap[1][2]) + hsScalarMul(fMap[1][2], b.fMap[2][2]) + hsScalarMul(fMap[1][3], b.fMap[3][2]); c.fMap[1][3] = hsScalarMul(fMap[1][0], b.fMap[0][3]) + hsScalarMul(fMap[1][1], b.fMap[1][3]) + hsScalarMul(fMap[1][2], b.fMap[2][3]) + hsScalarMul(fMap[1][3], b.fMap[3][3]); c.fMap[2][0] = hsScalarMul(fMap[2][0], b.fMap[0][0]) + hsScalarMul(fMap[2][1], b.fMap[1][0]) + hsScalarMul(fMap[2][2], b.fMap[2][0]) + hsScalarMul(fMap[2][3], b.fMap[3][0]); c.fMap[2][1] = hsScalarMul(fMap[2][0], b.fMap[0][1]) + hsScalarMul(fMap[2][1], b.fMap[1][1]) + hsScalarMul(fMap[2][2], b.fMap[2][1]) + hsScalarMul(fMap[2][3], b.fMap[3][1]); c.fMap[2][2] = hsScalarMul(fMap[2][0], b.fMap[0][2]) + hsScalarMul(fMap[2][1], b.fMap[1][2]) + hsScalarMul(fMap[2][2], b.fMap[2][2]) + hsScalarMul(fMap[2][3], b.fMap[3][2]); c.fMap[2][3] = hsScalarMul(fMap[2][0], b.fMap[0][3]) + hsScalarMul(fMap[2][1], b.fMap[1][3]) + hsScalarMul(fMap[2][2], b.fMap[2][3]) + hsScalarMul(fMap[2][3], b.fMap[3][3]); c.fMap[3][0] = hsScalarMul(fMap[3][0], b.fMap[0][0]) + hsScalarMul(fMap[3][1], b.fMap[1][0]) + hsScalarMul(fMap[3][2], b.fMap[2][0]) + hsScalarMul(fMap[3][3], b.fMap[3][0]); c.fMap[3][1] = hsScalarMul(fMap[3][0], b.fMap[0][1]) + hsScalarMul(fMap[3][1], b.fMap[1][1]) + hsScalarMul(fMap[3][2], b.fMap[2][1]) + hsScalarMul(fMap[3][3], b.fMap[3][1]); c.fMap[3][2] = hsScalarMul(fMap[3][0], b.fMap[0][2]) + hsScalarMul(fMap[3][1], b.fMap[1][2]) + hsScalarMul(fMap[3][2], b.fMap[2][2]) + hsScalarMul(fMap[3][3], b.fMap[3][2]); c.fMap[3][3] = hsScalarMul(fMap[3][0], b.fMap[0][3]) + hsScalarMul(fMap[3][1], b.fMap[1][3]) + hsScalarMul(fMap[3][2], b.fMap[2][3]) + hsScalarMul(fMap[3][3], b.fMap[3][3]); #endif return c; } hsVector3 hsMatrix44::operator*(const hsVector3& p) const { if( fFlags & hsMatrix44::kIsIdent ) return p; hsVector3 rVal; #if HS_BUILD_FOR_PS2 MulVectorVU0(fMap, (MATRIX4) &p, (MATRIX4) &rVal); #else rVal.fX = hsScalarMul(p.fX, fMap[0][0]) + hsScalarMul(p.fY, fMap[0][1]) + hsScalarMul(p.fZ, fMap[0][2]); rVal.fY = hsScalarMul(p.fX, fMap[1][0]) + hsScalarMul(p.fY, fMap[1][1]) + hsScalarMul(p.fZ, fMap[1][2]); rVal.fZ = hsScalarMul(p.fX, fMap[2][0]) + hsScalarMul(p.fY, fMap[2][1]) + hsScalarMul(p.fZ, fMap[2][2]); #endif return rVal; } #endif // Havok reeks #if 0 // Havok reeks int operator==(const hsMatrix44& s, const hsMatrix44& t) { if( s.fFlags & t.fFlags & hsMatrix44::kIsIdent ) { return true; } for(int i = 0; i < 4; i++) { for(int j = 0; j < 4; j++) { if (s.fMap[i][j] != t.fMap[i][j]) return false; } } return true; } #else // Havok reeks int hsMatrix44::operator==(const hsMatrix44& ss) const { if( ss.fFlags & fFlags & hsMatrix44::kIsIdent ) { return true; } for(int i = 0; i < 4; i++) { for(int j = 0; j < 4; j++) { if (ss.fMap[i][j] != fMap[i][j]) return false; } } return true; } #endif // Havok reeks hsMatrix44& hsMatrix44::Scale(const hsVector3* scale) { #if HS_BUILD_FOR_PS2 MulScaleVU0(fMap, (MATRIX4 *) scale); #else fMap[0][0] *= scale->fX; fMap[0][1] *= scale->fX; fMap[0][2] *= scale->fX; fMap[0][3] *= scale->fX; fMap[1][0] *= scale->fY; fMap[1][1] *= scale->fY; fMap[1][2] *= scale->fY; fMap[1][3] *= scale->fY; fMap[2][0] *= scale->fZ; fMap[2][1] *= scale->fZ; fMap[2][2] *= scale->fZ; fMap[2][3] *= scale->fZ; #endif NotIdentity(); return *this; } hsVector3 hsMatrix44::RemoveScale() { hsVector3 xCol(fMap[0][0], fMap[0][1], fMap[0][2]); float xLen = xCol.Magnitude(); hsVector3 yCol(fMap[1][0], fMap[1][1], fMap[1][2]); float yLen = yCol.Magnitude(); hsVector3 zCol(fMap[2][0], fMap[2][1], fMap[2][2]); float zLen = zCol.Magnitude(); fMap[0][0] /= xLen; fMap[0][1] /= xLen; fMap[0][2] /= xLen; fMap[1][0] /= yLen; fMap[1][1] /= yLen; fMap[1][2] /= yLen; fMap[2][0] /= zLen; fMap[2][1] /= zLen; fMap[2][2] /= zLen; hsVector3 oldScale(xLen, yLen, zLen); return oldScale; } hsMatrix44& hsMatrix44::Translate(const hsVector3* pt) { #if HS_BUILD_FOR_PS2 TranslateVU0(fMap, (MATRIX4 *) pt); /* SUNSOFT */ #else for (int i =0; i < 3; i++) { fMap[i][3] += (*pt)[i]; } #endif NotIdentity(); return *this; } hsMatrix44& hsMatrix44::SetTranslate(const hsScalarTriple* pt) { for (int i =0; i < 3; i++) { fMap[i][3] = (*pt)[i]; } NotIdentity(); return *this; } hsMatrix44& hsMatrix44::MakeRotateMat(int axis, hsScalar radians) { Reset(); SetRotate(axis, radians); NotIdentity(); return *this; } hsMatrix44& hsMatrix44::Rotate(int axis, hsScalar radians) { hsMatrix44 rMat; rMat.MakeRotateMat(axis, radians); *this = rMat * *this; return *this; } hsMatrix44& hsMatrix44::SetRotate(int axis, hsScalar radians) { hsScalar s = hsSine(radians); hsScalar c = hsCosine(radians); int c1,c2; switch (axis) { case 0: c1 = 1; c2 = 2; break; case 1: c1 = 0; c2 = 2; break; case 2: c1 = 0; c2 = 1; break; } fMap[c1][c1] = c; fMap[c2][c2] = c; fMap[c1][c2] = s; fMap[c2][c1] = -s; NotIdentity(); return *this; } void hsMatrix44::MakeXRotation(hsScalar radians) { Reset(); hsScalar s = hsSine(radians); hsScalar c = hsCosine(radians); fMap[1][1] = c; fMap[2][2] = c; fMap[1][2] = s; fMap[2][1] = -s; NotIdentity(); } void hsMatrix44::MakeYRotation(hsScalar radians) { Reset(); hsScalar s = hsSine(radians); hsScalar c = hsCosine(radians); fMap[0][0] = c; fMap[2][2] = c; fMap[0][2] = -s; fMap[2][0] = s; NotIdentity(); } void hsMatrix44::MakeZRotation(hsScalar radians) { Reset(); hsScalar s = hsSine(radians); hsScalar c = hsCosine(radians); fMap[0][0] = c; fMap[1][1] = c; fMap[0][1] = s; fMap[1][0] = -s; NotIdentity(); } // // Not a camera matrix // hsMatrix44& hsMatrix44::Make(const hsPoint3* f, const hsPoint3* at, const hsVector3* up) { MakeTranslateMat(&hsVector3(f->fX, f->fY, f->fZ)); hsVector3 back (f,at); // Z back.Normalize(); hsVector3 leftEar = *up % back; // X, LHS leftEar.Normalize(); #if 1 // Ignore actual up vector hsVector3 topHead = leftEar % back; // Y, RHS (should be flipped) #else hsVector3 topHead = *up; #endif topHead.Normalize(); for(int i = 0; i < 3; i++) { fMap[i][0] = leftEar[i]; fMap[i][1] = back[i]; fMap[i][2] = topHead[i]; } NotIdentity(); return *this; } // // Not a camera matrix // hsMatrix44& hsMatrix44::MakeUpPreserving(const hsPoint3* f, const hsPoint3* at, const hsVector3* up) { MakeTranslateMat(&hsVector3(f->fX, f->fY, f->fZ)); hsVector3 topHead = *up; topHead.Normalize(); hsVector3 back (f,at); // Z back = -back; // really front hsVector3 leftEar = *up % back; // X leftEar.Normalize(); back = (topHead % leftEar); back.Normalize(); for(int i = 0; i < 3; i++) { fMap[i][0] = leftEar[i]; fMap[i][1] = back[i]; fMap[i][2] = topHead[i]; } NotIdentity(); return *this; } // // Get info from a non-camera matrix. Vectors are normalized. // void hsMatrix44::GetAxis(hsVector3* view, hsVector3 *up, hsVector3* right) { if (view) view->Set(-fMap[0][1],-fMap[1][1],-fMap[2][1]); if (right) right->Set(-fMap[0][0],-fMap[1][0],-fMap[2][0]); if (up) up->Set(fMap[0][2], fMap[1][2], fMap[2][2]); } const hsVector3 hsMatrix44::GetAxis(int i) const { hsVector3 ret; switch(i) { case kView: { ret.Set(-fMap[0][1],-fMap[1][1],-fMap[2][1]); break; } case kUp: { ret.Set(fMap[0][2], fMap[1][2], fMap[2][2]); break; } case kRight: { ret.Set(-fMap[0][0],-fMap[1][0],-fMap[2][0]); break; } } return ret; } // // Camera matrix // hsMatrix44& hsMatrix44::MakeCamera(const hsPoint3* from, const hsPoint3* at, const hsVector3* up) { hsVector3 dirZ(at, from); hsVector3 trans( -from->fX, -from->fY, -from->fZ ); hsVector3 dirY, dirX; hsMatrix44 rmat; dirX = (*up) % dirZ; // Stop passing in down!!! // mf_flip_up - mf if (dirX.MagnitudeSquared()) dirX.Normalize(); if (dirZ.MagnitudeSquared()) dirZ.Normalize(); dirY = dirZ % dirX; if (dirY.MagnitudeSquared()) dirY.Normalize(); this->Reset(); this->Translate(&trans); rmat.Reset(); for(int i = 0; i < 3; i++) { rmat.fMap[0][i] = -dirX[i]; rmat.fMap[1][i] = dirY[i]; rmat.fMap[2][i] = dirZ[i]; } rmat.NotIdentity(); *this = rmat * *this; return *this; } void hsMatrix44::MakeCameraMatrices(const hsPoint3& from, const hsPoint3& at, const hsVector3& up, hsMatrix44& worldToCamera, hsMatrix44& cameraToWorld) { hsVector3 dirZ(&at, &from); hsVector3 dirX(dirZ % up); dirX.Normalize(); hsVector3 dirY(dirX % dirZ); dirY.Normalize(); worldToCamera.Reset(false); cameraToWorld.Reset(false); int i; for( i = 0; i < 3; i++ ) { worldToCamera.fMap[0][i] = dirX[i]; worldToCamera.fMap[1][i] = dirY[i]; worldToCamera.fMap[2][i] = dirZ[i]; cameraToWorld.fMap[i][0] = dirX[i]; cameraToWorld.fMap[i][1] = dirY[i]; cameraToWorld.fMap[i][2] = dirZ[i]; } hsPoint3 trans = -from; worldToCamera.fMap[0][3] = dirX.InnerProduct(trans); worldToCamera.fMap[1][3] = dirY.InnerProduct(trans); worldToCamera.fMap[2][3] = dirZ.InnerProduct(trans); cameraToWorld.fMap[0][3] = from.fX; cameraToWorld.fMap[1][3] = from.fY; cameraToWorld.fMap[2][3] = from.fZ; } void hsMatrix44::MakeEnvMapMatrices(const hsPoint3& pos, hsMatrix44* worldToCameras, hsMatrix44* cameraToWorlds) { MakeCameraMatrices(pos, hsPoint3(pos.fX - 1.f, pos.fY, pos.fZ), hsVector3(0, 0, 1.f), worldToCameras[0], cameraToWorlds[0]); MakeCameraMatrices(pos, hsPoint3(pos.fX + 1.f, pos.fY, pos.fZ), hsVector3(0, 0, 1.f), worldToCameras[1], cameraToWorlds[1]); MakeCameraMatrices(pos, hsPoint3(pos.fX, pos.fY + 1.f, pos.fZ), hsVector3(0, 0, 1.f), worldToCameras[2], cameraToWorlds[2]); MakeCameraMatrices(pos, hsPoint3(pos.fX, pos.fY - 1.f, pos.fZ), hsVector3(0, 0, 1.f), worldToCameras[3], cameraToWorlds[3]); MakeCameraMatrices(pos, hsPoint3(pos.fX, pos.fY, pos.fZ + 1.f), hsVector3(0, -1.f, 0), worldToCameras[4], cameraToWorlds[4]); MakeCameraMatrices(pos, hsPoint3(pos.fX, pos.fY, pos.fZ - 1.f), hsVector3(0, 1.f, 0), worldToCameras[5], cameraToWorlds[5]); } // // Vectors are normalized. // void hsMatrix44::GetAxisFromCamera(hsVector3* view, hsVector3 *up, hsVector3* right) { if (view) view->Set(fMap[2][0],fMap[2][1],fMap[2][2]); if (right) right->Set(fMap[0][0],fMap[0][1],fMap[0][2]); if (up) up->Set(fMap[1][0], fMap[1][1], fMap[1][2]); } // // Camera matrix // hsMatrix44& hsMatrix44::MakeCameraUpPreserving(const hsPoint3* from, const hsPoint3* at, const hsVector3* up) { hsVector3 dirZ(at, from); hsVector3 trans( -from->fX, -from->fY, -from->fZ ); hsVector3 dirY( up->fX, up->fY, up->fZ ); hsVector3 dirX; hsMatrix44 rmat; dirX = dirY % dirZ; dirX.Normalize(); dirY.Normalize(); dirZ = dirX % dirY; dirZ.Normalize(); this->Reset(); this->Translate(&trans); rmat.Reset(); for(int i = 0; i < 3; i++) { rmat.fMap[0][i] = -dirX[i]; rmat.fMap[1][i] = dirY[i]; rmat.fMap[2][i] = dirZ[i]; } rmat.NotIdentity(); *this = rmat * *this; return *this; } /////////////////////////////////////////////////////// static hsScalar GetDeterminant33(const hsMatrix44* mat) { return hsScalarMul(hsScalarMul(mat->fMap[0][0], mat->fMap[1][1]), mat->fMap[2][2]) + hsScalarMul(hsScalarMul(mat->fMap[0][1], mat->fMap[1][2]), mat->fMap[2][0]) + hsScalarMul(hsScalarMul(mat->fMap[0][2], mat->fMap[1][0]), mat->fMap[2][1]) - hsScalarMul(hsScalarMul(mat->fMap[0][2], mat->fMap[1][1]), mat->fMap[2][0]) - hsScalarMul(hsScalarMul(mat->fMap[0][1], mat->fMap[1][0]), mat->fMap[2][2]) - hsScalarMul(hsScalarMul(mat->fMap[0][0], mat->fMap[1][2]), mat->fMap[2][1]); } hsMatrix44* hsMatrix44::GetTranspose(hsMatrix44* transp) const { for(int i = 0 ; i < 4; i++) for(int j=0; j < 4; j++) transp->fMap[i][j] = fMap[j][i]; return transp; } static inline hsScalar Determinant2(hsScalar a, hsScalar b,hsScalar c, hsScalar d) { return hsScalarMul(a,d) - hsScalarMul(c,b); } static inline hsScalar Determinant3(hsScalar a, hsScalar b, hsScalar c, hsScalar d, hsScalar e, hsScalar f, hsScalar g, hsScalar h, hsScalar i) { return hsScalarMul(a, Determinant2(e, f, h, i)) - hsScalarMul(b, Determinant2(d, f, g, i)) + hsScalarMul(c, Determinant2(d, e, g, h)); } hsScalar hsMatrix44::GetDeterminant() const { #if HS_BUILD_FOR_PS2 return (GetDeterminantVU0(fMap)); #else return (fMap[0][0]*Determinant3(fMap[1][1], fMap[2][1], fMap[3][1], fMap[1][2], fMap[2][2], fMap[3][2], fMap[1][3], fMap[2][3], fMap[3][3]) - fMap[1][0]*Determinant3(fMap[0][1], fMap[2][1], fMap[3][1], fMap[0][2], fMap[2][2], fMap[3][2], fMap[0][3], fMap[2][3], fMap[3][3]) + fMap[2][0]*Determinant3(fMap[0][1], fMap[1][1], fMap[3][1], fMap[0][2], fMap[1][2], fMap[3][2], fMap[0][3], fMap[1][3], fMap[3][3]) - fMap[3][0]*Determinant3(fMap[0][1], fMap[1][1], fMap[2][1], fMap[0][2], fMap[1][2], fMap[2][2], fMap[0][3], fMap[1][3], fMap[2][3])); #endif } hsMatrix44 *hsMatrix44::GetAdjoint(hsMatrix44 *adj) const { #if HS_BUILD_FOR_PS2 GetAdjointVU0(fMap, adj->fMap); #else float a1, a2, a3, a4, b1, b2, b3, b4; float c1, c2, c3, c4, d1, d2, d3, d4; /* * calculate the adjoint of a 4x4 matrix * * Let a denote the minor determinant of matrix A obtained by * ij * * deleting the ith row and jth column from A. * * i+j * Let b = (-1) a * ij ji * * The matrix B = (b ) is the adjoint of A * ij */ /* assign to individual variable names to aid */ /* selecting correct values */ a1 = fMap[0][0]; b1 = fMap[0][1]; c1 = fMap[0][2]; d1 = fMap[0][3]; a2 = fMap[1][0]; b2 = fMap[1][1]; c2 = fMap[1][2]; d2 = fMap[1][3]; a3 = fMap[2][0]; b3 = fMap[2][1]; c3 = fMap[2][2]; d3 = fMap[2][3]; a4 = fMap[3][0]; b4 = fMap[3][1]; c4 = fMap[3][2]; d4 = fMap[3][3]; /* * row column labeling reversed since we transpose rows & columns */ adj->fMap[0][0] = Determinant3(b2, b3, b4, c2, c3, c4, d2, d3, d4); adj->fMap[1][0] = -Determinant3(a2, a3, a4, c2, c3, c4, d2, d3, d4); adj->fMap[2][0] = Determinant3(a2, a3, a4, b2, b3, b4, d2, d3, d4); adj->fMap[3][0] = -Determinant3(a2, a3, a4, b2, b3, b4, c2, c3, c4); adj->fMap[0][1] = -Determinant3(b1, b3, b4, c1, c3, c4, d1, d3, d4); adj->fMap[1][1] = Determinant3(a1, a3, a4, c1, c3, c4, d1, d3, d4); adj->fMap[2][1] = -Determinant3(a1, a3, a4, b1, b3, b4, d1, d3, d4); adj->fMap[3][1] = Determinant3(a1, a3, a4, b1, b3, b4, c1, c3, c4); adj->fMap[0][2] = Determinant3(b1, b2, b4, c1, c2, c4, d1, d2, d4); adj->fMap[1][2] = -Determinant3(a1, a2, a4, c1, c2, c4, d1, d2, d4); adj->fMap[2][2] = Determinant3(a1, a2, a4, b1, b2, b4, d1, d2, d4); adj->fMap[3][2] = -Determinant3(a1, a2, a4, b1, b2, b4, c1, c2, c4); adj->fMap[0][3] = -Determinant3(b1, b2, b3, c1, c2, c3, d1, d2, d3); adj->fMap[1][3] = Determinant3(a1, a2, a3, c1, c2, c3, d1, d2, d3); adj->fMap[2][3] = -Determinant3(a1, a2, a3, b1, b2, b3, d1, d2, d3); adj->fMap[3][3] = Determinant3(a1, a2, a3, b1, b2, b3, c1, c2, c3); #endif adj->NotIdentity(); return adj; } hsMatrix44* hsMatrix44::GetInverse(hsMatrix44* inverse) const { hsScalar det = GetDeterminant(); int i,j; if (det == 0.0f) { inverse->Reset(); return inverse; } det = hsScalarInvert(det); GetAdjoint(inverse); #if HS_BUILD_FOR_PS2 MatMulVU0(inverse->fMap, det); #else for (i=0; i<4; i++) for (j=0; j<4; j++) inverse->fMap[i][j] *= det; #endif return inverse; } hsMatrix44& hsMatrix44::SetScale(const hsVector3* pt) { for (int i =0; i < 3; i++) { fMap[i][i] = (*pt)[i]; } NotIdentity(); return *this; } hsMatrix44& hsMatrix44::MakeScaleMat(const hsVector3* pt) { Reset(); SetScale(pt); return *this; } hsMatrix44 &hsMatrix44::MakeTranslateMat(const hsVector3 *trans) { Reset(); SetTranslate(trans); return *this; } hsVector3* hsMatrix44::GetTranslate(hsVector3 *pt) const { for (int i =0; i < 3; i++) { (*pt)[i] = fMap[i][3]; } return pt; } const hsPoint3 hsMatrix44::GetTranslate() const { hsPoint3 pt; for (int i =0; i < 3; i++) { (pt)[i] = fMap[i][3]; } return pt; } hsPoint3* hsMatrix44::MapPoints(long count, hsPoint3 points[]) const { if( !(fFlags & hsMatrix44::kIsIdent) ) { int i; for(i = 0; i < count; i++) { points[i] = *this * points[i]; } } return points; } hsBool hsMatrix44::IsIdentity(void) { hsBool retVal = true; int i, j; for( i = 0; i < 4; i++ ) { for( j = 0; j < 4; j++ ) { #if 0 // IDENTITY_CRISIS if( i == j) { if (fMap[i][j] != hsScalar1) { NotIdentity(); retVal = false; } } else { if( fMap[i][j] != 0 ) { NotIdentity(); retVal = false; } } #else // IDENTITY_CRISIS const hsScalar kEPS = 1.e-5f; if( i == j) { if( (fMap[i][j] < hsScalar1-kEPS) || (fMap[i][j] > hsScalar1+kEPS) ) { NotIdentity(); retVal = false; } else { fMap[i][j] = hsScalar1; } } else { if( (fMap[i][j] < -kEPS) || (fMap[i][j] > kEPS) ) { NotIdentity(); retVal = false; } else { fMap[i][j] = 0; } } #endif // IDENTITY_CRISIS } } if( retVal ) fFlags |= kIsIdent; return retVal; } hsBool hsMatrix44::GetParity() const { if( fFlags & kIsIdent ) return false; hsVector3* rows[3]; rows[0] = (hsVector3*)&fMap[0][0]; rows[1] = (hsVector3*)&fMap[1][0]; rows[2] = (hsVector3*)&fMap[2][0]; hsVector3 zeroXone = *rows[0] % *rows[1]; return zeroXone.InnerProduct(rows[2]) < 0; } void hsMatrix44::Read(hsStream *stream) { if (stream->ReadBool()) { int i,j; for(i=0; i<4; i++) for(j=0; j<4; j++) #if HS_SCALAR_IS_FIXED fMap[i][j] = stream->ReadSwap32(); #endif #if HS_SCALAR_IS_FLOAT fMap[i][j] = stream->ReadSwapFloat(); #endif IsIdentity(); } else Reset(); } void hsMatrix44::Write(hsStream *stream) { hsBool ident = IsIdentity(); stream->WriteBool(!ident); if (!ident) { int i,j; for(i=0; i<4; i++) for(j=0; j<4; j++) #if HS_SCALAR_IS_FIXED stream->WriteSwap32(fMap[i][j]); #endif #if HS_SCALAR_IS_FLOAT stream->WriteSwapFloat(fMap[i][j]); #endif } }