1 /****************************************************************************** 2 3 @File PVRTVector.cpp 4 5 @Title PVRTVector 6 7 @Version 8 9 @Copyright Copyright (c) Imagination Technologies Limited. 10 11 @Platform ANSI compatible 12 13 @Description Vector and matrix mathematics library 14 15 ******************************************************************************/ 16 17 #include "PVRTVector.h" 18 19 #include <math.h> 20 21 /*!*************************************************************************** 22 ** PVRTVec2 2 component vector 23 ****************************************************************************/ 24 25 /*!*************************************************************************** 26 @Function PVRTVec2 27 @Input v3Vec a Vec3 28 @Description Constructor from a Vec3 29 *****************************************************************************/ 30 PVRTVec2::PVRTVec2(const PVRTVec3& vec3) 31 { 32 x = vec3.x; y = vec3.y; 33 } 34 35 /*!*************************************************************************** 36 ** PVRTVec3 3 component vector 37 ****************************************************************************/ 38 39 /*!*************************************************************************** 40 @Function PVRTVec3 41 @Input v4Vec a PVRTVec4 42 @Description Constructor from a PVRTVec4 43 *****************************************************************************/ 44 PVRTVec3::PVRTVec3(const PVRTVec4& vec4) 45 { 46 x = vec4.x; y = vec4.y; z = vec4.z; 47 } 48 49 /*!*************************************************************************** 50 @Function * 51 @Input rhs a PVRTMat3 52 @Returns result of multiplication 53 @Description matrix multiplication operator PVRTVec3 and PVRTMat3 54 ****************************************************************************/ 55 PVRTVec3 PVRTVec3::operator*(const PVRTMat3& rhs) const 56 { 57 PVRTVec3 out; 58 59 out.x = VERTTYPEMUL(x,rhs.f[0])+VERTTYPEMUL(y,rhs.f[1])+VERTTYPEMUL(z,rhs.f[2]); 60 out.y = VERTTYPEMUL(x,rhs.f[3])+VERTTYPEMUL(y,rhs.f[4])+VERTTYPEMUL(z,rhs.f[5]); 61 out.z = VERTTYPEMUL(x,rhs.f[6])+VERTTYPEMUL(y,rhs.f[7])+VERTTYPEMUL(z,rhs.f[8]); 62 63 return out; 64 } 65 66 /*!*************************************************************************** 67 @Function *= 68 @Input rhs a PVRTMat3 69 @Returns result of multiplication and assignment 70 @Description matrix multiplication and assignment operator for PVRTVec3 and PVRTMat3 71 ****************************************************************************/ 72 PVRTVec3& PVRTVec3::operator*=(const PVRTMat3& rhs) 73 { 74 VERTTYPE tx = VERTTYPEMUL(x,rhs.f[0])+VERTTYPEMUL(y,rhs.f[1])+VERTTYPEMUL(z,rhs.f[2]); 75 VERTTYPE ty = VERTTYPEMUL(x,rhs.f[3])+VERTTYPEMUL(y,rhs.f[4])+VERTTYPEMUL(z,rhs.f[5]); 76 z = VERTTYPEMUL(x,rhs.f[6])+VERTTYPEMUL(y,rhs.f[7])+VERTTYPEMUL(z,rhs.f[8]); 77 x = tx; 78 y = ty; 79 80 return *this; 81 } 82 83 /*!*************************************************************************** 84 ** PVRTVec4 4 component vector 85 ****************************************************************************/ 86 87 /*!*************************************************************************** 88 @Function * 89 @Input rhs a PVRTMat4 90 @Returns result of multiplication 91 @Description matrix multiplication operator PVRTVec4 and PVRTMat4 92 ****************************************************************************/ 93 PVRTVec4 PVRTVec4::operator*(const PVRTMat4& rhs) const 94 { 95 PVRTVec4 out; 96 out.x = VERTTYPEMUL(x,rhs.f[0])+VERTTYPEMUL(y,rhs.f[1])+VERTTYPEMUL(z,rhs.f[2])+VERTTYPEMUL(w,rhs.f[3]); 97 out.y = VERTTYPEMUL(x,rhs.f[4])+VERTTYPEMUL(y,rhs.f[5])+VERTTYPEMUL(z,rhs.f[6])+VERTTYPEMUL(w,rhs.f[7]); 98 out.z = VERTTYPEMUL(x,rhs.f[8])+VERTTYPEMUL(y,rhs.f[9])+VERTTYPEMUL(z,rhs.f[10])+VERTTYPEMUL(w,rhs.f[11]); 99 out.w = VERTTYPEMUL(x,rhs.f[12])+VERTTYPEMUL(y,rhs.f[13])+VERTTYPEMUL(z,rhs.f[14])+VERTTYPEMUL(w,rhs.f[15]); 100 return out; 101 } 102 103 /*!*************************************************************************** 104 @Function *= 105 @Input rhs a PVRTMat4 106 @Returns result of multiplication and assignment 107 @Description matrix multiplication and assignment operator for PVRTVec4 and PVRTMat4 108 ****************************************************************************/ 109 PVRTVec4& PVRTVec4::operator*=(const PVRTMat4& rhs) 110 { 111 VERTTYPE tx = VERTTYPEMUL(x,rhs.f[0])+VERTTYPEMUL(y,rhs.f[1])+VERTTYPEMUL(z,rhs.f[2])+VERTTYPEMUL(w,rhs.f[3]); 112 VERTTYPE ty = VERTTYPEMUL(x,rhs.f[4])+VERTTYPEMUL(y,rhs.f[5])+VERTTYPEMUL(z,rhs.f[6])+VERTTYPEMUL(w,rhs.f[7]); 113 VERTTYPE tz = VERTTYPEMUL(x,rhs.f[8])+VERTTYPEMUL(y,rhs.f[9])+VERTTYPEMUL(z,rhs.f[10])+VERTTYPEMUL(w,rhs.f[11]); 114 w = VERTTYPEMUL(x,rhs.f[12])+VERTTYPEMUL(y,rhs.f[13])+VERTTYPEMUL(z,rhs.f[14])+VERTTYPEMUL(w,rhs.f[15]); 115 x = tx; 116 y = ty; 117 z = tz; 118 return *this; 119 } 120 121 /*!*************************************************************************** 122 ** PVRTMat3 3x3 matrix 123 ****************************************************************************/ 124 /*!*************************************************************************** 125 @Function PVRTMat3 126 @Input mat a PVRTMat4 127 @Description constructor to form a PVRTMat3 from a PVRTMat4 128 ****************************************************************************/ 129 PVRTMat3::PVRTMat3(const PVRTMat4& mat) 130 { 131 VERTTYPE *dest = (VERTTYPE*)f, *src = (VERTTYPE*)mat.f; 132 for(int i=0;i<3;i++) 133 { 134 for(int j=0;j<3;j++) 135 { 136 (*dest++) = (*src++); 137 } 138 src++; 139 } 140 } 141 142 /*!*************************************************************************** 143 @Function RotationX 144 @Input angle the angle of rotation 145 @Returns rotation matrix 146 @Description generates a 3x3 rotation matrix about the X axis 147 ****************************************************************************/ 148 PVRTMat3 PVRTMat3::RotationX(VERTTYPE angle) 149 { 150 PVRTMat4 out; 151 PVRTMatrixRotationX(out,angle); 152 return PVRTMat3(out); 153 } 154 /*!*************************************************************************** 155 @Function RotationY 156 @Input angle the angle of rotation 157 @Returns rotation matrix 158 @Description generates a 3x3 rotation matrix about the Y axis 159 ****************************************************************************/ 160 PVRTMat3 PVRTMat3::RotationY(VERTTYPE angle) 161 { 162 PVRTMat4 out; 163 PVRTMatrixRotationY(out,angle); 164 return PVRTMat3(out); 165 } 166 /*!*************************************************************************** 167 @Function RotationZ 168 @Input angle the angle of rotation 169 @Returns rotation matrix 170 @Description generates a 3x3 rotation matrix about the Z axis 171 ****************************************************************************/ 172 PVRTMat3 PVRTMat3::RotationZ(VERTTYPE angle) 173 { 174 PVRTMat4 out; 175 PVRTMatrixRotationZ(out,angle); 176 return PVRTMat3(out); 177 } 178 179 180 /*!*************************************************************************** 181 ** PVRTMat4 4x4 matrix 182 ****************************************************************************/ 183 /*!*************************************************************************** 184 @Function RotationX 185 @Input angle the angle of rotation 186 @Returns rotation matrix 187 @Description generates a 4x4 rotation matrix about the X axis 188 ****************************************************************************/ 189 PVRTMat4 PVRTMat4::RotationX(VERTTYPE angle) 190 { 191 PVRTMat4 out; 192 PVRTMatrixRotationX(out,angle); 193 return out; 194 } 195 /*!*************************************************************************** 196 @Function RotationY 197 @Input angle the angle of rotation 198 @Returns rotation matrix 199 @Description generates a 4x4 rotation matrix about the Y axis 200 ****************************************************************************/ 201 PVRTMat4 PVRTMat4::RotationY(VERTTYPE angle) 202 { 203 PVRTMat4 out; 204 PVRTMatrixRotationY(out,angle); 205 return out; 206 } 207 /*!*************************************************************************** 208 @Function RotationZ 209 @Input angle the angle of rotation 210 @Returns rotation matrix 211 @Description generates a 4x4 rotation matrix about the Z axis 212 ****************************************************************************/ 213 PVRTMat4 PVRTMat4::RotationZ(VERTTYPE angle) 214 { 215 PVRTMat4 out; 216 PVRTMatrixRotationZ(out,angle); 217 return out; 218 } 219 220 /*!*************************************************************************** 221 @Function * 222 @Input rhs another PVRTMat4 223 @Returns result of multiplication 224 @Description Matrix multiplication of two 4x4 matrices. 225 *****************************************************************************/ 226 PVRTMat4 PVRTMat4::operator*(const PVRTMat4& rhs) const 227 { 228 PVRTMat4 out; 229 // col 1 230 out.f[0] = VERTTYPEMUL(f[0],rhs.f[0])+VERTTYPEMUL(f[4],rhs.f[1])+VERTTYPEMUL(f[8],rhs.f[2])+VERTTYPEMUL(f[12],rhs.f[3]); 231 out.f[1] = VERTTYPEMUL(f[1],rhs.f[0])+VERTTYPEMUL(f[5],rhs.f[1])+VERTTYPEMUL(f[9],rhs.f[2])+VERTTYPEMUL(f[13],rhs.f[3]); 232 out.f[2] = VERTTYPEMUL(f[2],rhs.f[0])+VERTTYPEMUL(f[6],rhs.f[1])+VERTTYPEMUL(f[10],rhs.f[2])+VERTTYPEMUL(f[14],rhs.f[3]); 233 out.f[3] = VERTTYPEMUL(f[3],rhs.f[0])+VERTTYPEMUL(f[7],rhs.f[1])+VERTTYPEMUL(f[11],rhs.f[2])+VERTTYPEMUL(f[15],rhs.f[3]); 234 235 // col 2 236 out.f[4] = VERTTYPEMUL(f[0],rhs.f[4])+VERTTYPEMUL(f[4],rhs.f[5])+VERTTYPEMUL(f[8],rhs.f[6])+VERTTYPEMUL(f[12],rhs.f[7]); 237 out.f[5] = VERTTYPEMUL(f[1],rhs.f[4])+VERTTYPEMUL(f[5],rhs.f[5])+VERTTYPEMUL(f[9],rhs.f[6])+VERTTYPEMUL(f[13],rhs.f[7]); 238 out.f[6] = VERTTYPEMUL(f[2],rhs.f[4])+VERTTYPEMUL(f[6],rhs.f[5])+VERTTYPEMUL(f[10],rhs.f[6])+VERTTYPEMUL(f[14],rhs.f[7]); 239 out.f[7] = VERTTYPEMUL(f[3],rhs.f[4])+VERTTYPEMUL(f[7],rhs.f[5])+VERTTYPEMUL(f[11],rhs.f[6])+VERTTYPEMUL(f[15],rhs.f[7]); 240 241 // col3 242 out.f[8] = VERTTYPEMUL(f[0],rhs.f[8])+VERTTYPEMUL(f[4],rhs.f[9])+VERTTYPEMUL(f[8],rhs.f[10])+VERTTYPEMUL(f[12],rhs.f[11]); 243 out.f[9] = VERTTYPEMUL(f[1],rhs.f[8])+VERTTYPEMUL(f[5],rhs.f[9])+VERTTYPEMUL(f[9],rhs.f[10])+VERTTYPEMUL(f[13],rhs.f[11]); 244 out.f[10] = VERTTYPEMUL(f[2],rhs.f[8])+VERTTYPEMUL(f[6],rhs.f[9])+VERTTYPEMUL(f[10],rhs.f[10])+VERTTYPEMUL(f[14],rhs.f[11]); 245 out.f[11] = VERTTYPEMUL(f[3],rhs.f[8])+VERTTYPEMUL(f[7],rhs.f[9])+VERTTYPEMUL(f[11],rhs.f[10])+VERTTYPEMUL(f[15],rhs.f[11]); 246 247 // col3 248 out.f[12] = VERTTYPEMUL(f[0],rhs.f[12])+VERTTYPEMUL(f[4],rhs.f[13])+VERTTYPEMUL(f[8],rhs.f[14])+VERTTYPEMUL(f[12],rhs.f[15]); 249 out.f[13] = VERTTYPEMUL(f[1],rhs.f[12])+VERTTYPEMUL(f[5],rhs.f[13])+VERTTYPEMUL(f[9],rhs.f[14])+VERTTYPEMUL(f[13],rhs.f[15]); 250 out.f[14] = VERTTYPEMUL(f[2],rhs.f[12])+VERTTYPEMUL(f[6],rhs.f[13])+VERTTYPEMUL(f[10],rhs.f[14])+VERTTYPEMUL(f[14],rhs.f[15]); 251 out.f[15] = VERTTYPEMUL(f[3],rhs.f[12])+VERTTYPEMUL(f[7],rhs.f[13])+VERTTYPEMUL(f[11],rhs.f[14])+VERTTYPEMUL(f[15],rhs.f[15]); 252 return out; 253 } 254 255 256 /*!*************************************************************************** 257 @Function inverse 258 @Returns inverse mat4 259 @Description Calculates multiplicative inverse of this matrix 260 The matrix must be of the form : 261 A 0 262 C 1 263 Where A is a 3x3 matrix and C is a 1x3 matrix. 264 *****************************************************************************/ 265 PVRTMat4 PVRTMat4::inverse() const 266 { 267 PVRTMat4 out; 268 VERTTYPE det_1; 269 VERTTYPE pos, neg, temp; 270 271 /* Calculate the determinant of submatrix A and determine if the 272 the matrix is singular as limited by the double precision 273 floating-point data representation. */ 274 pos = neg = f2vt(0.0); 275 temp = VERTTYPEMUL(VERTTYPEMUL(f[ 0], f[ 5]), f[10]); 276 if (temp >= 0) pos += temp; else neg += temp; 277 temp = VERTTYPEMUL(VERTTYPEMUL(f[ 4], f[ 9]), f[ 2]); 278 if (temp >= 0) pos += temp; else neg += temp; 279 temp = VERTTYPEMUL(VERTTYPEMUL(f[ 8], f[ 1]), f[ 6]); 280 if (temp >= 0) pos += temp; else neg += temp; 281 temp = VERTTYPEMUL(VERTTYPEMUL(-f[ 8], f[ 5]), f[ 2]); 282 if (temp >= 0) pos += temp; else neg += temp; 283 temp = VERTTYPEMUL(VERTTYPEMUL(-f[ 4], f[ 1]), f[10]); 284 if (temp >= 0) pos += temp; else neg += temp; 285 temp = VERTTYPEMUL(VERTTYPEMUL(-f[ 0], f[ 9]), f[ 6]); 286 if (temp >= 0) pos += temp; else neg += temp; 287 det_1 = pos + neg; 288 289 /* Is the submatrix A singular? */ 290 if (det_1 == f2vt(0.0)) //|| (VERTTYPEABS(det_1 / (pos - neg)) < 1.0e-15) 291 { 292 /* Matrix M has no inverse */ 293 _RPT0(_CRT_WARN, "Matrix has no inverse : singular matrix\n"); 294 } 295 else 296 { 297 /* Calculate inverse(A) = adj(A) / det(A) */ 298 //det_1 = 1.0 / det_1; 299 det_1 = VERTTYPEDIV(f2vt(1.0f), det_1); 300 out.f[ 0] = VERTTYPEMUL(( VERTTYPEMUL(f[ 5], f[10]) - VERTTYPEMUL(f[ 9], f[ 6]) ), det_1); 301 out.f[ 1] = - VERTTYPEMUL(( VERTTYPEMUL(f[ 1], f[10]) - VERTTYPEMUL(f[ 9], f[ 2]) ), det_1); 302 out.f[ 2] = VERTTYPEMUL(( VERTTYPEMUL(f[ 1], f[ 6]) - VERTTYPEMUL(f[ 5], f[ 2]) ), det_1); 303 out.f[ 4] = - VERTTYPEMUL(( VERTTYPEMUL(f[ 4], f[10]) - VERTTYPEMUL(f[ 8], f[ 6]) ), det_1); 304 out.f[ 5] = VERTTYPEMUL(( VERTTYPEMUL(f[ 0], f[10]) - VERTTYPEMUL(f[ 8], f[ 2]) ), det_1); 305 out.f[ 6] = - VERTTYPEMUL(( VERTTYPEMUL(f[ 0], f[ 6]) - VERTTYPEMUL(f[ 4], f[ 2]) ), det_1); 306 out.f[ 8] = VERTTYPEMUL(( VERTTYPEMUL(f[ 4], f[ 9]) - VERTTYPEMUL(f[ 8], f[ 5]) ), det_1); 307 out.f[ 9] = - VERTTYPEMUL(( VERTTYPEMUL(f[ 0], f[ 9]) - VERTTYPEMUL(f[ 8], f[ 1]) ), det_1); 308 out.f[10] = VERTTYPEMUL(( VERTTYPEMUL(f[ 0], f[ 5]) - VERTTYPEMUL(f[ 4], f[ 1]) ), det_1); 309 310 /* Calculate -C * inverse(A) */ 311 out.f[12] = - ( VERTTYPEMUL(f[12], out.f[ 0]) + VERTTYPEMUL(f[13], out.f[ 4]) + VERTTYPEMUL(f[14], out.f[ 8]) ); 312 out.f[13] = - ( VERTTYPEMUL(f[12], out.f[ 1]) + VERTTYPEMUL(f[13], out.f[ 5]) + VERTTYPEMUL(f[14], out.f[ 9]) ); 313 out.f[14] = - ( VERTTYPEMUL(f[12], out.f[ 2]) + VERTTYPEMUL(f[13], out.f[ 6]) + VERTTYPEMUL(f[14], out.f[10]) ); 314 315 /* Fill in last row */ 316 out.f[ 3] = f2vt(0.0f); 317 out.f[ 7] = f2vt(0.0f); 318 out.f[11] = f2vt(0.0f); 319 out.f[15] = f2vt(1.0f); 320 } 321 322 return out; 323 } 324 325 /*!*************************************************************************** 326 @Function PVRTLinearEqSolve 327 @Input pSrc 2D array of floats. 4 Eq linear problem is 5x4 328 matrix, constants in first column 329 @Input nCnt Number of equations to solve 330 @Output pRes Result 331 @Description Solves 'nCnt' simultaneous equations of 'nCnt' variables. 332 pRes should be an array large enough to contain the 333 results: the values of the 'nCnt' variables. 334 This fn recursively uses Gaussian Elimination. 335 *****************************************************************************/ 336 void PVRTLinearEqSolve(VERTTYPE * const pRes, VERTTYPE ** const pSrc, const int nCnt) 337 { 338 int i, j, k; 339 VERTTYPE f; 340 341 if (nCnt == 1) 342 { 343 _ASSERT(pSrc[0][1] != 0); 344 pRes[0] = VERTTYPEDIV(pSrc[0][0], pSrc[0][1]); 345 return; 346 } 347 348 // Loop backwards in an attempt avoid the need to swap rows 349 i = nCnt; 350 while(i) 351 { 352 --i; 353 354 if(pSrc[i][nCnt] != f2vt(0.0f)) 355 { 356 // Row i can be used to zero the other rows; let's move it to the bottom 357 if(i != (nCnt-1)) 358 { 359 for(j = 0; j <= nCnt; ++j) 360 { 361 // Swap the two values 362 f = pSrc[nCnt-1][j]; 363 pSrc[nCnt-1][j] = pSrc[i][j]; 364 pSrc[i][j] = f; 365 } 366 } 367 368 // Now zero the last columns of the top rows 369 for(j = 0; j < (nCnt-1); ++j) 370 { 371 _ASSERT(pSrc[nCnt-1][nCnt] != f2vt(0.0f)); 372 f = VERTTYPEDIV(pSrc[j][nCnt], pSrc[nCnt-1][nCnt]); 373 374 // No need to actually calculate a zero for the final column 375 for(k = 0; k < nCnt; ++k) 376 { 377 pSrc[j][k] -= VERTTYPEMUL(f, pSrc[nCnt-1][k]); 378 } 379 } 380 381 break; 382 } 383 } 384 385 // Solve the top-left sub matrix 386 PVRTLinearEqSolve(pRes, pSrc, nCnt - 1); 387 388 // Now calc the solution for the bottom row 389 f = pSrc[nCnt-1][0]; 390 for(k = 1; k < nCnt; ++k) 391 { 392 f -= VERTTYPEMUL(pSrc[nCnt-1][k], pRes[k-1]); 393 } 394 _ASSERT(pSrc[nCnt-1][nCnt] != f2vt(0)); 395 f = VERTTYPEDIV(f, pSrc[nCnt-1][nCnt]); 396 pRes[nCnt-1] = f; 397 } 398 399 /***************************************************************************** 400 End of file (PVRTVector.cpp) 401 *****************************************************************************/ 402 403
