absence_thelab

annotate src/common/n3dmath.cpp @ 0:1cffe3409164

find changesets by author, revision, files, or words in the commit message
initial commit
author John Tsiombikas <nuclear@member.fsf.org>
date Thu, 23 Oct 2014 01:46:07 +0300
parents
children
rev   line source
nuclear@0 1 #include "n3dmath.h"
nuclear@0 2
nuclear@0 3 #define fsin (float)sin
nuclear@0 4 #define fcos (float)cos
nuclear@0 5
nuclear@0 6 float frand(float range) {
nuclear@0 7 return ((float)rand() / (float)RAND_MAX) * range;
nuclear@0 8 }
nuclear@0 9
nuclear@0 10 Vector3::Vector3() {
nuclear@0 11 x = y = z = 0.0f;
nuclear@0 12 }
nuclear@0 13
nuclear@0 14 Vector3::Vector3(float x, float y, float z) {
nuclear@0 15 this->x = x;
nuclear@0 16 this->y = y;
nuclear@0 17 this->z = z;
nuclear@0 18 }
nuclear@0 19 /* inlined
nuclear@0 20 float Vector3::DotProduct(const Vector3 &vec) const {
nuclear@0 21 return x * vec.x + y * vec.y + z * vec.z;
nuclear@0 22 }
nuclear@0 23
nuclear@0 24 float DotProduct(const Vector3 &vec1, const Vector3 &vec2) {
nuclear@0 25 return vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z;
nuclear@0 26 }
nuclear@0 27
nuclear@0 28 Vector3 Vector3::CrossProduct(const Vector3 &vec) const {
nuclear@0 29 return Vector3(y * vec.z - z * vec.y, z * vec.x - x * vec.z, x * vec.y - y * vec.x);
nuclear@0 30 }
nuclear@0 31
nuclear@0 32 Vector3 CrossProduct(const Vector3 &vec1, const Vector3 &vec2) {
nuclear@0 33 return Vector3(vec1.y * vec2.z - vec1.z * vec2.y, vec1.z * vec2.x - vec1.x * vec2.z, vec1.x * vec2.y - vec1.y * vec2.x);
nuclear@0 34 }
nuclear@0 35
nuclear@0 36 Vector3 Vector3::operator +(const Vector3 &vec) const {
nuclear@0 37 return Vector3(x + vec.x, y + vec.y, z + vec.z);
nuclear@0 38 }
nuclear@0 39
nuclear@0 40 Vector3 Vector3::operator -(const Vector3 &vec) const {
nuclear@0 41 return Vector3(x - vec.x, y - vec.y, z - vec.z);
nuclear@0 42 }
nuclear@0 43
nuclear@0 44 Vector3 Vector3::operator *(float scalar) const {
nuclear@0 45 return Vector3(x * scalar, y * scalar, z * scalar);
nuclear@0 46 }
nuclear@0 47
nuclear@0 48 Vector3 Vector3::operator /(float scalar) const {
nuclear@0 49 return Vector3(x / scalar, y / scalar, z / scalar);
nuclear@0 50 }
nuclear@0 51
nuclear@0 52 void Vector3::operator +=(const Vector3 &vec) {
nuclear@0 53 x += vec.x;
nuclear@0 54 y += vec.y;
nuclear@0 55 z += vec.z;
nuclear@0 56 }
nuclear@0 57
nuclear@0 58 void Vector3::operator -=(const Vector3 &vec) {
nuclear@0 59 x -= vec.x;
nuclear@0 60 y -= vec.y;
nuclear@0 61 z -= vec.z;
nuclear@0 62 }
nuclear@0 63
nuclear@0 64 void Vector3::operator *=(float scalar) {
nuclear@0 65 x *= scalar;
nuclear@0 66 y *= scalar;
nuclear@0 67 z *= scalar;
nuclear@0 68 }
nuclear@0 69
nuclear@0 70 void Vector3::operator /=(float scalar) {
nuclear@0 71 x /= scalar;
nuclear@0 72 y /= scalar;
nuclear@0 73 z /= scalar;
nuclear@0 74 }
nuclear@0 75
nuclear@0 76 Vector3 Vector3::operator -() const {
nuclear@0 77 return Vector3(-x, -y, -z);
nuclear@0 78 }
nuclear@0 79
nuclear@0 80 bool Vector3::operator >(const Vector3 &vec) const {
nuclear@0 81 return LengthSq() > vec.LengthSq();
nuclear@0 82 }
nuclear@0 83
nuclear@0 84 bool Vector3::operator <(const Vector3 &vec) const {
nuclear@0 85 return LengthSq() < vec.LengthSq();
nuclear@0 86 }
nuclear@0 87
nuclear@0 88 bool Vector3::operator >(float len) const {
nuclear@0 89 return LengthSq() > len;
nuclear@0 90 }
nuclear@0 91
nuclear@0 92 bool Vector3::operator <(float len) const {
nuclear@0 93 return LengthSq() < len;
nuclear@0 94 }
nuclear@0 95
nuclear@0 96 bool Vector3::operator ==(const Vector3 &vec) const {
nuclear@0 97 return ((*this - vec).Length() < XSmallNumber);
nuclear@0 98 }
nuclear@0 99
nuclear@0 100 bool Vector3::operator ==(float len) const {
nuclear@0 101 return ((this->Length() - len) < XSmallNumber);
nuclear@0 102 }
nuclear@0 103
nuclear@0 104 Vector3::operator Vector2() const {
nuclear@0 105 return Vector2(x, y);
nuclear@0 106 }
nuclear@0 107
nuclear@0 108 Vector3::operator Vector4() const {
nuclear@0 109 return Vector4(x, y, z, 1.0f);
nuclear@0 110 }
nuclear@0 111
nuclear@0 112
nuclear@0 113 float Vector3::Length() const {
nuclear@0 114 return (float)sqrt(x*x + y*y + z*z);
nuclear@0 115 }
nuclear@0 116
nuclear@0 117 float Vector3::LengthSq() const {
nuclear@0 118 return x*x + y*y + z*z;
nuclear@0 119 }
nuclear@0 120
nuclear@0 121 void Vector3::Normalize() {
nuclear@0 122 float len = (float)sqrt(x*x + y*y + z*z);
nuclear@0 123 x /= len;
nuclear@0 124 y /= len;
nuclear@0 125 z /= len;
nuclear@0 126 }
nuclear@0 127
nuclear@0 128 Vector3 Vector3::Normalized() const {
nuclear@0 129 float len = (float)sqrt(x*x + y*y + z*z);
nuclear@0 130 return Vector3(x / len, y / len, z / len);
nuclear@0 131 }
nuclear@0 132
nuclear@0 133 Vector3 Vector3::Reflection(const Vector3 &normal) const {
nuclear@0 134 return normal * this->DotProduct(normal) * 2.0f - *this;
nuclear@0 135 }
nuclear@0 136 */
nuclear@0 137 Vector3 Vector3::Refraction(const Vector3 &normal, float FromIOR, float ToIOR) const {
nuclear@0 138 float m = FromIOR / ToIOR;
nuclear@0 139 Vector3 dir = *this;
nuclear@0 140 dir.Normalize();
nuclear@0 141 float CosAngleIncoming = dir.DotProduct(normal);
nuclear@0 142 float CosAngleRefr = (1.0f / (m*m)) * (float)sqrt(1.0f - m*m * (1 - CosAngleIncoming * CosAngleIncoming));
nuclear@0 143
nuclear@0 144 return dir * m - normal * (CosAngleRefr + m * CosAngleIncoming);
nuclear@0 145 }
nuclear@0 146
nuclear@0 147 void Vector3::Transform(const Matrix4x4 &mat) {
nuclear@0 148 // assume row vectors
nuclear@0 149 float nx = x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0] + mat.m[3][0];
nuclear@0 150 float ny = x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1] + mat.m[3][1];
nuclear@0 151 z = x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] + mat.m[3][2];
nuclear@0 152 x = nx;
nuclear@0 153 y = ny;
nuclear@0 154 }
nuclear@0 155
nuclear@0 156 void Vector3::Transform(const Quaternion &quat) {
nuclear@0 157 Quaternion vq(0.0f, *this);
nuclear@0 158 vq = quat * vq * quat.Inverse();
nuclear@0 159 *this = vq.v;
nuclear@0 160 }
nuclear@0 161
nuclear@0 162 // direct transformations
nuclear@0 163
nuclear@0 164 void Vector3::Translate(float x, float y, float z) {
nuclear@0 165 this->x += x;
nuclear@0 166 this->y += y;
nuclear@0 167 this->z += z;
nuclear@0 168 }
nuclear@0 169
nuclear@0 170 void Vector3::Rotate(float x, float y, float z) {
nuclear@0 171
nuclear@0 172 Matrix4x4 xform;
nuclear@0 173 xform.SetRotation(x, y, z);
nuclear@0 174
nuclear@0 175 Transform(xform);
nuclear@0 176 }
nuclear@0 177
nuclear@0 178 void Vector3::Rotate(const Vector3 &axis, float angle) {
nuclear@0 179
nuclear@0 180 Matrix4x4 xform;
nuclear@0 181 xform.SetRotation(axis, angle);
nuclear@0 182
nuclear@0 183 Transform(xform);
nuclear@0 184 }
nuclear@0 185
nuclear@0 186 void Vector3::Scale(float x, float y, float z) {
nuclear@0 187 this->x *= x;
nuclear@0 188 this->y *= y;
nuclear@0 189 this->z *= z;
nuclear@0 190 }
nuclear@0 191
nuclear@0 192 float &Vector3::operator [](int index) {
nuclear@0 193 return !index ? x : index == 1 ? y : z;
nuclear@0 194 }
nuclear@0 195
nuclear@0 196 std::ostream &operator <<(std::ostream &out, const Vector3 &vec) {
nuclear@0 197 out << vec.x << ", " << vec.y << ", " << vec.z;
nuclear@0 198 return out;
nuclear@0 199 }
nuclear@0 200
nuclear@0 201 // ------------- Vector4 implementation ---------------
nuclear@0 202
nuclear@0 203 Vector4::Vector4() {
nuclear@0 204 x = y = z = 0.0f;
nuclear@0 205 }
nuclear@0 206
nuclear@0 207 Vector4::Vector4(const Vector4 &vec) {
nuclear@0 208 x = vec.x;
nuclear@0 209 y = vec.y;
nuclear@0 210 z = vec.z;
nuclear@0 211 w = vec.w;
nuclear@0 212 }
nuclear@0 213
nuclear@0 214 Vector4::Vector4(const Vector3 &vec) {
nuclear@0 215 x = vec.x;
nuclear@0 216 y = vec.y;
nuclear@0 217 z = vec.z;
nuclear@0 218 w = 1.0f;
nuclear@0 219 }
nuclear@0 220
nuclear@0 221 Vector4::Vector4(float x, float y, float z, float w) {
nuclear@0 222 this->x = x;
nuclear@0 223 this->y = y;
nuclear@0 224 this->z = z;
nuclear@0 225 this->w = w;
nuclear@0 226 }
nuclear@0 227
nuclear@0 228 float Vector4::DotProduct(const Vector4 &vec) const {
nuclear@0 229 return x * vec.x + y * vec.y + z * vec.z + w * vec.w;
nuclear@0 230 }
nuclear@0 231
nuclear@0 232 float DotProduct(const Vector4 &vec1, const Vector4 &vec2) {
nuclear@0 233 return vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z + vec1.w * vec2.w;
nuclear@0 234 }
nuclear@0 235
nuclear@0 236 Vector4 Vector4::CrossProduct(const Vector4 &vec1, const Vector4 &vec2) const {
nuclear@0 237 float A, B, C, D, E, F; // Intermediate Values
nuclear@0 238 Vector4 result;
nuclear@0 239
nuclear@0 240 // Calculate intermediate values.
nuclear@0 241 A = (vec1.x * vec2.y) - (vec1.y * vec2.x);
nuclear@0 242 B = (vec1.x * vec2.z) - (vec1.z * vec2.x);
nuclear@0 243 C = (vec1.x * vec2.w) - (vec1.w * vec2.x);
nuclear@0 244 D = (vec1.y * vec2.z) - (vec1.z * vec2.y);
nuclear@0 245 E = (vec1.y * vec2.w) - (vec1.w * vec2.y);
nuclear@0 246 F = (vec1.z * vec2.w) - (vec1.w * vec2.z);
nuclear@0 247
nuclear@0 248 // Calculate the result-vector components.
nuclear@0 249 result.x = (y * F) - (z * E) + (w * D);
nuclear@0 250 result.y = - (x * F) + (z * C) - (w * B);
nuclear@0 251 result.z = (x * E) - (y * C) + (w * A);
nuclear@0 252 result.w = - (x * D) + (y * B) - (z * A);
nuclear@0 253 return result;
nuclear@0 254 }
nuclear@0 255
nuclear@0 256 Vector4 CrossProduct(const Vector4 &vec1, const Vector4 &vec2, const Vector4 &vec3) {
nuclear@0 257 float A, B, C, D, E, F; // Intermediate Values
nuclear@0 258 Vector4 result;
nuclear@0 259
nuclear@0 260 // Calculate intermediate values.
nuclear@0 261 A = (vec2.x * vec3.y) - (vec2.y * vec3.x);
nuclear@0 262 B = (vec2.x * vec3.z) - (vec2.z * vec3.x);
nuclear@0 263 C = (vec2.x * vec3.w) - (vec2.w * vec3.x);
nuclear@0 264 D = (vec2.y * vec3.z) - (vec2.z * vec3.y);
nuclear@0 265 E = (vec2.y * vec3.w) - (vec2.w * vec3.y);
nuclear@0 266 F = (vec2.z * vec3.w) - (vec2.w * vec3.z);
nuclear@0 267
nuclear@0 268 // Calculate the result-vector components.
nuclear@0 269 result.x = (vec1.y * F) - (vec1.z * E) + (vec1.w * D);
nuclear@0 270 result.y = - (vec1.x * F) + (vec1.z * C) - (vec1.w * B);
nuclear@0 271 result.z = (vec1.x * E) - (vec1.y * C) + (vec1.w * A);
nuclear@0 272 result.w = - (vec1.x * D) + (vec1.y * B) - (vec1.z * A);
nuclear@0 273 return result;
nuclear@0 274 }
nuclear@0 275
nuclear@0 276 Vector4 Vector4::operator +(const Vector4 &vec) const {
nuclear@0 277 return Vector4(x + vec.x, y + vec.y, z + vec.z, w + vec.w);
nuclear@0 278 }
nuclear@0 279
nuclear@0 280 Vector4 Vector4::operator -(const Vector4 &vec) const {
nuclear@0 281 return Vector4(x - vec.x, y - vec.y, z - vec.z, w - vec.w);
nuclear@0 282 }
nuclear@0 283
nuclear@0 284 Vector4 Vector4::operator *(float scalar) const {
nuclear@0 285 return Vector4(x * scalar, y * scalar, z * scalar, w * scalar);
nuclear@0 286 }
nuclear@0 287
nuclear@0 288 Vector4 Vector4::operator /(float scalar) const {
nuclear@0 289 return Vector4(x / scalar, y / scalar, z / scalar, w / scalar);
nuclear@0 290 }
nuclear@0 291
nuclear@0 292 void Vector4::operator +=(const Vector4 &vec) {
nuclear@0 293 x += vec.x;
nuclear@0 294 y += vec.y;
nuclear@0 295 z += vec.z;
nuclear@0 296 w += vec.w;
nuclear@0 297 }
nuclear@0 298
nuclear@0 299 void Vector4::operator -=(const Vector4 &vec) {
nuclear@0 300 x -= vec.x;
nuclear@0 301 y -= vec.y;
nuclear@0 302 z -= vec.z;
nuclear@0 303 w -= vec.w;
nuclear@0 304 }
nuclear@0 305
nuclear@0 306 void Vector4::operator *=(float scalar) {
nuclear@0 307 x *= scalar;
nuclear@0 308 y *= scalar;
nuclear@0 309 z *= scalar;
nuclear@0 310 w *= scalar;
nuclear@0 311 }
nuclear@0 312
nuclear@0 313 void Vector4::operator /=(float scalar) {
nuclear@0 314 x /= scalar;
nuclear@0 315 y /= scalar;
nuclear@0 316 z /= scalar;
nuclear@0 317 w /= scalar;
nuclear@0 318 }
nuclear@0 319
nuclear@0 320 Vector4 Vector4::operator -() const {
nuclear@0 321 return Vector4(-x, -y, -z, -w);
nuclear@0 322 }
nuclear@0 323
nuclear@0 324
nuclear@0 325 bool Vector4::operator >(const Vector4 &vec) const {
nuclear@0 326 return LengthSq() > vec.LengthSq();
nuclear@0 327 }
nuclear@0 328
nuclear@0 329 bool Vector4::operator <(const Vector4 &vec) const {
nuclear@0 330 return LengthSq() < vec.LengthSq();
nuclear@0 331 }
nuclear@0 332
nuclear@0 333 bool Vector4::operator >(float len) const {
nuclear@0 334 return LengthSq() > len;
nuclear@0 335 }
nuclear@0 336
nuclear@0 337 bool Vector4::operator <(float len) const {
nuclear@0 338 return LengthSq() < len;
nuclear@0 339 }
nuclear@0 340
nuclear@0 341 bool Vector4::operator ==(const Vector4 &vec) const {
nuclear@0 342 return ((*this - vec).Length() < XSmallNumber);
nuclear@0 343 }
nuclear@0 344
nuclear@0 345 bool Vector4::operator ==(float len) const {
nuclear@0 346 return ((this->Length() - len) < XSmallNumber);
nuclear@0 347 }
nuclear@0 348
nuclear@0 349 Vector4::operator Vector3() const {
nuclear@0 350 return Vector3(x, y, z);
nuclear@0 351 }
nuclear@0 352
nuclear@0 353
nuclear@0 354 float Vector4::Length() const {
nuclear@0 355 return (float)sqrt(x*x + y*y + z*z + w*w);
nuclear@0 356 }
nuclear@0 357
nuclear@0 358 float Vector4::LengthSq() const {
nuclear@0 359 return x*x + y*y + z*z + w*w;
nuclear@0 360 }
nuclear@0 361
nuclear@0 362 void Vector4::Normalize() {
nuclear@0 363 float len = (float)sqrt(x*x + y*y + z*z + w*w);
nuclear@0 364 x /= len;
nuclear@0 365 y /= len;
nuclear@0 366 z /= len;
nuclear@0 367 w /= len;
nuclear@0 368 }
nuclear@0 369
nuclear@0 370 Vector4 Vector4::Normalized() const {
nuclear@0 371 float len = (float)sqrt(x*x + y*y + z*z + w*w);
nuclear@0 372 return Vector4(x / len, y / len, z / len, w / len);
nuclear@0 373 }
nuclear@0 374
nuclear@0 375 void Vector4::Transform(const Matrix4x4 &mat) {
nuclear@0 376 // assume row vectors
nuclear@0 377 float nx = x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0] + w * mat.m[3][0];
nuclear@0 378 float ny = x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1] + w * mat.m[3][1];
nuclear@0 379 float nz = x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] + w * mat.m[3][2];
nuclear@0 380 w = x * mat.m[0][3] + y * mat.m[1][3] + z * mat.m[2][3] + w * mat.m[3][3];
nuclear@0 381 x = nx;
nuclear@0 382 y = ny;
nuclear@0 383 z = nz;
nuclear@0 384 }
nuclear@0 385
nuclear@0 386
nuclear@0 387 // Direct transformations on the vector
nuclear@0 388 void Vector4::Translate(float x, float y, float z, float w) {
nuclear@0 389 x += x;
nuclear@0 390 y += y;
nuclear@0 391 z += z;
nuclear@0 392 w += w;
nuclear@0 393 }
nuclear@0 394
nuclear@0 395 void Vector4::Rotate(float x, float y, float z) {
nuclear@0 396 Matrix4x4 xform;
nuclear@0 397 xform.SetRotation(x, y, z);
nuclear@0 398 Transform(xform);
nuclear@0 399 }
nuclear@0 400
nuclear@0 401 void Vector4::Rotate(const Vector3 &axis, float angle) {
nuclear@0 402 Matrix4x4 xform;
nuclear@0 403 xform.SetRotation(axis, angle);
nuclear@0 404 Transform(xform);
nuclear@0 405 }
nuclear@0 406
nuclear@0 407 void Vector4::Scale(float x, float y, float z, float w) {
nuclear@0 408 this->x *= x;
nuclear@0 409 this->y *= y;
nuclear@0 410 this->z *= z;
nuclear@0 411 this->w *= w;
nuclear@0 412 }
nuclear@0 413
nuclear@0 414 float &Vector4::operator [](int index) {
nuclear@0 415 return !index ? x : index == 1 ? y : index == 2 ? z : w;
nuclear@0 416 }
nuclear@0 417
nuclear@0 418 std::ostream &operator <<(std::ostream &out, const Vector4 &vec) {
nuclear@0 419 out << vec.x << ", " << vec.y << ", " << vec.z << ", " << vec.w;
nuclear@0 420 return out;
nuclear@0 421 }
nuclear@0 422
nuclear@0 423 // ------------- Vector2 implementation ---------------
nuclear@0 424
nuclear@0 425 Vector2::Vector2() {
nuclear@0 426 x = y = 0.0f;
nuclear@0 427 }
nuclear@0 428
nuclear@0 429 Vector2::Vector2(const Vector2 &vec) {
nuclear@0 430 x = vec.x;
nuclear@0 431 y = vec.y;
nuclear@0 432 }
nuclear@0 433
nuclear@0 434 Vector2::Vector2(float x, float y) {
nuclear@0 435 this->x = x;
nuclear@0 436 this->y = y;
nuclear@0 437 }
nuclear@0 438
nuclear@0 439 float Vector2::DotProduct(const Vector2 &vec) const {
nuclear@0 440 return x * vec.x + y * vec.y;
nuclear@0 441 }
nuclear@0 442
nuclear@0 443 float DotProduct(const Vector2 &vec1, const Vector2 &vec2) {
nuclear@0 444 return vec1.x * vec2.x + vec1.y + vec2.y;
nuclear@0 445 }
nuclear@0 446
nuclear@0 447 Vector2 Vector2::operator +(const Vector2 &vec) const {
nuclear@0 448 return Vector2(x + vec.x, y + vec.y);
nuclear@0 449 }
nuclear@0 450
nuclear@0 451 Vector2 Vector2::operator -(const Vector2 &vec) const {
nuclear@0 452 return Vector2(x - vec.x, y - vec.y);
nuclear@0 453 }
nuclear@0 454
nuclear@0 455 Vector2 Vector2::operator *(float scalar) const {
nuclear@0 456 return Vector2(x * scalar, y * scalar);
nuclear@0 457 }
nuclear@0 458
nuclear@0 459 Vector2 Vector2::operator /(float scalar) const {
nuclear@0 460 return Vector2(x / scalar, y / scalar);
nuclear@0 461 }
nuclear@0 462
nuclear@0 463 void Vector2::operator +=(const Vector2 &vec) {
nuclear@0 464 x += vec.x;
nuclear@0 465 y += vec.y;
nuclear@0 466 }
nuclear@0 467
nuclear@0 468 void Vector2::operator -=(const Vector2 &vec) {
nuclear@0 469 x -= vec.x;
nuclear@0 470 y -= vec.y;
nuclear@0 471 }
nuclear@0 472
nuclear@0 473 void Vector2::operator *=(float scalar) {
nuclear@0 474 x *= scalar;
nuclear@0 475 y *= scalar;
nuclear@0 476 }
nuclear@0 477
nuclear@0 478 void Vector2::operator /=(float scalar) {
nuclear@0 479 x /= scalar;
nuclear@0 480 y /= scalar;
nuclear@0 481 }
nuclear@0 482
nuclear@0 483 Vector2 Vector2::operator -() const {
nuclear@0 484 return Vector2(-x, -y);
nuclear@0 485 }
nuclear@0 486
nuclear@0 487 bool Vector2::operator >(const Vector2 &vec) const {
nuclear@0 488 return LengthSq() > vec.LengthSq();
nuclear@0 489 }
nuclear@0 490
nuclear@0 491 bool Vector2::operator <(const Vector2 &vec) const {
nuclear@0 492 return LengthSq() < vec.LengthSq();
nuclear@0 493 }
nuclear@0 494
nuclear@0 495 bool Vector2::operator >(float len) const {
nuclear@0 496 return LengthSq() > len;
nuclear@0 497 }
nuclear@0 498
nuclear@0 499 bool Vector2::operator <(float len) const {
nuclear@0 500 return LengthSq() < len;
nuclear@0 501 }
nuclear@0 502
nuclear@0 503 bool Vector2::operator ==(const Vector2 &vec) const {
nuclear@0 504 return ((*this - vec).Length() < XSmallNumber);
nuclear@0 505 }
nuclear@0 506
nuclear@0 507 bool Vector2::operator ==(float len) const {
nuclear@0 508 return ((this->Length() - len) < XSmallNumber);
nuclear@0 509 }
nuclear@0 510
nuclear@0 511 Vector2::operator Vector3() const {
nuclear@0 512 return Vector3(x, y, 1.0f);
nuclear@0 513 }
nuclear@0 514
nuclear@0 515 float Vector2::Length() const {
nuclear@0 516 return (float)sqrt(x * x + y * y);
nuclear@0 517 }
nuclear@0 518
nuclear@0 519 float Vector2::LengthSq() const {
nuclear@0 520 return x * x + y * y;
nuclear@0 521 }
nuclear@0 522
nuclear@0 523 void Vector2::Normalize() {
nuclear@0 524 float len = (float)sqrt(x * x + y * y);
nuclear@0 525 x /= len;
nuclear@0 526 y /= len;
nuclear@0 527 }
nuclear@0 528
nuclear@0 529 Vector2 Vector2::Normalized() const {
nuclear@0 530 float len = (float)sqrt(x * x + y * y);
nuclear@0 531 return Vector2(x / len, y / len);
nuclear@0 532 }
nuclear@0 533
nuclear@0 534 //Vector2 Vector2::Reflection(const Vector2 &normal) const;
nuclear@0 535 //Vector2 Vector2::Refraction(const Vector2 &normal, float FromIOR, float ToIOR) const;
nuclear@0 536
nuclear@0 537 void Vector2::Transform(const Matrix3x3 &mat) {
nuclear@0 538 float nx = x * mat.m[0][0] + y * mat.m[1][0] + mat.m[2][0];
nuclear@0 539 y = x * mat.m[0][1] + y * mat.m[1][1] + mat.m[2][1];
nuclear@0 540 x = nx;
nuclear@0 541 }
nuclear@0 542
nuclear@0 543 void Vector2::Translate(float x, float y) {
nuclear@0 544 this->x += x;
nuclear@0 545 this->y += y;
nuclear@0 546 }
nuclear@0 547
nuclear@0 548 void Vector2::Rotate(float angle) {
nuclear@0 549 Matrix3x3 xform;
nuclear@0 550 xform.SetRotation(angle);
nuclear@0 551
nuclear@0 552 Transform(xform);
nuclear@0 553 }
nuclear@0 554
nuclear@0 555 void Vector2::Scale(float x, float y) {
nuclear@0 556 this->x *= x;
nuclear@0 557 this->y *= y;
nuclear@0 558 }
nuclear@0 559
nuclear@0 560 float &Vector2::operator [](int index) {
nuclear@0 561 return !index ? x : y;
nuclear@0 562 }
nuclear@0 563
nuclear@0 564 std::ostream &operator <<(std::ostream &out, const Vector2 &vec) {
nuclear@0 565 out << vec.x << ", " << vec.y;
nuclear@0 566 return out;
nuclear@0 567 }
nuclear@0 568
nuclear@0 569
nuclear@0 570 // --------------- Quaternion implementation ---------------
nuclear@0 571
nuclear@0 572 Quaternion::Quaternion() {
nuclear@0 573 s = 1.0f;
nuclear@0 574 v.x = v.y = v.z = 0.0f;
nuclear@0 575 }
nuclear@0 576
nuclear@0 577 Quaternion::Quaternion(float s, float x, float y, float z) {
nuclear@0 578 v.x = x;
nuclear@0 579 v.y = y;
nuclear@0 580 v.z = z;
nuclear@0 581 this->s = s;
nuclear@0 582 }
nuclear@0 583
nuclear@0 584 Quaternion::Quaternion(float s, const Vector3 &v) {
nuclear@0 585 this->s = s;
nuclear@0 586 this->v = v;
nuclear@0 587 }
nuclear@0 588
nuclear@0 589 Quaternion Quaternion::operator +(const Quaternion &quat) const {
nuclear@0 590 return Quaternion(s + quat.s, v + quat.v);
nuclear@0 591 }
nuclear@0 592
nuclear@0 593 Quaternion Quaternion::operator -(const Quaternion &quat) const {
nuclear@0 594 return Quaternion(s - quat.s, v - quat.v);
nuclear@0 595 }
nuclear@0 596
nuclear@0 597 Quaternion Quaternion::operator -() const {
nuclear@0 598 return Quaternion(-s, -v);
nuclear@0 599 }
nuclear@0 600
nuclear@0 601 // Quaternion Multiplication:
nuclear@0 602 // Q1*Q2 = [s1*s2 - v1.v2, s1*v2 + s2*v1 + v1(x)v2]
nuclear@0 603 Quaternion Quaternion::operator *(const Quaternion &quat) const {
nuclear@0 604 Quaternion newq;
nuclear@0 605 newq.s = s * quat.s - DotProduct(v, quat.v);
nuclear@0 606 newq.v = quat.v * s + v * quat.s + CrossProduct(v, quat.v);
nuclear@0 607 return newq;
nuclear@0 608 }
nuclear@0 609
nuclear@0 610 void Quaternion::operator +=(const Quaternion &quat) {
nuclear@0 611 *this = Quaternion(s + quat.s, v + quat.v);
nuclear@0 612 }
nuclear@0 613
nuclear@0 614 void Quaternion::operator -=(const Quaternion &quat) {
nuclear@0 615 *this = Quaternion(s - quat.s, v - quat.v);
nuclear@0 616 }
nuclear@0 617
nuclear@0 618 void Quaternion::operator *=(const Quaternion &quat) {
nuclear@0 619 *this = *this * quat;
nuclear@0 620 }
nuclear@0 621
nuclear@0 622 void Quaternion::ResetIdentity() {
nuclear@0 623 s = 1.0f;
nuclear@0 624 v.x = v.y = v.z = 0.0f;
nuclear@0 625 }
nuclear@0 626
nuclear@0 627 Quaternion Quaternion::Conjugate() const {
nuclear@0 628 return Quaternion(s, -v);
nuclear@0 629 }
nuclear@0 630
nuclear@0 631 float Quaternion::Length() const {
nuclear@0 632 return (float)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
nuclear@0 633 }
nuclear@0 634
nuclear@0 635 // Q * ~Q = ||Q||^2
nuclear@0 636 float Quaternion::LengthSq() const {
nuclear@0 637 return v.x*v.x + v.y*v.y + v.z*v.z + s*s;
nuclear@0 638 }
nuclear@0 639
nuclear@0 640 void Quaternion::Normalize() {
nuclear@0 641 float len = (float)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
nuclear@0 642 v.x /= len;
nuclear@0 643 v.y /= len;
nuclear@0 644 v.z /= len;
nuclear@0 645 s /= len;
nuclear@0 646 }
nuclear@0 647
nuclear@0 648 Quaternion Quaternion::Normalized() const {
nuclear@0 649 Quaternion nq = *this;
nuclear@0 650 float len = (float)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
nuclear@0 651 nq.v.x /= len;
nuclear@0 652 nq.v.y /= len;
nuclear@0 653 nq.v.z /= len;
nuclear@0 654 nq.s /= len;
nuclear@0 655 return nq;
nuclear@0 656 }
nuclear@0 657
nuclear@0 658 // Quaternion Inversion: Q^-1 = ~Q / ||Q||^2
nuclear@0 659 Quaternion Quaternion::Inverse() const {
nuclear@0 660 Quaternion inv = Conjugate();
nuclear@0 661 float lensq = LengthSq();
nuclear@0 662 inv.v /= lensq;
nuclear@0 663 inv.s /= lensq;
nuclear@0 664
nuclear@0 665 return inv;
nuclear@0 666 }
nuclear@0 667
nuclear@0 668
nuclear@0 669 void Quaternion::SetRotation(const Vector3 &axis, float angle) {
nuclear@0 670 float HalfAngle = angle / 2.0f;
nuclear@0 671 s = cosf(HalfAngle);
nuclear@0 672 v = axis * sinf(HalfAngle);
nuclear@0 673 }
nuclear@0 674
nuclear@0 675 void Quaternion::Rotate(const Vector3 &axis, float angle) {
nuclear@0 676 Quaternion q;
nuclear@0 677 float HalfAngle = angle / 2.0f;
nuclear@0 678 q.s = cosf(HalfAngle);
nuclear@0 679 q.v = axis * sinf(HalfAngle);
nuclear@0 680
nuclear@0 681 *this *= q;
nuclear@0 682 }
nuclear@0 683
nuclear@0 684
nuclear@0 685 Matrix3x3 Quaternion::GetRotationMatrix() const {
nuclear@0 686 return Matrix3x3(1.0f - 2.0f * v.y*v.y - 2.0f * v.z*v.z, 2.0f * v.x * v.y + 2.0f * s * v.z, 2.0f * v.z * v.x - 2.0f * s * v.y,
nuclear@0 687 2.0f * v.x * v.y - 2.0f * s * v.z, 1.0f - 2.0f * v.x*v.x - 2.0f * v.z*v.z, 2.0f * v.y * v.z + 2.0f * s * v.x,
nuclear@0 688 2.0f * v.z * v.x + 2.0f * s * v.y, 2.0f * v.y * v.z - 2.0f * s * v.x, 1.0f - 2.0f * v.x*v.x - 2.0f * v.y*v.y);
nuclear@0 689 }
nuclear@0 690
nuclear@0 691
nuclear@0 692 // ------------- Matrix implementation ---------------
nuclear@0 693
nuclear@0 694 Matrix4x4::Matrix4x4() {
nuclear@0 695 memset(m, 0, 16*sizeof(float));
nuclear@0 696 m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0f;
nuclear@0 697 }
nuclear@0 698
nuclear@0 699 Matrix4x4::Matrix4x4(const Matrix4x4 &mat) {
nuclear@0 700 memcpy(m, mat.m, 16*sizeof(float));
nuclear@0 701 }
nuclear@0 702
nuclear@0 703 Matrix4x4::Matrix4x4(const Matrix3x3 &mat) {
nuclear@0 704 for(int i=0; i<3; i++) {
nuclear@0 705 for(int j=0; j<3; j++) {
nuclear@0 706 m[i][j] = mat.m[i][j];
nuclear@0 707 }
nuclear@0 708 }
nuclear@0 709 m[3][0] = m[3][1] = m[3][2] = m[0][3] = m[1][3] = m[2][3] = 0.0f;
nuclear@0 710 m[3][3] = 1.0f;
nuclear@0 711 }
nuclear@0 712
nuclear@0 713 Matrix4x4::Matrix4x4( float m00, float m01, float m02, float m03,
nuclear@0 714 float m10, float m11, float m12, float m13,
nuclear@0 715 float m20, float m21, float m22, float m23,
nuclear@0 716 float m30, float m31, float m32, float m33 ) {
nuclear@0 717
nuclear@0 718 memcpy(m, &m00, 16*sizeof(float)); // arguments are adjacent in stack
nuclear@0 719 }
nuclear@0 720
nuclear@0 721 Matrix4x4 Matrix4x4::operator +(const Matrix4x4 &mat) const {
nuclear@0 722
nuclear@0 723 Matrix4x4 tmp;
nuclear@0 724
nuclear@0 725 const float *op1 = (float*)m;
nuclear@0 726 const float *op2 = (float*)mat.m;
nuclear@0 727 float *dst = (float*)tmp.m;
nuclear@0 728
nuclear@0 729 for(int i=0; i<16; i++) *dst++ = *op1++ + *op2++;
nuclear@0 730
nuclear@0 731 return tmp;
nuclear@0 732 }
nuclear@0 733
nuclear@0 734 Matrix4x4 Matrix4x4::operator -(const Matrix4x4 &mat) const {
nuclear@0 735
nuclear@0 736 Matrix4x4 tmp;
nuclear@0 737
nuclear@0 738 const float *op1 = (float*)m;
nuclear@0 739 const float *op2 = (float*)mat.m;
nuclear@0 740 float *dst = (float*)tmp.m;
nuclear@0 741
nuclear@0 742 for(int i=0; i<16; i++) *dst++ = *op1++ - *op2++;
nuclear@0 743
nuclear@0 744 return tmp;
nuclear@0 745 }
nuclear@0 746
nuclear@0 747 Matrix4x4 Matrix4x4::operator *(float scalar) const {
nuclear@0 748
nuclear@0 749 Matrix4x4 tmp;
nuclear@0 750
nuclear@0 751 const float *op1 = (float*)m;
nuclear@0 752 float *dst = (float*)tmp.m;
nuclear@0 753
nuclear@0 754 for(int i=0; i<16; i++) *dst++ = *op1++ * scalar;
nuclear@0 755
nuclear@0 756 return tmp;
nuclear@0 757 }
nuclear@0 758
nuclear@0 759 Matrix4x4 Matrix4x4::operator *(const Matrix4x4 &mat) const {
nuclear@0 760 Matrix4x4 tmp;
nuclear@0 761
nuclear@0 762 for(int i=0; i<4; i++) {
nuclear@0 763 for(int j=0; j<4; j++) {
nuclear@0 764 tmp.m[i][j] = m[i][0]*mat.m[0][j] + m[i][1]*mat.m[1][j] + m[i][2]*mat.m[2][j] + m[i][3]*mat.m[3][j];
nuclear@0 765 }
nuclear@0 766 }
nuclear@0 767
nuclear@0 768 return tmp;
nuclear@0 769 }
nuclear@0 770
nuclear@0 771 void Matrix4x4::operator +=(const Matrix4x4 &mat) {
nuclear@0 772
nuclear@0 773 const float *op2 = (float*)mat.m;
nuclear@0 774 float *dst = (float*)m;
nuclear@0 775
nuclear@0 776 for(int i=0; i<16; i++) *dst++ += *op2++;
nuclear@0 777 }
nuclear@0 778
nuclear@0 779 void Matrix4x4::operator -=(const Matrix4x4 &mat) {
nuclear@0 780
nuclear@0 781 const float *op2 = (float*)mat.m;
nuclear@0 782 float *dst = (float*)m;
nuclear@0 783
nuclear@0 784 for(int i=0; i<16; i++) *dst++ -= *op2++;
nuclear@0 785 }
nuclear@0 786
nuclear@0 787 void Matrix4x4::operator *=(float scalar) {
nuclear@0 788
nuclear@0 789 float *dst = (float*)m;
nuclear@0 790
nuclear@0 791 for(int i=0; i<16; i++) *dst++ *= scalar;
nuclear@0 792 }
nuclear@0 793
nuclear@0 794 void Matrix4x4::operator *=(const Matrix4x4 &mat) {
nuclear@0 795 Matrix4x4 tmp;
nuclear@0 796
nuclear@0 797 for(int i=0; i<4; i++) {
nuclear@0 798 for(int j=0; j<4; j++) {
nuclear@0 799 tmp.m[i][j] = m[i][0]*mat.m[0][j] + m[i][1]*mat.m[1][j] + m[i][2]*mat.m[2][j] + m[i][3]*mat.m[3][j];
nuclear@0 800 }
nuclear@0 801 }
nuclear@0 802
nuclear@0 803 memcpy(m, tmp.m, 16*sizeof(float));
nuclear@0 804 }
nuclear@0 805
nuclear@0 806
nuclear@0 807 void Matrix4x4::ResetIdentity() {
nuclear@0 808 memset(m, 0, 16*sizeof(float));
nuclear@0 809 m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0f;
nuclear@0 810 }
nuclear@0 811
nuclear@0 812
nuclear@0 813 // Transformations (assuming column vectors)
nuclear@0 814
nuclear@0 815 void Matrix4x4::Translate(float x, float y, float z) {
nuclear@0 816
nuclear@0 817 Matrix4x4 tmp( 1, 0, 0, 0,
nuclear@0 818 0, 1, 0, 0,
nuclear@0 819 0, 0, 1, 0,
nuclear@0 820 x, y, z, 1 );
nuclear@0 821 *this *= tmp;
nuclear@0 822 }
nuclear@0 823
nuclear@0 824 void Matrix4x4::Rotate(float x, float y, float z) {
nuclear@0 825
nuclear@0 826 *this *= Matrix4x4( 1, 0, 0, 0,
nuclear@0 827 0, fcos(x), fsin(x), 0,
nuclear@0 828 0, -fsin(x), fcos(x), 0,
nuclear@0 829 0, 0, 0, 1 );
nuclear@0 830
nuclear@0 831 *this *= Matrix4x4( fcos(y), 0, -fsin(y), 0,
nuclear@0 832 0, 1, 0, 0,
nuclear@0 833 fsin(y), 0, fcos(y), 0,
nuclear@0 834 0, 0, 0, 1 );
nuclear@0 835
nuclear@0 836 *this *= Matrix4x4( fcos(z), fsin(z), 0, 0,
nuclear@0 837 -fsin(z), fcos(z), 0, 0,
nuclear@0 838 0, 0, 1, 0,
nuclear@0 839 0, 0, 0, 1 );
nuclear@0 840 }
nuclear@0 841
nuclear@0 842 void Matrix4x4::Rotate(const Vector3 &axis, float angle) {
nuclear@0 843
nuclear@0 844 float sina = fsin(angle);
nuclear@0 845 float cosa = fcos(angle);
nuclear@0 846 float invcosa = 1-cosa;
nuclear@0 847 float nxsq = axis.x * axis.x;
nuclear@0 848 float nysq = axis.y * axis.y;
nuclear@0 849 float nzsq = axis.z * axis.z;
nuclear@0 850
nuclear@0 851 Matrix4x4 xform;
nuclear@0 852 xform.m[0][0] = nxsq + (1-nxsq) * cosa;
nuclear@0 853 xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
nuclear@0 854 xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
nuclear@0 855 xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
nuclear@0 856 xform.m[1][1] = nysq + (1-nysq) * cosa;
nuclear@0 857 xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
nuclear@0 858 xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
nuclear@0 859 xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
nuclear@0 860 xform.m[2][2] = nzsq + (1-nzsq) * cosa;
nuclear@0 861
nuclear@0 862 *this *= xform;
nuclear@0 863 }
nuclear@0 864
nuclear@0 865 void Matrix4x4::Scale(float x, float y, float z) {
nuclear@0 866
nuclear@0 867 Matrix4x4 xform(x, 0, 0, 0,
nuclear@0 868 0, y, 0, 0,
nuclear@0 869 0, 0, z, 0,
nuclear@0 870 0, 0, 0, 1 );
nuclear@0 871 *this *= xform;
nuclear@0 872 }
nuclear@0 873
nuclear@0 874
nuclear@0 875 //////////////////////////////
nuclear@0 876
nuclear@0 877 void Matrix4x4::SetTranslation(float x, float y, float z) {
nuclear@0 878
nuclear@0 879 *this = Matrix4x4( 1, 0, 0, 0,
nuclear@0 880 0, 1, 0, 0,
nuclear@0 881 0, 0, 1, 0,
nuclear@0 882 x, y, z, 1 );
nuclear@0 883 }
nuclear@0 884
nuclear@0 885 void Matrix4x4::SetRotation(float x, float y, float z) {
nuclear@0 886
nuclear@0 887 *this = Matrix4x4( 1, 0, 0, 0,
nuclear@0 888 0, fcos(x), fsin(x), 0,
nuclear@0 889 0, -fsin(x), fcos(x), 0,
nuclear@0 890 0, 0, 0, 1 );
nuclear@0 891
nuclear@0 892 *this *= Matrix4x4( fcos(y), 0, -fsin(y), 0,
nuclear@0 893 0, 1, 0, 0,
nuclear@0 894 fsin(y), 0, fcos(y), 0,
nuclear@0 895 0, 0, 0, 1 );
nuclear@0 896
nuclear@0 897 *this *= Matrix4x4( fcos(z), fsin(z), 0, 0,
nuclear@0 898 -fsin(z), fcos(z), 0, 0,
nuclear@0 899 0, 0, 1, 0,
nuclear@0 900 0, 0, 0, 1 );
nuclear@0 901 }
nuclear@0 902
nuclear@0 903 void Matrix4x4::SetRotation(const Vector3 &axis, float angle) {
nuclear@0 904
nuclear@0 905 // caching of multiply used function results (opt)
nuclear@0 906 float sina = fsin(angle);
nuclear@0 907 float cosa = fcos(angle);
nuclear@0 908 float invcosa = 1-cosa;
nuclear@0 909 float nxsq = axis.x * axis.x;
nuclear@0 910 float nysq = axis.y * axis.y;
nuclear@0 911 float nzsq = axis.z * axis.z;
nuclear@0 912
nuclear@0 913 Matrix4x4 xform;
nuclear@0 914 xform.m[0][0] = nxsq + (1-nxsq) * cosa;
nuclear@0 915 xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
nuclear@0 916 xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
nuclear@0 917 xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
nuclear@0 918 xform.m[1][1] = nysq + (1-nysq) * cosa;
nuclear@0 919 xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
nuclear@0 920 xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
nuclear@0 921 xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
nuclear@0 922 xform.m[2][2] = nzsq + (1-nzsq) * cosa;
nuclear@0 923
nuclear@0 924 *this = xform;
nuclear@0 925 }
nuclear@0 926
nuclear@0 927 void Matrix4x4::SetScaling(float x, float y, float z) {
nuclear@0 928
nuclear@0 929 Matrix4x4 xform(x, 0, 0, 0,
nuclear@0 930 0, y, 0, 0,
nuclear@0 931 0, 0, z, 0,
nuclear@0 932 0, 0, 0, 1 );
nuclear@0 933 *this = xform;
nuclear@0 934 }
nuclear@0 935
nuclear@0 936 void Matrix4x4::SetColumnVector(const Vector4 &vec, int columnindex) {
nuclear@0 937
nuclear@0 938 m[0][columnindex] = vec.x;
nuclear@0 939 m[1][columnindex] = vec.y;
nuclear@0 940 m[2][columnindex] = vec.z;
nuclear@0 941 m[3][columnindex] = vec.w;
nuclear@0 942 }
nuclear@0 943
nuclear@0 944 void Matrix4x4::SetRowVector(const Vector4 &vec, int rowindex) {
nuclear@0 945
nuclear@0 946 m[rowindex][0] = vec.x;
nuclear@0 947 m[rowindex][1] = vec.y;
nuclear@0 948 m[rowindex][2] = vec.z;
nuclear@0 949 m[rowindex][3] = vec.w;
nuclear@0 950 }
nuclear@0 951
nuclear@0 952 Vector4 Matrix4x4::GetColumnVector(int columnindex) const {
nuclear@0 953
nuclear@0 954 return Vector4(m[0][columnindex], m[1][columnindex], m[2][columnindex], m[3][columnindex]);
nuclear@0 955 }
nuclear@0 956
nuclear@0 957 Vector4 Matrix4x4::GetRowVector(int rowindex) const {
nuclear@0 958
nuclear@0 959 return Vector4(m[rowindex][0], m[rowindex][1], m[rowindex][2], m[rowindex][3]);
nuclear@0 960 }
nuclear@0 961
nuclear@0 962 // other operations on matrices
nuclear@0 963
nuclear@0 964 void Matrix4x4::Transpose() {
nuclear@0 965 Matrix4x4 mat = *this;
nuclear@0 966
nuclear@0 967 for(int i=0; i<4; i++) {
nuclear@0 968 for(int j=0; j<4; j++) {
nuclear@0 969 m[i][j] = mat.m[j][i];
nuclear@0 970 }
nuclear@0 971 }
nuclear@0 972 }
nuclear@0 973
nuclear@0 974 Matrix4x4 Matrix4x4::Transposed() const {
nuclear@0 975 Matrix4x4 mat = *this;
nuclear@0 976
nuclear@0 977 for(int i=0; i<4; i++) {
nuclear@0 978 for(int j=0; j<4; j++) {
nuclear@0 979 mat.m[i][j] = m[j][i];
nuclear@0 980 }
nuclear@0 981 }
nuclear@0 982
nuclear@0 983 return mat;
nuclear@0 984 }
nuclear@0 985
nuclear@0 986
nuclear@0 987 float Matrix4x4::Determinant() const {
nuclear@0 988
nuclear@0 989 float det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@0 990 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@0 991 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@0 992
nuclear@0 993 float det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@0 994 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@0 995 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@0 996
nuclear@0 997 float det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@0 998 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@0 999 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@0 1000
nuclear@0 1001 float det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@0 1002 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@0 1003 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@0 1004
nuclear@0 1005 return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
nuclear@0 1006 }
nuclear@0 1007
nuclear@0 1008
nuclear@0 1009 Matrix4x4 Matrix4x4::Adjoint() const {
nuclear@0 1010
nuclear@0 1011 Matrix4x4 coef;
nuclear@0 1012
nuclear@0 1013 coef.m[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@0 1014 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@0 1015 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@0 1016 coef.m[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@0 1017 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@0 1018 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@0 1019 coef.m[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@0 1020 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@0 1021 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@0 1022 coef.m[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@0 1023 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@0 1024 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@0 1025
nuclear@0 1026 coef.m[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@0 1027 (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@0 1028 (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@0 1029 coef.m[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@0 1030 (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@0 1031 (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@0 1032 coef.m[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@0 1033 (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@0 1034 (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@0 1035 coef.m[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@0 1036 (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@0 1037 (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@0 1038
nuclear@0 1039 coef.m[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
nuclear@0 1040 (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
nuclear@0 1041 (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
nuclear@0 1042 coef.m[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
nuclear@0 1043 (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
nuclear@0 1044 (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
nuclear@0 1045 coef.m[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
nuclear@0 1046 (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
nuclear@0 1047 (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
nuclear@0 1048 coef.m[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
nuclear@0 1049 (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
nuclear@0 1050 (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
nuclear@0 1051
nuclear@0 1052 coef.m[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
nuclear@0 1053 (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
nuclear@0 1054 (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
nuclear@0 1055 coef.m[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
nuclear@0 1056 (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
nuclear@0 1057 (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
nuclear@0 1058 coef.m[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
nuclear@0 1059 (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
nuclear@0 1060 (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
nuclear@0 1061 coef.m[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
nuclear@0 1062 (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
nuclear@0 1063 (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
nuclear@0 1064
nuclear@0 1065 coef.Transpose();
nuclear@0 1066
nuclear@0 1067 float *elem = (float*)coef.m;
nuclear@0 1068 for(int i=0; i<4; i++) {
nuclear@0 1069 for(int j=0; j<4; j++) {
nuclear@0 1070 coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
nuclear@0 1071 if(i%2) coef.m[i][j] = -coef.m[i][j];
nuclear@0 1072 }
nuclear@0 1073 }
nuclear@0 1074
nuclear@0 1075 return coef;
nuclear@0 1076 }
nuclear@0 1077
nuclear@0 1078 Matrix4x4 Matrix4x4::Inverse() const {
nuclear@0 1079
nuclear@0 1080 Matrix4x4 AdjMat = Adjoint();
nuclear@0 1081
nuclear@0 1082 return AdjMat * (1.0f / Determinant());
nuclear@0 1083 }
nuclear@0 1084
nuclear@0 1085
nuclear@0 1086 // --------- 3 by 3 matrices implementation --------------
nuclear@0 1087
nuclear@0 1088 Matrix3x3::Matrix3x3() {
nuclear@0 1089 memset(m, 0, 9 * sizeof(float));
nuclear@0 1090 m[0][0] = m[1][1] = m[2][2] = 1.0f;
nuclear@0 1091 }
nuclear@0 1092
nuclear@0 1093 Matrix3x3::Matrix3x3(const Matrix3x3 &mat) {
nuclear@0 1094 memcpy(m, mat.m, 9 * sizeof(float));
nuclear@0 1095 }
nuclear@0 1096
nuclear@0 1097 Matrix3x3::Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22) {
nuclear@0 1098 memcpy(m, &m00, 9*sizeof(float)); // arguments are adjacent in stack
nuclear@0 1099 }
nuclear@0 1100
nuclear@0 1101 Matrix3x3 Matrix3x3::operator +(const Matrix3x3 &mat) const {
nuclear@0 1102 Matrix3x3 tmp;
nuclear@0 1103
nuclear@0 1104 const float *op1 = (float*)m;
nuclear@0 1105 const float *op2 = (float*)mat.m;
nuclear@0 1106 float *dst = (float*)tmp.m;
nuclear@0 1107
nuclear@0 1108 for(int i=0; i<9; i++) *dst++ = *op1++ + *op2++;
nuclear@0 1109
nuclear@0 1110 return tmp;
nuclear@0 1111 }
nuclear@0 1112
nuclear@0 1113 Matrix3x3 Matrix3x3::operator -(const Matrix3x3 &mat) const {
nuclear@0 1114 Matrix3x3 tmp;
nuclear@0 1115
nuclear@0 1116 const float *op1 = (float*)m;
nuclear@0 1117 const float *op2 = (float*)mat.m;
nuclear@0 1118 float *dst = (float*)tmp.m;
nuclear@0 1119
nuclear@0 1120 for(int i=0; i<9; i++) *dst++ = *op1++ - *op2++;
nuclear@0 1121
nuclear@0 1122 return tmp;
nuclear@0 1123 }
nuclear@0 1124
nuclear@0 1125 Matrix3x3 Matrix3x3::operator *(const Matrix3x3 &mat) const {
nuclear@0 1126 Matrix3x3 tmp;
nuclear@0 1127
nuclear@0 1128 for(int i=0; i<3; i++) {
nuclear@0 1129 for(int j=0; j<3; j++) {
nuclear@0 1130 tmp.m[i][j] = m[i][0]*mat.m[0][j] + m[i][1]*mat.m[1][j] + m[i][2]*mat.m[2][j];
nuclear@0 1131 }
nuclear@0 1132 }
nuclear@0 1133
nuclear@0 1134 return tmp;
nuclear@0 1135 }
nuclear@0 1136
nuclear@0 1137 Matrix3x3 Matrix3x3::operator *(float scalar) const {
nuclear@0 1138 Matrix3x3 tmp;
nuclear@0 1139
nuclear@0 1140 const float *op1 = (float*)m;
nuclear@0 1141 float *dst = (float*)tmp.m;
nuclear@0 1142
nuclear@0 1143 for(int i=0; i<9; i++) *dst++ = *op1++ * scalar;
nuclear@0 1144
nuclear@0 1145 return tmp;
nuclear@0 1146 }
nuclear@0 1147
nuclear@0 1148 void Matrix3x3::operator +=(const Matrix3x3 &mat) {
nuclear@0 1149 const float *op = (float*)mat.m;
nuclear@0 1150 float *dst = (float*)m;
nuclear@0 1151
nuclear@0 1152 for(int i=0; i<9; i++) *dst++ += *op++;
nuclear@0 1153 }
nuclear@0 1154
nuclear@0 1155 void Matrix3x3::operator -=(const Matrix3x3 &mat) {
nuclear@0 1156 const float *op = (float*)mat.m;
nuclear@0 1157 float *dst = (float*)m;
nuclear@0 1158
nuclear@0 1159 for(int i=0; i<9; i++) *dst++ -= *op++;
nuclear@0 1160 }
nuclear@0 1161
nuclear@0 1162 void Matrix3x3::operator *=(const Matrix3x3 &mat) {
nuclear@0 1163 Matrix4x4 tmp;
nuclear@0 1164
nuclear@0 1165 for(int i=0; i<3; i++) {
nuclear@0 1166 for(int j=0; j<3; j++) {
nuclear@0 1167 tmp.m[i][j] = m[i][0]*mat.m[0][j] + m[i][1]*mat.m[1][j] + m[i][2]*mat.m[2][j];
nuclear@0 1168 }
nuclear@0 1169 }
nuclear@0 1170
nuclear@0 1171 memcpy(m, tmp.m, 9*sizeof(float));
nuclear@0 1172 }
nuclear@0 1173
nuclear@0 1174 void Matrix3x3::operator *=(float scalar) {
nuclear@0 1175 float *dst = (float*)m;
nuclear@0 1176
nuclear@0 1177 for(int i=0; i<9; i++) *dst++ *= scalar;
nuclear@0 1178 }
nuclear@0 1179
nuclear@0 1180
nuclear@0 1181 void Matrix3x3::ResetIdentity() {
nuclear@0 1182 memset(m, 0, 9 * sizeof(float));
nuclear@0 1183 m[0][0] = m[1][1] = m[2][2] = 1.0f;
nuclear@0 1184 }
nuclear@0 1185
nuclear@0 1186 void Matrix3x3::Translate(float x, float y) {
nuclear@0 1187 Matrix3x3 tmp( 1, 0, 0,
nuclear@0 1188 0, 1, 0,
nuclear@0 1189 x, y, 1 );
nuclear@0 1190 *this *= tmp;
nuclear@0 1191 }
nuclear@0 1192
nuclear@0 1193 void Matrix3x3::Rotate(float angle) {
nuclear@0 1194 Matrix3x3 tmp( fcos(angle), fsin(angle), 0,
nuclear@0 1195 -fsin(angle), fcos(angle), 0,
nuclear@0 1196 0, 0, 1 );
nuclear@0 1197 *this *= tmp;
nuclear@0 1198 }
nuclear@0 1199
nuclear@0 1200 void Matrix3x3::Scale(float x, float y) {
nuclear@0 1201 Matrix3x3 tmp( x, 0, 0,
nuclear@0 1202 0, y, 0,
nuclear@0 1203 0, 0, 1);
nuclear@0 1204
nuclear@0 1205 *this *= tmp;
nuclear@0 1206 }
nuclear@0 1207
nuclear@0 1208 void Matrix3x3::SetTranslation(float x, float y) {
nuclear@0 1209 Matrix3x3( 1, 0, 0,
nuclear@0 1210 0, 1, 0,
nuclear@0 1211 x, y, 1 );
nuclear@0 1212 }
nuclear@0 1213
nuclear@0 1214 void Matrix3x3::SetRotation(float angle) {
nuclear@0 1215 Matrix3x3( fcos(angle), fsin(angle), 0,
nuclear@0 1216 -fsin(angle), fcos(angle), 0,
nuclear@0 1217 0, 0, 1 );
nuclear@0 1218 }
nuclear@0 1219
nuclear@0 1220 void Matrix3x3::SetScaling(float x, float y) {
nuclear@0 1221 Matrix3x3( x, 0, 0,
nuclear@0 1222 0, y, 0,
nuclear@0 1223 0, 0, 1 );
nuclear@0 1224 }
nuclear@0 1225
nuclear@0 1226 void Matrix3x3::SetColumnVector(const Vector3 &vec, int columnindex) {
nuclear@0 1227 m[columnindex][0] = vec.x;
nuclear@0 1228 m[columnindex][1] = vec.y;
nuclear@0 1229 m[columnindex][2] = vec.z;
nuclear@0 1230 }
nuclear@0 1231
nuclear@0 1232 void Matrix3x3::SetRowVector(const Vector3 &vec, int rowindex) {
nuclear@0 1233 m[0][rowindex] = vec.x;
nuclear@0 1234 m[1][rowindex] = vec.y;
nuclear@0 1235 m[2][rowindex] = vec.z;
nuclear@0 1236 }
nuclear@0 1237
nuclear@0 1238 Vector3 Matrix3x3::GetColumnVector(int columnindex) const {
nuclear@0 1239 return Vector3(m[columnindex][0], m[columnindex][1], m[columnindex][2]);
nuclear@0 1240 }
nuclear@0 1241
nuclear@0 1242 Vector3 Matrix3x3::GetRowVector(int rowindex) const {
nuclear@0 1243 return Vector3(m[0][rowindex], m[1][rowindex], m[2][rowindex]);
nuclear@0 1244 }
nuclear@0 1245
nuclear@0 1246 void Matrix3x3::Transpose() {
nuclear@0 1247 Matrix3x3 mat = *this;
nuclear@0 1248
nuclear@0 1249 for(int i=0; i<3; i++) {
nuclear@0 1250 for(int j=0; j<3; j++) {
nuclear@0 1251 m[i][j] = mat.m[j][i];
nuclear@0 1252 }
nuclear@0 1253 }
nuclear@0 1254 }
nuclear@0 1255
nuclear@0 1256 Matrix3x3 Matrix3x3::Transposed() const {
nuclear@0 1257 Matrix3x3 mat;
nuclear@0 1258
nuclear@0 1259 for(int i=0; i<3; i++) {
nuclear@0 1260 for(int j=0; j<3; j++) {
nuclear@0 1261 mat.m[i][j] = m[j][i];
nuclear@0 1262 }
nuclear@0 1263 }
nuclear@0 1264
nuclear@0 1265 return mat;
nuclear@0 1266 }
nuclear@0 1267
nuclear@0 1268
nuclear@0 1269 void Matrix3x3::OrthoNormalize() {
nuclear@0 1270 Vector3 i, j, k;
nuclear@0 1271 i = GetRowVector(0);
nuclear@0 1272 j = GetRowVector(1);
nuclear@0 1273 k = GetRowVector(2);
nuclear@0 1274
nuclear@0 1275 i = CrossProduct(j, k);
nuclear@0 1276 j = CrossProduct(k, i);
nuclear@0 1277 k = CrossProduct(i, j);
nuclear@0 1278
nuclear@0 1279 SetRowVector(i, 0);
nuclear@0 1280 SetRowVector(j, 1);
nuclear@0 1281 SetRowVector(k, 2);
nuclear@0 1282 }
nuclear@0 1283
nuclear@0 1284 Matrix3x3 Matrix3x3::OrthoNormalized() {
nuclear@0 1285 Vector3 i, j, k;
nuclear@0 1286 i = GetRowVector(0);
nuclear@0 1287 j = GetRowVector(1);
nuclear@0 1288 k = GetRowVector(2);
nuclear@0 1289
nuclear@0 1290 i = CrossProduct(j, k);
nuclear@0 1291 j = CrossProduct(k, i);
nuclear@0 1292 k = CrossProduct(i, j);
nuclear@0 1293
nuclear@0 1294 Matrix3x3 newmat;
nuclear@0 1295 newmat.SetRowVector(i, 0);
nuclear@0 1296 newmat.SetRowVector(j, 1);
nuclear@0 1297 newmat.SetRowVector(k, 2);
nuclear@0 1298
nuclear@0 1299 return newmat;
nuclear@0 1300 }
nuclear@0 1301
nuclear@0 1302
nuclear@0 1303
nuclear@0 1304 // ----------- Ray implementation --------------
nuclear@0 1305 Ray::Ray() {
nuclear@0 1306 Origin = Vector3(0.0f, 0.0f, 0.0f);
nuclear@0 1307 Direction = Vector3(0.0f, 0.0f, 1.0f);
nuclear@0 1308 Energy = 1.0f;
nuclear@0 1309 CurrentIOR = 1.0f;
nuclear@0 1310 }
nuclear@0 1311
nuclear@0 1312 Ray::Ray(const Vector3 &origin, const Vector3 &direction) {
nuclear@0 1313 Origin = origin;
nuclear@0 1314 Direction = direction;
nuclear@0 1315 }
nuclear@0 1316
nuclear@0 1317 // ----------- Base implementation --------------
nuclear@0 1318 Base::Base() {
nuclear@0 1319 i = Vector3(1, 0, 0);
nuclear@0 1320 j = Vector3(0, 1, 0);
nuclear@0 1321 k = Vector3(0, 0, 1);
nuclear@0 1322 }
nuclear@0 1323
nuclear@0 1324 Base::Base(const Vector3 &i, const Vector3 &j, const Vector3 &k) {
nuclear@0 1325 this->i = i;
nuclear@0 1326 this->j = j;
nuclear@0 1327 this->k = k;
nuclear@0 1328 }
nuclear@0 1329
nuclear@0 1330 Base::Base(const Vector3 &dir, bool LeftHanded) {
nuclear@0 1331 k = dir;
nuclear@0 1332 j = VECTOR3_J;
nuclear@0 1333 i = CrossProduct(j, k);
nuclear@0 1334 j = CrossProduct(k, i);
nuclear@0 1335 }
nuclear@0 1336
nuclear@0 1337
nuclear@0 1338 void Base::Rotate(float x, float y, float z) {
nuclear@0 1339 Matrix4x4 RotMat;
nuclear@0 1340 RotMat.SetRotation(x, y, z);
nuclear@0 1341 i.Transform(RotMat);
nuclear@0 1342 j.Transform(RotMat);
nuclear@0 1343 k.Transform(RotMat);
nuclear@0 1344 }
nuclear@0 1345
nuclear@0 1346 void Base::Rotate(const Vector3 &axis, float angle) {
nuclear@0 1347 Quaternion q;
nuclear@0 1348 q.SetRotation(axis, angle);
nuclear@0 1349 i.Transform(q);
nuclear@0 1350 j.Transform(q);
nuclear@0 1351 k.Transform(q);
nuclear@0 1352 }
nuclear@0 1353
nuclear@0 1354 void Base::Rotate(const Matrix4x4 &mat) {
nuclear@0 1355 i.Transform(mat);
nuclear@0 1356 j.Transform(mat);
nuclear@0 1357 k.Transform(mat);
nuclear@0 1358 }
nuclear@0 1359
nuclear@0 1360 void Base::Rotate(const Quaternion &quat) {
nuclear@0 1361 i.Transform(quat);
nuclear@0 1362 j.Transform(quat);
nuclear@0 1363 k.Transform(quat);
nuclear@0 1364 }
nuclear@0 1365
nuclear@0 1366 Matrix3x3 Base::CreateRotationMatrix() const {
nuclear@0 1367 return Matrix3x3( i.x, i.y, i.z,
nuclear@0 1368 j.x, j.y, j.z,
nuclear@0 1369 k.x, k.y, k.z);
nuclear@0 1370 }