mMatrix.cpp
Engine/source/math/mMatrix.cpp
Detailed Description
1 2//----------------------------------------------------------------------------- 3// Copyright (c) 2012 GarageGames, LLC 4// 5// Permission is hereby granted, free of charge, to any person obtaining a copy 6// of this software and associated documentation files (the "Software"), to 7// deal in the Software without restriction, including without limitation the 8// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9// sell copies of the Software, and to permit persons to whom the Software is 10// furnished to do so, subject to the following conditions: 11// 12// The above copyright notice and this permission notice shall be included in 13// all copies or substantial portions of the Software. 14// 15// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21// IN THE SOFTWARE. 22//----------------------------------------------------------------------------- 23 24#include "core/strings/stringFunctions.h" 25#include "core/frameAllocator.h" 26 27#include "math/mMatrix.h" 28#include "console/console.h" 29 30#include "console/enginePrimitives.h" 31#include "console/engineTypes.h" 32 33const MatrixF MatrixF::Identity( true ); 34 35// idx(i,j) is index to element in column i, row j 36 37void MatrixF::transposeTo(F32 *matrix) const 38{ 39 matrix[idx(0,0)] = m[idx(0,0)]; 40 matrix[idx(0,1)] = m[idx(1,0)]; 41 matrix[idx(0,2)] = m[idx(2,0)]; 42 matrix[idx(0,3)] = m[idx(3,0)]; 43 matrix[idx(1,0)] = m[idx(0,1)]; 44 matrix[idx(1,1)] = m[idx(1,1)]; 45 matrix[idx(1,2)] = m[idx(2,1)]; 46 matrix[idx(1,3)] = m[idx(3,1)]; 47 matrix[idx(2,0)] = m[idx(0,2)]; 48 matrix[idx(2,1)] = m[idx(1,2)]; 49 matrix[idx(2,2)] = m[idx(2,2)]; 50 matrix[idx(2,3)] = m[idx(3,2)]; 51 matrix[idx(3,0)] = m[idx(0,3)]; 52 matrix[idx(3,1)] = m[idx(1,3)]; 53 matrix[idx(3,2)] = m[idx(2,3)]; 54 matrix[idx(3,3)] = m[idx(3,3)]; 55} 56 57bool MatrixF::isAffine() const 58{ 59 // An affine transform is defined by the following structure 60 // 61 // [ X X X P ] 62 // [ X X X P ] 63 // [ X X X P ] 64 // [ 0 0 0 1 ] 65 // 66 // Where X is an orthonormal 3x3 submatrix and P is an arbitrary translation 67 // We'll check in the following order: 68 // 1: [3][3] must be 1 69 // 2: Shear portion must be zero 70 // 3: Dot products of rows and columns must be zero 71 // 4: Length of rows and columns must be 1 72 // 73 if (m[idx(3,3)] != 1.0f) 74 return false; 75 76 if (m[idx(0,3)] != 0.0f || 77 m[idx(1,3)] != 0.0f || 78 m[idx(2,3)] != 0.0f) 79 return false; 80 81 Point3F one, two, three; 82 getColumn(0, &one); 83 getColumn(1, &two); 84 getColumn(2, &three); 85 if (mDot(one, two) > 0.0001f || 86 mDot(one, three) > 0.0001f || 87 mDot(two, three) > 0.0001f) 88 return false; 89 90 if (mFabs(1.0f - one.lenSquared()) > 0.0001f || 91 mFabs(1.0f - two.lenSquared()) > 0.0001f || 92 mFabs(1.0f - three.lenSquared()) > 0.0001f) 93 return false; 94 95 getRow(0, &one); 96 getRow(1, &two); 97 getRow(2, &three); 98 if (mDot(one, two) > 0.0001f || 99 mDot(one, three) > 0.0001f || 100 mDot(two, three) > 0.0001f) 101 return false; 102 103 if (mFabs(1.0f - one.lenSquared()) > 0.0001f || 104 mFabs(1.0f - two.lenSquared()) > 0.0001f || 105 mFabs(1.0f - three.lenSquared()) > 0.0001f) 106 return false; 107 108 // We're ok. 109 return true; 110} 111 112// Perform inverse on full 4x4 matrix. Used in special cases only, so not at all optimized. 113bool MatrixF::fullInverse() 114{ 115 Point4F a,b,c,d; 116 getRow(0,&a); 117 getRow(1,&b); 118 getRow(2,&c); 119 getRow(3,&d); 120 121 // det = a0*b1*c2*d3 - a0*b1*c3*d2 - a0*c1*b2*d3 + a0*c1*b3*d2 + a0*d1*b2*c3 - a0*d1*b3*c2 - 122 // b0*a1*c2*d3 + b0*a1*c3*d2 + b0*c1*a2*d3 - b0*c1*a3*d2 - b0*d1*a2*c3 + b0*d1*a3*c2 + 123 // c0*a1*b2*d3 - c0*a1*b3*d2 - c0*b1*a2*d3 + c0*b1*a3*d2 + c0*d1*a2*b3 - c0*d1*a3*b2 - 124 // d0*a1*b2*c3 + d0*a1*b3*c2 + d0*b1*a2*c3 - d0*b1*a3*c2 - d0*c1*a2*b3 + d0*c1*a3*b2 125 F32 det = a.x*b.y*c.z*d.w - a.x*b.y*c.w*d.z - a.x*c.y*b.z*d.w + a.x*c.y*b.w*d.z + a.x*d.y*b.z*c.w - a.x*d.y*b.w*c.z 126 - b.x*a.y*c.z*d.w + b.x*a.y*c.w*d.z + b.x*c.y*a.z*d.w - b.x*c.y*a.w*d.z - b.x*d.y*a.z*c.w + b.x*d.y*a.w*c.z 127 + c.x*a.y*b.z*d.w - c.x*a.y*b.w*d.z - c.x*b.y*a.z*d.w + c.x*b.y*a.w*d.z + c.x*d.y*a.z*b.w - c.x*d.y*a.w*b.z 128 - d.x*a.y*b.z*c.w + d.x*a.y*b.w*c.z + d.x*b.y*a.z*c.w - d.x*b.y*a.w*c.z - d.x*c.y*a.z*b.w + d.x*c.y*a.w*b.z; 129 130 if (mFabs(det)<0.00001f) 131 return false; 132 133 Point4F aa,bb,cc,dd; 134 aa.x = b.y*c.z*d.w - b.y*c.w*d.z - c.y*b.z*d.w + c.y*b.w*d.z + d.y*b.z*c.w - d.y*b.w*c.z; 135 aa.y = -a.y*c.z*d.w + a.y*c.w*d.z + c.y*a.z*d.w - c.y*a.w*d.z - d.y*a.z*c.w + d.y*a.w*c.z; 136 aa.z = a.y*b.z*d.w - a.y*b.w*d.z - b.y*a.z*d.w + b.y*a.w*d.z + d.y*a.z*b.w - d.y*a.w*b.z; 137 aa.w = -a.y*b.z*c.w + a.y*b.w*c.z + b.y*a.z*c.w - b.y*a.w*c.z - c.y*a.z*b.w + c.y*a.w*b.z; 138 139 bb.x = -b.x*c.z*d.w + b.x*c.w*d.z + c.x*b.z*d.w - c.x*b.w*d.z - d.x*b.z*c.w + d.x*b.w*c.z; 140 bb.y = a.x*c.z*d.w - a.x*c.w*d.z - c.x*a.z*d.w + c.x*a.w*d.z + d.x*a.z*c.w - d.x*a.w*c.z; 141 bb.z = -a.x*b.z*d.w + a.x*b.w*d.z + b.x*a.z*d.w - b.x*a.w*d.z - d.x*a.z*b.w + d.x*a.w*b.z; 142 bb.w = a.x*b.z*c.w - a.x*b.w*c.z - b.x*a.z*c.w + b.x*a.w*c.z + c.x*a.z*b.w - c.x*a.w*b.z; 143 144 cc.x = b.x*c.y*d.w - b.x*c.w*d.y - c.x*b.y*d.w + c.x*b.w*d.y + d.x*b.y*c.w - d.x*b.w*c.y; 145 cc.y = -a.x*c.y*d.w + a.x*c.w*d.y + c.x*a.y*d.w - c.x*a.w*d.y - d.x*a.y*c.w + d.x*a.w*c.y; 146 cc.z = a.x*b.y*d.w - a.x*b.w*d.y - b.x*a.y*d.w + b.x*a.w*d.y + d.x*a.y*b.w - d.x*a.w*b.y; 147 cc.w = -a.x*b.y*c.w + a.x*b.w*c.y + b.x*a.y*c.w - b.x*a.w*c.y - c.x*a.y*b.w + c.x*a.w*b.y; 148 149 dd.x = -b.x*c.y*d.z + b.x*c.z*d.y + c.x*b.y*d.z - c.x*b.z*d.y - d.x*b.y*c.z + d.x*b.z*c.y; 150 dd.y = a.x*c.y*d.z - a.x*c.z*d.y - c.x*a.y*d.z + c.x*a.z*d.y + d.x*a.y*c.z - d.x*a.z*c.y; 151 dd.z = -a.x*b.y*d.z + a.x*b.z*d.y + b.x*a.y*d.z - b.x*a.z*d.y - d.x*a.y*b.z + d.x*a.z*b.y; 152 dd.w = a.x*b.y*c.z - a.x*b.z*c.y - b.x*a.y*c.z + b.x*a.z*c.y + c.x*a.y*b.z - c.x*a.z*b.y; 153 154 setRow(0,aa); 155 setRow(1,bb); 156 setRow(2,cc); 157 setRow(3,dd); 158 159 mul(1.0f/det); 160 161 return true; 162} 163 164EulerF MatrixF::toEuler() const 165{ 166 const F32 * mat = m; 167 168 EulerF r; 169 r.x = mAsin(mClampF(mat[MatrixF::idx(2,1)], -1.0, 1.0)); 170 171 if(mCos(r.x) != 0.f) 172 { 173 r.y = mAtan2(-mat[MatrixF::idx(2,0)], mat[MatrixF::idx(2,2)]); 174 r.z = mAtan2(-mat[MatrixF::idx(0,1)], mat[MatrixF::idx(1,1)]); 175 } 176 else 177 { 178 r.y = 0.f; 179 r.z = mAtan2(mat[MatrixF::idx(1,0)], mat[MatrixF::idx(0,0)]); 180 } 181 182 return r; 183} 184 185void MatrixF::dumpMatrix(const char *caption /* =NULL */) const 186{ 187 U32 size = dStrlen(caption); 188 FrameTemp<char> spacer(size+1); 189 char *spacerRef = spacer; 190 191 dMemset(spacerRef, ' ', size); 192 spacerRef[size] = 0; 193 194 Con::printf("%s = | %-8.4f %-8.4f %-8.4f %-8.4f |", caption, m[idx(0,0)], m[idx(0, 1)], m[idx(0, 2)], m[idx(0, 3)]); 195 Con::printf("%s | %-8.4f %-8.4f %-8.4f %-8.4f |", spacerRef, m[idx(1,0)], m[idx(1, 1)], m[idx(1, 2)], m[idx(1, 3)]); 196 Con::printf("%s | %-8.4f %-8.4f %-8.4f %-8.4f |", spacerRef, m[idx(2,0)], m[idx(2, 1)], m[idx(2, 2)], m[idx(2, 3)]); 197 Con::printf("%s | %-8.4f %-8.4f %-8.4f %-8.4f |", spacerRef, m[idx(3,0)], m[idx(3, 1)], m[idx(3, 2)], m[idx(3, 3)]); 198} 199 200EngineFieldTable::Field MatrixFEngineExport::getMatrixField() 201{ 202 typedef MatrixF ThisType; 203 return _FIELD_AS(F32, m, m, 16, ""); 204} 205