[2746] | 1 | #include "Matrix4x4.h"
|
---|
| 2 | #include "Vector3.h"
|
---|
[2913] | 3 | #include "AxisAlignedBox3.h"
|
---|
| 4 |
|
---|
[3078] | 5 |
|
---|
[2746] | 6 | using namespace std;
|
---|
| 7 |
|
---|
[3078] | 8 |
|
---|
[2776] | 9 | namespace CHCDemoEngine
|
---|
[2751] | 10 | {
|
---|
[2746] | 11 |
|
---|
[2751] | 12 | Matrix4x4::Matrix4x4()
|
---|
| 13 | {}
|
---|
[2746] | 14 |
|
---|
[2751] | 15 |
|
---|
[2746] | 16 | Matrix4x4::Matrix4x4(const Vector3 &a, const Vector3 &b, const Vector3 &c)
|
---|
| 17 | {
|
---|
| 18 | // first index is column [x], the second is row [y]
|
---|
| 19 | x[0][0] = a.x;
|
---|
| 20 | x[1][0] = b.x;
|
---|
| 21 | x[2][0] = c.x;
|
---|
| 22 | x[3][0] = 0.0f;
|
---|
| 23 |
|
---|
| 24 | x[0][1] = a.y;
|
---|
| 25 | x[1][1] = b.y;
|
---|
| 26 | x[2][1] = c.y;
|
---|
| 27 | x[3][1] = 0.0f;
|
---|
| 28 |
|
---|
| 29 | x[0][2] = a.z;
|
---|
| 30 | x[1][2] = b.z;
|
---|
| 31 | x[2][2] = c.z;
|
---|
| 32 | x[3][2] = 0.0f;
|
---|
| 33 |
|
---|
| 34 | x[0][3] = 0.0f;
|
---|
| 35 | x[1][3] = 0.0f;
|
---|
| 36 | x[2][3] = 0.0f;
|
---|
| 37 | x[3][3] = 1.0f;
|
---|
| 38 | }
|
---|
| 39 |
|
---|
| 40 |
|
---|
| 41 | void Matrix4x4::SetColumns(const Vector3 &a, const Vector3 &b, const Vector3 &c)
|
---|
| 42 | {
|
---|
| 43 | // first index is column [x], the second is row [y]
|
---|
| 44 | x[0][0] = a.x;
|
---|
| 45 | x[1][0] = a.y;
|
---|
| 46 | x[2][0] = a.z;
|
---|
| 47 | x[3][0] = 0.0f;
|
---|
| 48 |
|
---|
| 49 | x[0][1] = b.x;
|
---|
| 50 | x[1][1] = b.y;
|
---|
| 51 | x[2][1] = b.z;
|
---|
| 52 | x[3][1] = 0.0f;
|
---|
| 53 |
|
---|
| 54 | x[0][2] = c.x;
|
---|
| 55 | x[1][2] = c.y;
|
---|
| 56 | x[2][2] = c.z;
|
---|
| 57 | x[3][2] = 0.0f;
|
---|
| 58 |
|
---|
| 59 | x[0][3] = 0.0f;
|
---|
| 60 | x[1][3] = 0.0f;
|
---|
| 61 | x[2][3] = 0.0f;
|
---|
| 62 | x[3][3] = 1.0f;
|
---|
| 63 | }
|
---|
| 64 |
|
---|
| 65 | // full constructor
|
---|
| 66 | Matrix4x4::Matrix4x4(float x11, float x12, float x13, float x14,
|
---|
| 67 | float x21, float x22, float x23, float x24,
|
---|
| 68 | float x31, float x32, float x33, float x34,
|
---|
| 69 | float x41, float x42, float x43, float x44)
|
---|
| 70 | {
|
---|
| 71 | // first index is column [x], the second is row [y]
|
---|
| 72 | x[0][0] = x11;
|
---|
| 73 | x[1][0] = x12;
|
---|
| 74 | x[2][0] = x13;
|
---|
| 75 | x[3][0] = x14;
|
---|
| 76 |
|
---|
| 77 | x[0][1] = x21;
|
---|
| 78 | x[1][1] = x22;
|
---|
| 79 | x[2][1] = x23;
|
---|
| 80 | x[3][1] = x24;
|
---|
| 81 |
|
---|
| 82 | x[0][2] = x31;
|
---|
| 83 | x[1][2] = x32;
|
---|
| 84 | x[2][2] = x33;
|
---|
| 85 | x[3][2] = x34;
|
---|
| 86 |
|
---|
| 87 | x[0][3] = x41;
|
---|
| 88 | x[1][3] = x42;
|
---|
| 89 | x[2][3] = x43;
|
---|
| 90 | x[3][3] = x44;
|
---|
| 91 | }
|
---|
| 92 |
|
---|
| 93 |
|
---|
[2751] | 94 | float Matrix4x4::Det3x3() const
|
---|
| 95 | {
|
---|
| 96 | return (x[0][0]*x[1][1]*x[2][2] + \
|
---|
| 97 | x[1][0]*x[2][1]*x[0][2] + \
|
---|
| 98 | x[2][0]*x[0][1]*x[1][2] - \
|
---|
| 99 | x[2][0]*x[1][1]*x[0][2] - \
|
---|
| 100 | x[0][0]*x[2][1]*x[1][2] - \
|
---|
| 101 | x[1][0]*x[0][1]*x[2][2]);
|
---|
| 102 | }
|
---|
| 103 |
|
---|
| 104 |
|
---|
| 105 |
|
---|
[2746] | 106 | // inverse matrix computation gauss_jacobiho method .. from N.R. in C
|
---|
| 107 | // if matrix is regular = computatation successfull = returns 0
|
---|
| 108 | // in case of singular matrix returns 1
|
---|
| 109 | int Matrix4x4::Invert()
|
---|
| 110 | {
|
---|
| 111 | int indxc[4],indxr[4],ipiv[4];
|
---|
| 112 | int i,icol,irow,j,k,l,ll,n;
|
---|
| 113 | float big,dum,pivinv,temp;
|
---|
| 114 | // satisfy the compiler
|
---|
| 115 | icol = irow = 0;
|
---|
| 116 |
|
---|
| 117 | // the size of the matrix
|
---|
| 118 | n = 4;
|
---|
| 119 |
|
---|
| 120 | for ( j = 0 ; j < n ; j++) /* zero pivots */
|
---|
| 121 | ipiv[j] = 0;
|
---|
| 122 |
|
---|
| 123 | for ( i = 0; i < n; i++)
|
---|
| 124 | {
|
---|
| 125 | big = 0.0;
|
---|
| 126 | for (j = 0 ; j < n ; j++)
|
---|
| 127 | if (ipiv[j] != 1)
|
---|
| 128 | for ( k = 0 ; k<n ; k++)
|
---|
| 129 | {
|
---|
| 130 | if (ipiv[k] == 0)
|
---|
| 131 | {
|
---|
| 132 | if (fabs(x[k][j]) >= big)
|
---|
| 133 | {
|
---|
| 134 | big = fabs(x[k][j]);
|
---|
| 135 | irow = j;
|
---|
| 136 | icol = k;
|
---|
| 137 | }
|
---|
| 138 | }
|
---|
| 139 | else
|
---|
| 140 | if (ipiv[k] > 1)
|
---|
| 141 | return 1; /* singular matrix */
|
---|
| 142 | }
|
---|
| 143 | ++(ipiv[icol]);
|
---|
| 144 | if (irow != icol)
|
---|
| 145 | {
|
---|
| 146 | for ( l = 0 ; l<n ; l++)
|
---|
| 147 | {
|
---|
| 148 | temp = x[l][icol];
|
---|
| 149 | x[l][icol] = x[l][irow];
|
---|
| 150 | x[l][irow] = temp;
|
---|
| 151 | }
|
---|
| 152 | }
|
---|
| 153 | indxr[i] = irow;
|
---|
| 154 | indxc[i] = icol;
|
---|
| 155 | if (x[icol][icol] == 0.0)
|
---|
| 156 | return 1; /* singular matrix */
|
---|
| 157 |
|
---|
| 158 | pivinv = 1.0f / x[icol][icol];
|
---|
| 159 | x[icol][icol] = 1.0f ;
|
---|
| 160 | for ( l = 0 ; l<n ; l++)
|
---|
| 161 | x[l][icol] = x[l][icol] * pivinv ;
|
---|
| 162 |
|
---|
| 163 | for (ll = 0 ; ll < n ; ll++)
|
---|
| 164 | if (ll != icol)
|
---|
| 165 | {
|
---|
| 166 | dum = x[icol][ll];
|
---|
| 167 | x[icol][ll] = 0.0;
|
---|
| 168 | for ( l = 0 ; l<n ; l++)
|
---|
| 169 | x[l][ll] = x[l][ll] - x[l][icol] * dum ;
|
---|
| 170 | }
|
---|
| 171 | }
|
---|
| 172 | for ( l = n; l--; )
|
---|
| 173 | {
|
---|
| 174 | if (indxr[l] != indxc[l])
|
---|
| 175 | for ( k = 0; k<n ; k++)
|
---|
| 176 | {
|
---|
| 177 | temp = x[indxr[l]][k];
|
---|
| 178 | x[indxr[l]][k] = x[indxc[l]][k];
|
---|
| 179 | x[indxc[l]][k] = temp;
|
---|
| 180 | }
|
---|
| 181 | }
|
---|
| 182 |
|
---|
| 183 | return 0 ; // matrix is regular .. inversion has been succesfull
|
---|
| 184 | }
|
---|
| 185 |
|
---|
[2751] | 186 |
|
---|
[2746] | 187 | // Invert the given matrix using the above inversion routine.
|
---|
[2751] | 188 | Matrix4x4 Invert(const Matrix4x4& M)
|
---|
[2746] | 189 | {
|
---|
| 190 | Matrix4x4 InvertMe = M;
|
---|
| 191 | InvertMe.Invert();
|
---|
| 192 | return InvertMe;
|
---|
| 193 | }
|
---|
| 194 |
|
---|
| 195 |
|
---|
| 196 | // Transpose the matrix.
|
---|
[2751] | 197 | void Matrix4x4::Transpose()
|
---|
[2746] | 198 | {
|
---|
| 199 | for (int i = 0; i < 4; i++)
|
---|
| 200 | for (int j = i; j < 4; j++)
|
---|
| 201 | if (i != j) {
|
---|
| 202 | float temp = x[i][j];
|
---|
| 203 | x[i][j] = x[j][i];
|
---|
| 204 | x[j][i] = temp;
|
---|
| 205 | }
|
---|
| 206 | }
|
---|
| 207 |
|
---|
[2751] | 208 |
|
---|
[2746] | 209 | // Transpose the given matrix using the transpose routine above.
|
---|
[2751] | 210 | Matrix4x4 Transpose(const Matrix4x4& M)
|
---|
[2746] | 211 | {
|
---|
| 212 | Matrix4x4 TransposeMe = M;
|
---|
| 213 | TransposeMe.Transpose();
|
---|
| 214 | return TransposeMe;
|
---|
| 215 | }
|
---|
| 216 |
|
---|
[2751] | 217 |
|
---|
[2746] | 218 | // Construct an identity matrix.
|
---|
[2751] | 219 | Matrix4x4 IdentityMatrix()
|
---|
[2746] | 220 | {
|
---|
| 221 | Matrix4x4 M;
|
---|
| 222 |
|
---|
| 223 | for (int i = 0; i < 4; i++)
|
---|
| 224 | for (int j = 0; j < 4; j++)
|
---|
| 225 | M.x[i][j] = (i == j) ? 1.0f : 0.0f;
|
---|
| 226 | return M;
|
---|
| 227 | }
|
---|
| 228 |
|
---|
[2751] | 229 |
|
---|
[2746] | 230 | // Construct a zero matrix.
|
---|
[2751] | 231 | Matrix4x4 ZeroMatrix()
|
---|
[2746] | 232 | {
|
---|
| 233 | Matrix4x4 M;
|
---|
| 234 | for (int i = 0; i < 4; i++)
|
---|
| 235 | for (int j = 0; j < 4; j++)
|
---|
| 236 | M.x[i][j] = 0;
|
---|
| 237 | return M;
|
---|
| 238 | }
|
---|
| 239 |
|
---|
| 240 | // Construct a translation matrix given the location to translate to.
|
---|
| 241 | Matrix4x4
|
---|
| 242 | TranslationMatrix(const Vector3& Location)
|
---|
| 243 | {
|
---|
| 244 | Matrix4x4 M = IdentityMatrix();
|
---|
| 245 | M.x[3][0] = Location.x;
|
---|
| 246 | M.x[3][1] = Location.y;
|
---|
| 247 | M.x[3][2] = Location.z;
|
---|
| 248 | return M;
|
---|
| 249 | }
|
---|
| 250 |
|
---|
| 251 | // Construct a rotation matrix. Rotates Angle radians about the
|
---|
| 252 | // X axis.
|
---|
| 253 | Matrix4x4
|
---|
| 254 | RotationXMatrix(float Angle)
|
---|
| 255 | {
|
---|
| 256 | Matrix4x4 M = IdentityMatrix();
|
---|
| 257 | float Cosine = cos(Angle);
|
---|
| 258 | float Sine = sin(Angle);
|
---|
| 259 | M.x[1][1] = Cosine;
|
---|
| 260 | M.x[2][1] = -Sine;
|
---|
| 261 | M.x[1][2] = Sine;
|
---|
| 262 | M.x[2][2] = Cosine;
|
---|
| 263 | return M;
|
---|
| 264 | }
|
---|
| 265 |
|
---|
| 266 | // Construct a rotation matrix. Rotates Angle radians about the
|
---|
| 267 | // Y axis.
|
---|
| 268 | Matrix4x4 RotationYMatrix(float angle)
|
---|
| 269 | {
|
---|
| 270 | Matrix4x4 m = IdentityMatrix();
|
---|
| 271 |
|
---|
| 272 | float cosine = cos(angle);
|
---|
| 273 | float sine = sin(angle);
|
---|
| 274 |
|
---|
| 275 | m.x[0][0] = cosine;
|
---|
| 276 | m.x[2][0] = -sine;
|
---|
| 277 | m.x[0][2] = sine;
|
---|
| 278 | m.x[2][2] = cosine;
|
---|
| 279 |
|
---|
| 280 | return m;
|
---|
| 281 | }
|
---|
| 282 |
|
---|
| 283 |
|
---|
| 284 | // Construct a rotation matrix. Rotates Angle radians about the
|
---|
| 285 | // Z axis.
|
---|
| 286 | Matrix4x4 RotationZMatrix(float angle)
|
---|
| 287 | {
|
---|
| 288 | Matrix4x4 m = IdentityMatrix();
|
---|
| 289 |
|
---|
| 290 | float cosine = cos(angle);
|
---|
| 291 | float sine = sin(angle);
|
---|
| 292 |
|
---|
| 293 | m.x[0][0] = cosine;
|
---|
| 294 | m.x[1][0] = -sine;
|
---|
| 295 | m.x[0][1] = sine;
|
---|
| 296 | m.x[1][1] = cosine;
|
---|
| 297 |
|
---|
| 298 | return m;
|
---|
| 299 | }
|
---|
| 300 |
|
---|
| 301 |
|
---|
| 302 | // Construct a yaw-pitch-roll rotation matrix. Rotate Yaw
|
---|
| 303 | // radians about the XY axis, rotate Pitch radians in the
|
---|
| 304 | // plane defined by the Yaw rotation, and rotate Roll radians
|
---|
| 305 | // about the axis defined by the previous two angles.
|
---|
| 306 | Matrix4x4 RotationYPRMatrix(float Yaw, float Pitch, float Roll)
|
---|
| 307 | {
|
---|
| 308 | Matrix4x4 M;
|
---|
| 309 | float ch = cos(Yaw);
|
---|
| 310 | float sh = sin(Yaw);
|
---|
| 311 | float cp = cos(Pitch);
|
---|
| 312 | float sp = sin(Pitch);
|
---|
| 313 | float cr = cos(Roll);
|
---|
| 314 | float sr = sin(Roll);
|
---|
| 315 |
|
---|
| 316 | M.x[0][0] = ch * cr + sh * sp * sr;
|
---|
| 317 | M.x[1][0] = -ch * sr + sh * sp * cr;
|
---|
| 318 | M.x[2][0] = sh * cp;
|
---|
| 319 | M.x[0][1] = sr * cp;
|
---|
| 320 | M.x[1][1] = cr * cp;
|
---|
| 321 | M.x[2][1] = -sp;
|
---|
| 322 | M.x[0][2] = -sh * cr - ch * sp * sr;
|
---|
| 323 | M.x[1][2] = sr * sh + ch * sp * cr;
|
---|
| 324 | M.x[2][2] = ch * cp;
|
---|
| 325 | for (int i = 0; i < 4; i++)
|
---|
| 326 | M.x[3][i] = M.x[i][3] = 0;
|
---|
| 327 | M.x[3][3] = 1;
|
---|
| 328 |
|
---|
| 329 | return M;
|
---|
| 330 | }
|
---|
| 331 |
|
---|
| 332 | // Construct a rotation of a given angle about a given axis.
|
---|
| 333 | // Derived from Eric Haines's SPD (Standard Procedural
|
---|
| 334 | // Database).
|
---|
| 335 | Matrix4x4 RotationAxisMatrix(const Vector3& axis, float angle)
|
---|
| 336 | {
|
---|
| 337 | Matrix4x4 M;
|
---|
| 338 |
|
---|
| 339 | float cosine = cos(angle);
|
---|
| 340 | float sine = sin(angle);
|
---|
| 341 | float one_minus_cosine = 1 - cosine;
|
---|
| 342 |
|
---|
| 343 | M.x[0][0] = axis.x * axis.x + (1.0f - axis.x * axis.x) * cosine;
|
---|
| 344 | M.x[0][1] = axis.x * axis.y * one_minus_cosine + axis.z * sine;
|
---|
| 345 | M.x[0][2] = axis.x * axis.z * one_minus_cosine - axis.y * sine;
|
---|
| 346 | M.x[0][3] = 0;
|
---|
| 347 |
|
---|
| 348 | M.x[1][0] = axis.x * axis.y * one_minus_cosine - axis.z * sine;
|
---|
| 349 | M.x[1][1] = axis.y * axis.y + (1.0f - axis.y * axis.y) * cosine;
|
---|
| 350 | M.x[1][2] = axis.y * axis.z * one_minus_cosine + axis.x * sine;
|
---|
| 351 | M.x[1][3] = 0;
|
---|
| 352 |
|
---|
| 353 | M.x[2][0] = axis.x * axis.z * one_minus_cosine + axis.y * sine;
|
---|
| 354 | M.x[2][1] = axis.y * axis.z * one_minus_cosine - axis.x * sine;
|
---|
| 355 | M.x[2][2] = axis.z * axis.z + (1.0f - axis.z * axis.z) * cosine;
|
---|
| 356 | M.x[2][3] = 0;
|
---|
| 357 |
|
---|
| 358 | M.x[3][0] = 0;
|
---|
| 359 | M.x[3][1] = 0;
|
---|
| 360 | M.x[3][2] = 0;
|
---|
| 361 | M.x[3][3] = 1;
|
---|
| 362 |
|
---|
| 363 | return M;
|
---|
| 364 | }
|
---|
| 365 |
|
---|
| 366 |
|
---|
| 367 | // Constructs the rotation matrix that rotates 'vec1' to 'vec2'
|
---|
| 368 | Matrix4x4 RotationVectorsMatrix(const Vector3 &vecStart, const Vector3 &vecTo)
|
---|
| 369 | {
|
---|
| 370 | Vector3 vec = CrossProd(vecStart, vecTo);
|
---|
| 371 |
|
---|
| 372 | if (Magnitude(vec) > Limits::Small) {
|
---|
| 373 | // vector exist, compute angle
|
---|
| 374 | float angle = acos(DotProd(vecStart, vecTo));
|
---|
| 375 | // normalize for sure
|
---|
| 376 | vec.Normalize();
|
---|
| 377 | return RotationAxisMatrix(vec, angle);
|
---|
| 378 | }
|
---|
| 379 |
|
---|
| 380 | // opposite or colinear vectors
|
---|
| 381 | Matrix4x4 ret = IdentityMatrix();
|
---|
| 382 | if (DotProd(vecStart, vecTo) < 0.0f)
|
---|
| 383 | ret *= -1.0f; // opposite vectors
|
---|
| 384 |
|
---|
| 385 | return ret;
|
---|
| 386 | }
|
---|
| 387 |
|
---|
| 388 |
|
---|
| 389 | // Construct a scale matrix given the X, Y, and Z parameters
|
---|
| 390 | // to scale by. To scale uniformly, let X==Y==Z.
|
---|
| 391 | Matrix4x4 ScaleMatrix(float X, float Y, float Z)
|
---|
| 392 | {
|
---|
| 393 | Matrix4x4 M = IdentityMatrix();
|
---|
| 394 |
|
---|
| 395 | M.x[0][0] = X;
|
---|
| 396 | M.x[1][1] = Y;
|
---|
| 397 | M.x[2][2] = Z;
|
---|
| 398 |
|
---|
| 399 | return M;
|
---|
| 400 | }
|
---|
| 401 |
|
---|
| 402 |
|
---|
[3279] | 403 | // uniform scale
|
---|
| 404 | Matrix4x4 ScaleMatrix(float x)
|
---|
| 405 | {
|
---|
| 406 | Matrix4x4 M = IdentityMatrix();
|
---|
| 407 |
|
---|
| 408 | M.x[0][0] = x;
|
---|
| 409 | M.x[1][1] = x;
|
---|
| 410 | M.x[2][2] = x;
|
---|
| 411 |
|
---|
| 412 | return M;
|
---|
| 413 | }
|
---|
| 414 |
|
---|
[2746] | 415 | // Construct a rotation matrix that makes the x, y, z axes
|
---|
| 416 | // correspond to the vectors given.
|
---|
| 417 | Matrix4x4 GenRotation(const Vector3 &x, const Vector3 &y, const Vector3 &z)
|
---|
| 418 | {
|
---|
| 419 | Matrix4x4 M = IdentityMatrix();
|
---|
| 420 |
|
---|
| 421 | // x y
|
---|
| 422 | M.x[0][0] = x.x;
|
---|
| 423 | M.x[1][0] = x.y;
|
---|
| 424 | M.x[2][0] = x.z;
|
---|
| 425 |
|
---|
| 426 | M.x[0][1] = y.x;
|
---|
| 427 | M.x[1][1] = y.y;
|
---|
| 428 | M.x[2][1] = y.z;
|
---|
| 429 |
|
---|
| 430 | M.x[0][2] = z.x;
|
---|
| 431 | M.x[1][2] = z.y;
|
---|
| 432 | M.x[2][2] = z.z;
|
---|
| 433 |
|
---|
| 434 | return M;
|
---|
| 435 | }
|
---|
| 436 |
|
---|
| 437 | // Construct a quadric matrix. After Foley et al. pp. 528-529.
|
---|
| 438 | Matrix4x4
|
---|
| 439 | QuadricMatrix(float a, float b, float c, float d, float e,
|
---|
| 440 | float f, float g, float h, float j, float k)
|
---|
| 441 | {
|
---|
| 442 | Matrix4x4 M;
|
---|
| 443 |
|
---|
| 444 | M.x[0][0] = a; M.x[0][1] = d; M.x[0][2] = f; M.x[0][3] = g;
|
---|
| 445 | M.x[1][0] = d; M.x[1][1] = b; M.x[1][2] = e; M.x[1][3] = h;
|
---|
| 446 | M.x[2][0] = f; M.x[2][1] = e; M.x[2][2] = c; M.x[2][3] = j;
|
---|
| 447 | M.x[3][0] = g; M.x[3][1] = h; M.x[3][2] = j; M.x[3][3] = k;
|
---|
| 448 |
|
---|
| 449 | return M;
|
---|
| 450 | }
|
---|
| 451 |
|
---|
| 452 | // Construct various "mirror" matrices, which flip coordinate
|
---|
| 453 | // signs in the various axes specified.
|
---|
| 454 | Matrix4x4
|
---|
| 455 | MirrorX()
|
---|
| 456 | {
|
---|
| 457 | Matrix4x4 M = IdentityMatrix();
|
---|
| 458 | M.x[0][0] = -1;
|
---|
| 459 | return M;
|
---|
| 460 | }
|
---|
| 461 |
|
---|
| 462 | Matrix4x4
|
---|
| 463 | MirrorY()
|
---|
| 464 | {
|
---|
| 465 | Matrix4x4 M = IdentityMatrix();
|
---|
| 466 | M.x[1][1] = -1;
|
---|
| 467 | return M;
|
---|
| 468 | }
|
---|
| 469 |
|
---|
| 470 | Matrix4x4
|
---|
| 471 | MirrorZ()
|
---|
| 472 | {
|
---|
| 473 | Matrix4x4 M = IdentityMatrix();
|
---|
| 474 | M.x[2][2] = -1;
|
---|
| 475 | return M;
|
---|
| 476 | }
|
---|
| 477 |
|
---|
| 478 | Matrix4x4
|
---|
| 479 | RotationOnly(const Matrix4x4& x)
|
---|
| 480 | {
|
---|
| 481 | Matrix4x4 M = x;
|
---|
| 482 | M.x[3][0] = M.x[3][1] = M.x[3][2] = 0;
|
---|
| 483 | return M;
|
---|
| 484 | }
|
---|
| 485 |
|
---|
| 486 | // Add corresponding elements of the two matrices.
|
---|
| 487 | Matrix4x4&
|
---|
| 488 | Matrix4x4::operator+= (const Matrix4x4& A)
|
---|
| 489 | {
|
---|
| 490 | for (int i = 0; i < 4; i++)
|
---|
| 491 | for (int j = 0; j < 4; j++)
|
---|
| 492 | x[i][j] += A.x[i][j];
|
---|
| 493 | return *this;
|
---|
| 494 | }
|
---|
| 495 |
|
---|
| 496 | // Subtract corresponding elements of the matrices.
|
---|
| 497 | Matrix4x4&
|
---|
| 498 | Matrix4x4::operator-= (const Matrix4x4& A)
|
---|
| 499 | {
|
---|
| 500 | for (int i = 0; i < 4; i++)
|
---|
| 501 | for (int j = 0; j < 4; j++)
|
---|
| 502 | x[i][j] -= A.x[i][j];
|
---|
| 503 | return *this;
|
---|
| 504 | }
|
---|
| 505 |
|
---|
| 506 | // Scale each element of the matrix by A.
|
---|
| 507 | Matrix4x4&
|
---|
| 508 | Matrix4x4::operator*= (float A)
|
---|
| 509 | {
|
---|
| 510 | for (int i = 0; i < 4; i++)
|
---|
| 511 | for (int j = 0; j < 4; j++)
|
---|
| 512 | x[i][j] *= A;
|
---|
| 513 | return *this;
|
---|
| 514 | }
|
---|
| 515 |
|
---|
| 516 | // Multiply two matrices.
|
---|
| 517 | Matrix4x4&
|
---|
| 518 | Matrix4x4::operator*= (const Matrix4x4& A)
|
---|
| 519 | {
|
---|
| 520 | Matrix4x4 ret = *this;
|
---|
| 521 |
|
---|
| 522 | for (int i = 0; i < 4; i++)
|
---|
| 523 | for (int j = 0; j < 4; j++) {
|
---|
| 524 | float subt = 0;
|
---|
| 525 | for (int k = 0; k < 4; k++)
|
---|
| 526 | subt += ret.x[i][k] * A.x[k][j];
|
---|
| 527 | x[i][j] = subt;
|
---|
| 528 | }
|
---|
| 529 | return *this;
|
---|
| 530 | }
|
---|
| 531 |
|
---|
| 532 | // Add corresponding elements of the matrices.
|
---|
| 533 | Matrix4x4
|
---|
| 534 | operator+ (const Matrix4x4& A, const Matrix4x4& B)
|
---|
| 535 | {
|
---|
| 536 | Matrix4x4 ret;
|
---|
| 537 |
|
---|
| 538 | for (int i = 0; i < 4; i++)
|
---|
| 539 | for (int j = 0; j < 4; j++)
|
---|
| 540 | ret.x[i][j] = A.x[i][j] + B.x[i][j];
|
---|
| 541 | return ret;
|
---|
| 542 | }
|
---|
| 543 |
|
---|
| 544 | // Subtract corresponding elements of the matrices.
|
---|
| 545 | Matrix4x4
|
---|
| 546 | operator- (const Matrix4x4& A, const Matrix4x4& B)
|
---|
| 547 | {
|
---|
| 548 | Matrix4x4 ret;
|
---|
| 549 |
|
---|
| 550 | for (int i = 0; i < 4; i++)
|
---|
| 551 | for (int j = 0; j < 4; j++)
|
---|
| 552 | ret.x[i][j] = A.x[i][j] - B.x[i][j];
|
---|
| 553 | return ret;
|
---|
| 554 | }
|
---|
| 555 |
|
---|
| 556 | // Multiply matrices.
|
---|
| 557 | Matrix4x4
|
---|
| 558 | operator* (const Matrix4x4& A, const Matrix4x4& B)
|
---|
| 559 | {
|
---|
| 560 | Matrix4x4 ret;
|
---|
| 561 |
|
---|
| 562 | for (int i = 0; i < 4; i++)
|
---|
| 563 | for (int j = 0; j < 4; j++) {
|
---|
| 564 | float subt = 0;
|
---|
| 565 | for (int k = 0; k < 4; k++)
|
---|
| 566 | subt += A.x[i][k] * B.x[k][j];
|
---|
| 567 | ret.x[i][j] = subt;
|
---|
| 568 | }
|
---|
| 569 | return ret;
|
---|
| 570 | }
|
---|
| 571 |
|
---|
| 572 | // Transform a vector by a matrix.
|
---|
| 573 | Vector3
|
---|
| 574 | operator* (const Matrix4x4& M, const Vector3& v)
|
---|
| 575 | {
|
---|
| 576 | Vector3 ret;
|
---|
| 577 | float denom;
|
---|
| 578 |
|
---|
| 579 | ret.x = v.x * M.x[0][0] + v.y * M.x[1][0] + v.z * M.x[2][0] + M.x[3][0];
|
---|
| 580 | ret.y = v.x * M.x[0][1] + v.y * M.x[1][1] + v.z * M.x[2][1] + M.x[3][1];
|
---|
| 581 | ret.z = v.x * M.x[0][2] + v.y * M.x[1][2] + v.z * M.x[2][2] + M.x[3][2];
|
---|
[2939] | 582 | denom = v.x * M.x[0][3] + v.y * M.x[1][3] + v.z * M.x[2][3] + M.x[3][3];
|
---|
| 583 |
|
---|
[2746] | 584 | if (denom != 1.0)
|
---|
| 585 | ret /= denom;
|
---|
| 586 | return ret;
|
---|
| 587 | }
|
---|
| 588 |
|
---|
[2944] | 589 |
|
---|
| 590 | Vector3 Matrix4x4::Transform(float &w, const Vector3 &v, float h) const
|
---|
| 591 | {
|
---|
| 592 | Vector3 ret;
|
---|
[2953] | 593 |
|
---|
[2944] | 594 | ret.x = v.x * x[0][0] + v.y * x[1][0] + v.z * x[2][0] + h * x[3][0];
|
---|
| 595 | ret.y = v.x * x[0][1] + v.y * x[1][1] + v.z * x[2][1] + h * x[3][1];
|
---|
| 596 | ret.z = v.x * x[0][2] + v.y * x[1][2] + v.z * x[2][2] + h * x[3][2];
|
---|
| 597 | w = v.x * x[0][3] + v.y * x[1][3] + v.z * x[2][3] + h * x[3][3];
|
---|
| 598 |
|
---|
| 599 | return ret;
|
---|
| 600 | }
|
---|
| 601 |
|
---|
| 602 |
|
---|
[2746] | 603 | // Apply the rotation portion of a matrix to a vector.
|
---|
| 604 | Vector3
|
---|
| 605 | RotateOnly(const Matrix4x4& M, const Vector3& v)
|
---|
| 606 | {
|
---|
| 607 | Vector3 ret;
|
---|
| 608 | float denom;
|
---|
| 609 |
|
---|
| 610 | ret.x = v.x * M.x[0][0] + v.y * M.x[1][0] + v.z * M.x[2][0];
|
---|
| 611 | ret.y = v.x * M.x[0][1] + v.y * M.x[1][1] + v.z * M.x[2][1];
|
---|
| 612 | ret.z = v.x * M.x[0][2] + v.y * M.x[1][2] + v.z * M.x[2][2];
|
---|
| 613 | denom = M.x[0][3] + M.x[1][3] + M.x[2][3] + M.x[3][3];
|
---|
| 614 | if (denom != 1.0)
|
---|
| 615 | ret /= denom;
|
---|
| 616 | return ret;
|
---|
| 617 | }
|
---|
| 618 |
|
---|
| 619 | // Scale each element of the matrix by B.
|
---|
| 620 | Matrix4x4
|
---|
| 621 | operator* (const Matrix4x4& A, float B)
|
---|
| 622 | {
|
---|
| 623 | Matrix4x4 ret;
|
---|
| 624 |
|
---|
| 625 | for (int i = 0; i < 4; i++)
|
---|
| 626 | for (int j = 0; j < 4; j++)
|
---|
| 627 | ret.x[i][j] = A.x[i][j] * B;
|
---|
| 628 | return ret;
|
---|
| 629 | }
|
---|
| 630 |
|
---|
| 631 | // Overloaded << for C++-style output.
|
---|
| 632 | ostream&
|
---|
| 633 | operator<< (ostream& s, const Matrix4x4& M)
|
---|
| 634 | {
|
---|
| 635 | for (int i = 0; i < 4; i++) { // y
|
---|
| 636 | for (int j = 0; j < 4; j++) { // x
|
---|
| 637 | // x y
|
---|
[3078] | 638 | s << M.x[j][i];
|
---|
[2746] | 639 | }
|
---|
| 640 | s << '\n';
|
---|
| 641 | }
|
---|
| 642 | return s;
|
---|
| 643 | }
|
---|
| 644 |
|
---|
[2752] | 645 |
|
---|
| 646 | Vector3 PlaneRotate(const Matrix4x4& tform, const Vector3& p)
|
---|
[2746] | 647 | {
|
---|
| 648 | // I sure hope that matrix is invertible...
|
---|
| 649 | Matrix4x4 use = Transpose(Invert(tform));
|
---|
| 650 |
|
---|
| 651 | return RotateOnly(use, p);
|
---|
| 652 | }
|
---|
| 653 |
|
---|
| 654 | // Transform a normal
|
---|
[2752] | 655 | Vector3 TransformNormal(const Matrix4x4& tform, const Vector3& n)
|
---|
[2746] | 656 | {
|
---|
| 657 | Matrix4x4 use = NormalTransformMatrix(tform);
|
---|
| 658 |
|
---|
| 659 | return RotateOnly(use, n);
|
---|
| 660 | }
|
---|
| 661 |
|
---|
[3062] | 662 |
|
---|
| 663 | Matrix4x4 NormalTransformMatrix(const Matrix4x4 &tform)
|
---|
[2746] | 664 | {
|
---|
| 665 | Matrix4x4 m = tform;
|
---|
| 666 | // for normal translation vector must be zero!
|
---|
| 667 | m.x[3][0] = m.x[3][1] = m.x[3][2] = 0.0;
|
---|
| 668 | // I sure hope that matrix is invertible...
|
---|
| 669 | return Transpose(Invert(m));
|
---|
| 670 | }
|
---|
| 671 |
|
---|
| 672 |
|
---|
| 673 | Vector3 GetTranslation(const Matrix4x4 &M)
|
---|
| 674 | {
|
---|
| 675 | return Vector3(M.x[3][0], M.x[3][1], M.x[3][2]);
|
---|
| 676 | }
|
---|
| 677 |
|
---|
[2913] | 678 |
|
---|
| 679 | Matrix4x4 GetFittingProjectionMatrix(const AxisAlignedBox3 &box)
|
---|
| 680 | {
|
---|
| 681 | Matrix4x4 m;
|
---|
| 682 |
|
---|
[2916] | 683 | m.x[0][0] = 2.0f / (box.Max()[0] - box.Min()[0]);
|
---|
| 684 | m.x[0][1] = .0f;
|
---|
| 685 | m.x[0][2] = .0f;
|
---|
| 686 | m.x[0][3] = .0f;
|
---|
| 687 |
|
---|
| 688 | m.x[1][0] = .0f;
|
---|
| 689 | m.x[1][1] = 2.0f / (box.Max()[1] - box.Min()[1]);
|
---|
| 690 | m.x[1][2] = .0f;
|
---|
| 691 | m.x[1][3] = .0f;
|
---|
| 692 |
|
---|
| 693 | m.x[2][0] = .0f;
|
---|
| 694 | m.x[2][1] = .0f;
|
---|
| 695 | m.x[2][2] = 2.0f / (box.Max()[2] - box.Min()[2]);
|
---|
| 696 | m.x[2][3] = .0f;
|
---|
| 697 |
|
---|
| 698 | m.x[3][0] = -(box.Max()[0] + box.Min()[0]) / (box.Max()[0] - box.Min()[0]);
|
---|
| 699 | m.x[3][1] = -(box.Max()[1] + box.Min()[1]) / (box.Max()[1] - box.Min()[1]);
|
---|
| 700 | m.x[3][2] = -(box.Max()[2] + box.Min()[2]) / (box.Max()[2] - box.Min()[2]);
|
---|
| 701 | m.x[3][3] = 1.0f;
|
---|
| 702 |
|
---|
| 703 | return m;
|
---|
[2917] | 704 | }
|
---|
[2916] | 705 |
|
---|
[2917] | 706 |
|
---|
[3062] | 707 | /** The resultig matrix that is equal to the result of glFrustum
|
---|
[2939] | 708 | */
|
---|
[2920] | 709 | Matrix4x4 GetFrustum(float left, float right,
|
---|
| 710 | float bottom, float top,
|
---|
| 711 | float near, float far)
|
---|
[2915] | 712 | {
|
---|
| 713 | Matrix4x4 m;
|
---|
| 714 |
|
---|
| 715 | const float xDif = 1.0f / (right - left);
|
---|
| 716 | const float yDif = 1.0f / (top - bottom);
|
---|
| 717 | const float zDif = 1.0f / (near - far);
|
---|
| 718 |
|
---|
| 719 | m.x[0][0] = 2.0f * near * xDif;
|
---|
[2913] | 720 | m.x[0][1] = .0f;
|
---|
[2915] | 721 | m.x[0][2] = .0f;
|
---|
| 722 | m.x[0][3] = .0f;
|
---|
[2913] | 723 |
|
---|
[2915] | 724 | m.x[1][0] = .0f;
|
---|
| 725 | m.x[1][1] = 2.0f * near * yDif;
|
---|
[2913] | 726 | m.x[1][2] = .0f;
|
---|
| 727 | m.x[1][3] = .0f;
|
---|
| 728 |
|
---|
[2915] | 729 | m.x[2][0] = (right + left) * xDif;
|
---|
| 730 | m.x[2][1] = (top + bottom)* yDif;
|
---|
| 731 | m.x[2][2] = (far + near) * zDif;
|
---|
| 732 | m.x[2][3] = -1.0f;
|
---|
| 733 |
|
---|
| 734 | m.x[3][0] = .0f;
|
---|
| 735 | m.x[3][1] = .0f;
|
---|
| 736 | m.x[3][2] = 2.0f * near * far * zDif;
|
---|
| 737 | m.x[3][3] = .0f;
|
---|
[2917] | 738 |
|
---|
| 739 | return m;
|
---|
[2913] | 740 | }
|
---|
| 741 |
|
---|
| 742 |
|
---|
[2917] | 743 | Matrix4x4 LookAt(const Vector3 &pos, const Vector3 &dir, const Vector3& up)
|
---|
| 744 | {
|
---|
| 745 | const Vector3 nDir = Normalize(dir);
|
---|
| 746 | Vector3 nUp = Normalize(up);
|
---|
[2915] | 747 |
|
---|
[2917] | 748 | Vector3 nRight = Normalize(CrossProd(nDir, nUp));
|
---|
| 749 | nUp = Normalize(CrossProd(nRight, nDir));
|
---|
| 750 |
|
---|
[2940] | 751 | Matrix4x4 m(nRight.x, nRight.y, nRight.z, 0,
|
---|
| 752 | nUp.x, nUp.y, nUp.z, 0,
|
---|
| 753 | -nDir.x, -nDir.y, -nDir.z, 0,
|
---|
| 754 | 0, 0, 0, 1);
|
---|
[2932] | 755 |
|
---|
[2934] | 756 | // note: left handed system => we go into positive z
|
---|
[2940] | 757 | m.x[3][0] = -DotProd(nRight, pos);
|
---|
| 758 | m.x[3][1] = -DotProd(nUp, pos);
|
---|
[2942] | 759 | m.x[3][2] = DotProd(nDir, pos);
|
---|
[2934] | 760 |
|
---|
| 761 | return m;
|
---|
| 762 | }
|
---|
| 763 |
|
---|
| 764 |
|
---|
[3102] | 765 | /** The resulting matrix is equal to the result of glOrtho
|
---|
[3062] | 766 | */
|
---|
[3102] | 767 | Matrix4x4 GetOrtho(float left, float right, float bottom, float top, float near, float far)
|
---|
[3062] | 768 | {
|
---|
| 769 | Matrix4x4 m;
|
---|
| 770 |
|
---|
| 771 | const float xDif = 1.0f / (right - left);
|
---|
| 772 | const float yDif = 1.0f / (top - bottom);
|
---|
| 773 | const float zDif = 1.0f / (far - near);
|
---|
| 774 |
|
---|
| 775 | m.x[0][0] = 2.0f * xDif;
|
---|
| 776 | m.x[0][1] = .0f;
|
---|
| 777 | m.x[0][2] = .0f;
|
---|
| 778 | m.x[0][3] = .0f;
|
---|
| 779 |
|
---|
| 780 | m.x[1][0] = .0f;
|
---|
| 781 | m.x[1][1] = 2.0f * yDif;
|
---|
| 782 | m.x[1][2] = .0f;
|
---|
| 783 | m.x[1][3] = .0f;
|
---|
| 784 |
|
---|
| 785 | m.x[2][0] = .0f;
|
---|
| 786 | m.x[2][1] = .0f;
|
---|
| 787 | m.x[2][2] = -2.0f * zDif;
|
---|
| 788 | m.x[2][3] = .0f;
|
---|
| 789 |
|
---|
[3103] | 790 | m.x[3][0] = -(right + left) * xDif;
|
---|
| 791 | m.x[3][1] = -(top + bottom) * yDif;
|
---|
| 792 | m.x[3][2] = -(far + near) * zDif;
|
---|
[3062] | 793 | m.x[3][3] = 1.0f;
|
---|
| 794 |
|
---|
| 795 | return m;
|
---|
| 796 | }
|
---|
| 797 |
|
---|
| 798 | /** The resultig matrix that is equal to the result of gluPerspective
|
---|
| 799 | */
|
---|
| 800 | Matrix4x4 GetPerspective(float fov, float aspect, float near, float far)
|
---|
| 801 | {
|
---|
| 802 | Matrix4x4 m;
|
---|
| 803 |
|
---|
| 804 | const float x = 1.0f / tan(fov * 0.5f);
|
---|
| 805 | const float zDif = 1.0f / (near - far);
|
---|
| 806 |
|
---|
| 807 | m.x[0][0] = x / aspect;
|
---|
| 808 | m.x[0][1] = .0f;
|
---|
| 809 | m.x[0][2] = .0f;
|
---|
| 810 | m.x[0][3] = .0f;
|
---|
| 811 |
|
---|
| 812 | m.x[1][0] = .0f;
|
---|
| 813 | m.x[1][1] = x;
|
---|
| 814 | m.x[1][2] = .0f;
|
---|
| 815 | m.x[1][3] = .0f;
|
---|
| 816 |
|
---|
| 817 | m.x[2][0] = .0f;
|
---|
| 818 | m.x[2][1] = .0f;
|
---|
| 819 | m.x[2][2] = (far + near) * zDif;
|
---|
| 820 | m.x[2][3] = -1.0f;
|
---|
| 821 |
|
---|
| 822 | m.x[3][0] = .0f;
|
---|
| 823 | m.x[3][1] = .0f;
|
---|
| 824 | m.x[3][2] = 2.0f *(far * near) * zDif;
|
---|
| 825 | m.x[3][3] = 0.0f;
|
---|
| 826 |
|
---|
| 827 | return m;
|
---|
| 828 | }
|
---|
| 829 |
|
---|
| 830 |
|
---|
[2751] | 831 | } |
---|