1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/src/common/n3dmath.cpp	Thu Oct 23 01:46:07 2014 +0300
     1.3 @@ -0,0 +1,1370 @@
     1.4 +#include "n3dmath.h"
     1.5 +
     1.6 +#define fsin (float)sin
     1.7 +#define fcos (float)cos
     1.8 +
     1.9 +float frand(float range) {
    1.10 +	return ((float)rand() / (float)RAND_MAX) * range;
    1.11 +}
    1.12 +
    1.13 +Vector3::Vector3() {
    1.14 +	x = y = z = 0.0f;
    1.15 +}
    1.16 +
    1.17 +Vector3::Vector3(float x, float y, float z) {
    1.18 +	this->x = x;
    1.19 +	this->y = y;
    1.20 +	this->z = z;
    1.21 +}
    1.22 +/*  inlined
    1.23 +float Vector3::DotProduct(const Vector3 &vec) const {
    1.24 +	return x * vec.x + y * vec.y + z * vec.z;
    1.25 +}
    1.26 +
    1.27 +float DotProduct(const Vector3 &vec1, const Vector3 &vec2) {
    1.28 +	return vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z;
    1.29 +}
    1.30 +
    1.31 +Vector3 Vector3::CrossProduct(const Vector3 &vec) const {
    1.32 +	return Vector3(y * vec.z - z * vec.y,  z * vec.x - x * vec.z,  x * vec.y - y * vec.x);
    1.33 +}
    1.34 +
    1.35 +Vector3 CrossProduct(const Vector3 &vec1, const Vector3 &vec2) {
    1.36 +	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);
    1.37 +}
    1.38 +
    1.39 +Vector3 Vector3::operator +(const Vector3 &vec) const {
    1.40 +	return Vector3(x + vec.x, y + vec.y, z + vec.z);
    1.41 +}
    1.42 +
    1.43 +Vector3 Vector3::operator -(const Vector3 &vec) const {
    1.44 +	return Vector3(x - vec.x, y - vec.y, z - vec.z);
    1.45 +}
    1.46 +
    1.47 +Vector3 Vector3::operator *(float scalar) const {
    1.48 +	return Vector3(x * scalar, y * scalar, z * scalar);
    1.49 +}
    1.50 +
    1.51 +Vector3 Vector3::operator /(float scalar) const {
    1.52 +	return Vector3(x / scalar, y / scalar, z / scalar);
    1.53 +}
    1.54 +
    1.55 +void Vector3::operator +=(const Vector3 &vec) {
    1.56 +	x += vec.x;
    1.57 +	y += vec.y;
    1.58 +	z += vec.z;
    1.59 +}
    1.60 +
    1.61 +void Vector3::operator -=(const Vector3 &vec) {
    1.62 +	x -= vec.x;
    1.63 +	y -= vec.y;
    1.64 +	z -= vec.z;
    1.65 +}
    1.66 +
    1.67 +void Vector3::operator *=(float scalar) {
    1.68 +	x *= scalar;
    1.69 +	y *= scalar;
    1.70 +	z *= scalar;
    1.71 +}
    1.72 +
    1.73 +void Vector3::operator /=(float scalar) {
    1.74 +	x /= scalar;
    1.75 +	y /= scalar;
    1.76 +	z /= scalar;
    1.77 +}
    1.78 +
    1.79 +Vector3 Vector3::operator -() const {
    1.80 +	return Vector3(-x, -y, -z);
    1.81 +}
    1.82 +
    1.83 +bool Vector3::operator >(const Vector3 &vec) const {
    1.84 +	return LengthSq() > vec.LengthSq();
    1.85 +}
    1.86 +
    1.87 +bool Vector3::operator <(const Vector3 &vec) const {
    1.88 +	return LengthSq() < vec.LengthSq();
    1.89 +}
    1.90 +
    1.91 +bool Vector3::operator >(float len) const {
    1.92 +	return LengthSq() > len;
    1.93 +}
    1.94 +
    1.95 +bool Vector3::operator <(float len) const {
    1.96 +	return LengthSq() < len;
    1.97 +}
    1.98 +
    1.99 +bool Vector3::operator ==(const Vector3 &vec) const {
   1.100 +	return ((*this - vec).Length() < XSmallNumber);
   1.101 +}
   1.102 +
   1.103 +bool Vector3::operator ==(float len) const {
   1.104 +	return ((this->Length() - len) < XSmallNumber);
   1.105 +}
   1.106 +
   1.107 +Vector3::operator Vector2() const {
   1.108 +	return Vector2(x, y);
   1.109 +}
   1.110 +
   1.111 +Vector3::operator Vector4() const {
   1.112 +	return Vector4(x, y, z, 1.0f);
   1.113 +}
   1.114 +
   1.115 +
   1.116 +float Vector3::Length() const {
   1.117 +	return (float)sqrt(x*x + y*y + z*z);
   1.118 +}
   1.119 +
   1.120 +float Vector3::LengthSq() const {
   1.121 +	return x*x + y*y + z*z;
   1.122 +}
   1.123 +
   1.124 +void Vector3::Normalize() {
   1.125 +	float len = (float)sqrt(x*x + y*y + z*z);
   1.126 +	x /= len;
   1.127 +	y /= len;
   1.128 +	z /= len;
   1.129 +}
   1.130 +
   1.131 +Vector3 Vector3::Normalized() const {
   1.132 +	float len = (float)sqrt(x*x + y*y + z*z);
   1.133 +	return Vector3(x / len, y / len, z / len);
   1.134 +}
   1.135 +
   1.136 +Vector3 Vector3::Reflection(const Vector3 &normal) const {
   1.137 +	return normal * this->DotProduct(normal) * 2.0f - *this;
   1.138 +}
   1.139 +*/
   1.140 +Vector3 Vector3::Refraction(const Vector3 &normal, float FromIOR, float ToIOR) const {
   1.141 +	float m = FromIOR / ToIOR;
   1.142 +	Vector3 dir = *this;
   1.143 +	dir.Normalize();
   1.144 +	float CosAngleIncoming = dir.DotProduct(normal);
   1.145 +	float CosAngleRefr = (1.0f / (m*m)) * (float)sqrt(1.0f - m*m * (1 - CosAngleIncoming * CosAngleIncoming));
   1.146 +
   1.147 +	return dir * m - normal * (CosAngleRefr + m * CosAngleIncoming);
   1.148 +}
   1.149 +
   1.150 +void Vector3::Transform(const Matrix4x4 &mat) {
   1.151 +	// assume row vectors
   1.152 +	float nx = x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0] + mat.m[3][0];
   1.153 +	float ny = x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1] + mat.m[3][1];
   1.154 +	z  = x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] + mat.m[3][2];
   1.155 +	x = nx;
   1.156 +	y = ny;
   1.157 +}
   1.158 +
   1.159 +void Vector3::Transform(const Quaternion &quat) {
   1.160 +	Quaternion vq(0.0f, *this);
   1.161 +	vq = quat * vq * quat.Inverse();
   1.162 +	*this = vq.v;
   1.163 +}
   1.164 +
   1.165 +// direct transformations
   1.166 +
   1.167 +void Vector3::Translate(float x, float y, float z) {
   1.168 +	this->x += x;
   1.169 +	this->y += y;
   1.170 +	this->z += z;
   1.171 +}
   1.172 +
   1.173 +void Vector3::Rotate(float x, float y, float z) {
   1.174 +	
   1.175 +	Matrix4x4 xform;
   1.176 +	xform.SetRotation(x, y, z);
   1.177 +	
   1.178 +	Transform(xform);
   1.179 +}
   1.180 +
   1.181 +void Vector3::Rotate(const Vector3 &axis, float angle) {
   1.182 +
   1.183 +	Matrix4x4 xform;
   1.184 +	xform.SetRotation(axis, angle);
   1.185 +
   1.186 +	Transform(xform);
   1.187 +}
   1.188 +
   1.189 +void Vector3::Scale(float x, float y, float z) {
   1.190 +	this->x *= x;
   1.191 +	this->y *= y;
   1.192 +	this->z *= z;
   1.193 +}
   1.194 +
   1.195 +float &Vector3::operator [](int index) {
   1.196 +	return !index ? x : index == 1 ? y : z;
   1.197 +}
   1.198 +
   1.199 +std::ostream &operator <<(std::ostream &out, const Vector3 &vec) {
   1.200 +	out << vec.x << ", " << vec.y << ", " << vec.z;
   1.201 +	return out;
   1.202 +}
   1.203 +
   1.204 +// ------------- Vector4 implementation ---------------
   1.205 +
   1.206 +Vector4::Vector4() {
   1.207 +	x = y = z = 0.0f;
   1.208 +}
   1.209 +
   1.210 +Vector4::Vector4(const Vector4 &vec) {
   1.211 +	x = vec.x;
   1.212 +	y = vec.y;
   1.213 +	z = vec.z;
   1.214 +	w = vec.w;
   1.215 +}
   1.216 +
   1.217 +Vector4::Vector4(const Vector3 &vec) {
   1.218 +	x = vec.x;
   1.219 +	y = vec.y;
   1.220 +	z = vec.z;
   1.221 +	w = 1.0f;
   1.222 +}
   1.223 +
   1.224 +Vector4::Vector4(float x, float y, float z, float w) {
   1.225 +	this->x = x;
   1.226 +	this->y = y;
   1.227 +	this->z = z;
   1.228 +	this->w = w;
   1.229 +}
   1.230 +
   1.231 +float Vector4::DotProduct(const Vector4 &vec) const {
   1.232 +	return x * vec.x + y * vec.y + z * vec.z + w * vec.w;
   1.233 +}
   1.234 +
   1.235 +float DotProduct(const Vector4 &vec1, const Vector4 &vec2) {
   1.236 +	return vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z + vec1.w * vec2.w;
   1.237 +}
   1.238 +
   1.239 +Vector4 Vector4::CrossProduct(const Vector4 &vec1, const Vector4 &vec2) const {
   1.240 +    float  A, B, C, D, E, F;       // Intermediate Values
   1.241 +    Vector4 result;
   1.242 +
   1.243 +    // Calculate intermediate values.
   1.244 +    A = (vec1.x * vec2.y) - (vec1.y * vec2.x);
   1.245 +    B = (vec1.x * vec2.z) - (vec1.z * vec2.x);
   1.246 +    C = (vec1.x * vec2.w) - (vec1.w * vec2.x);
   1.247 +    D = (vec1.y * vec2.z) - (vec1.z * vec2.y);
   1.248 +    E = (vec1.y * vec2.w) - (vec1.w * vec2.y);
   1.249 +    F = (vec1.z * vec2.w) - (vec1.w * vec2.z);
   1.250 +
   1.251 +    // Calculate the result-vector components.
   1.252 +    result.x =   (y * F) - (z * E) + (w * D);
   1.253 +    result.y = - (x * F) + (z * C) - (w * B);
   1.254 +    result.z =   (x * E) - (y * C) + (w * A);
   1.255 +    result.w = - (x * D) + (y * B) - (z * A);
   1.256 +    return result;
   1.257 +}
   1.258 +
   1.259 +Vector4 CrossProduct(const Vector4 &vec1, const Vector4 &vec2, const Vector4 &vec3) {
   1.260 +    float  A, B, C, D, E, F;       // Intermediate Values
   1.261 +    Vector4 result;
   1.262 +
   1.263 +    // Calculate intermediate values.
   1.264 +    A = (vec2.x * vec3.y) - (vec2.y * vec3.x);
   1.265 +    B = (vec2.x * vec3.z) - (vec2.z * vec3.x);
   1.266 +    C = (vec2.x * vec3.w) - (vec2.w * vec3.x);
   1.267 +    D = (vec2.y * vec3.z) - (vec2.z * vec3.y);
   1.268 +    E = (vec2.y * vec3.w) - (vec2.w * vec3.y);
   1.269 +    F = (vec2.z * vec3.w) - (vec2.w * vec3.z);
   1.270 +
   1.271 +    // Calculate the result-vector components.
   1.272 +    result.x =   (vec1.y * F) - (vec1.z * E) + (vec1.w * D);
   1.273 +    result.y = - (vec1.x * F) + (vec1.z * C) - (vec1.w * B);
   1.274 +    result.z =   (vec1.x * E) - (vec1.y * C) + (vec1.w * A);
   1.275 +    result.w = - (vec1.x * D) + (vec1.y * B) - (vec1.z * A);
   1.276 +    return result;
   1.277 +}
   1.278 +
   1.279 +Vector4 Vector4::operator +(const Vector4 &vec) const {
   1.280 +	return Vector4(x + vec.x, y + vec.y, z + vec.z, w + vec.w);
   1.281 +}
   1.282 +
   1.283 +Vector4 Vector4::operator -(const Vector4 &vec) const {
   1.284 +	return Vector4(x - vec.x, y - vec.y, z - vec.z, w - vec.w);
   1.285 +}
   1.286 +
   1.287 +Vector4 Vector4::operator *(float scalar) const {
   1.288 +	return Vector4(x * scalar, y * scalar, z * scalar, w * scalar);
   1.289 +}
   1.290 +
   1.291 +Vector4 Vector4::operator /(float scalar) const {
   1.292 +	return Vector4(x / scalar, y / scalar, z / scalar, w / scalar);
   1.293 +}
   1.294 +
   1.295 +void Vector4::operator +=(const Vector4 &vec) {
   1.296 +	x += vec.x;
   1.297 +	y += vec.y;
   1.298 +	z += vec.z;
   1.299 +	w += vec.w;
   1.300 +}
   1.301 +
   1.302 +void Vector4::operator -=(const Vector4 &vec) {
   1.303 +	x -= vec.x;
   1.304 +	y -= vec.y;
   1.305 +	z -= vec.z;
   1.306 +	w -= vec.w;
   1.307 +}
   1.308 +
   1.309 +void Vector4::operator *=(float scalar) {
   1.310 +	x *= scalar;
   1.311 +	y *= scalar;
   1.312 +	z *= scalar;
   1.313 +	w *= scalar;
   1.314 +}
   1.315 +
   1.316 +void Vector4::operator /=(float scalar) {
   1.317 +	x /= scalar;
   1.318 +	y /= scalar;
   1.319 +	z /= scalar;
   1.320 +	w /= scalar;
   1.321 +}
   1.322 +
   1.323 +Vector4 Vector4::operator -() const {
   1.324 +	return Vector4(-x, -y, -z, -w);
   1.325 +}
   1.326 +
   1.327 +
   1.328 +bool Vector4::operator >(const Vector4 &vec) const {
   1.329 +	return LengthSq() > vec.LengthSq();
   1.330 +}
   1.331 +
   1.332 +bool Vector4::operator <(const Vector4 &vec) const {
   1.333 +	return LengthSq() < vec.LengthSq();
   1.334 +}
   1.335 +
   1.336 +bool Vector4::operator >(float len) const {
   1.337 +	return LengthSq() > len;
   1.338 +}
   1.339 +
   1.340 +bool Vector4::operator <(float len) const {
   1.341 +	return LengthSq() < len;
   1.342 +}
   1.343 +
   1.344 +bool Vector4::operator ==(const Vector4 &vec) const {
   1.345 +	return ((*this - vec).Length() < XSmallNumber);
   1.346 +}
   1.347 +
   1.348 +bool Vector4::operator ==(float len) const {
   1.349 +	return ((this->Length() - len) < XSmallNumber);
   1.350 +}
   1.351 +
   1.352 +Vector4::operator Vector3() const {
   1.353 +	return Vector3(x, y, z);
   1.354 +}
   1.355 +
   1.356 +
   1.357 +float Vector4::Length() const {
   1.358 +	return (float)sqrt(x*x + y*y + z*z + w*w);
   1.359 +}
   1.360 +
   1.361 +float Vector4::LengthSq() const {
   1.362 +	return x*x + y*y + z*z + w*w;
   1.363 +}
   1.364 +
   1.365 +void Vector4::Normalize() {
   1.366 +	float len = (float)sqrt(x*x + y*y + z*z + w*w);
   1.367 +	x /= len;
   1.368 +	y /= len;
   1.369 +	z /= len;
   1.370 +	w /= len;
   1.371 +}
   1.372 +
   1.373 +Vector4 Vector4::Normalized() const {
   1.374 +	float len = (float)sqrt(x*x + y*y + z*z + w*w);
   1.375 +	return Vector4(x / len, y / len, z / len, w / len);
   1.376 +}
   1.377 +
   1.378 +void Vector4::Transform(const Matrix4x4 &mat) {
   1.379 +	// assume row vectors
   1.380 +	float nx = x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0] + w * mat.m[3][0];
   1.381 +	float ny = x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1] + w * mat.m[3][1];
   1.382 +	float nz = x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] + w * mat.m[3][2];
   1.383 +	w = x * mat.m[0][3] + y * mat.m[1][3] + z * mat.m[2][3] + w * mat.m[3][3];
   1.384 +	x = nx;
   1.385 +	y = ny;
   1.386 +	z = nz;
   1.387 +}
   1.388 +
   1.389 +
   1.390 +// Direct transformations on the vector
   1.391 +void Vector4::Translate(float x, float y, float z, float w) {
   1.392 +	x += x;
   1.393 +	y += y;
   1.394 +	z += z;
   1.395 +	w += w;
   1.396 +}
   1.397 +
   1.398 +void Vector4::Rotate(float x, float y, float z) {
   1.399 +	Matrix4x4 xform;
   1.400 +	xform.SetRotation(x, y, z);
   1.401 +	Transform(xform);
   1.402 +}
   1.403 +
   1.404 +void Vector4::Rotate(const Vector3 &axis, float angle) {
   1.405 +	Matrix4x4 xform;
   1.406 +	xform.SetRotation(axis, angle);
   1.407 +	Transform(xform);
   1.408 +}
   1.409 +
   1.410 +void Vector4::Scale(float x, float y, float z, float w) {
   1.411 +	this->x *= x;
   1.412 +	this->y *= y;
   1.413 +	this->z *= z;
   1.414 +	this->w *= w;
   1.415 +}
   1.416 +
   1.417 +float &Vector4::operator [](int index) {
   1.418 +	return !index ? x : index == 1 ? y : index == 2 ? z : w;
   1.419 +}
   1.420 +
   1.421 +std::ostream &operator <<(std::ostream &out, const Vector4 &vec) {
   1.422 +	out << vec.x << ", " << vec.y << ", " << vec.z << ", " << vec.w;
   1.423 +	return out;
   1.424 +}
   1.425 +
   1.426 +// ------------- Vector2 implementation ---------------
   1.427 +
   1.428 +Vector2::Vector2() {
   1.429 +	x = y = 0.0f;
   1.430 +}
   1.431 +
   1.432 +Vector2::Vector2(const Vector2 &vec) {
   1.433 +	x = vec.x;
   1.434 +	y = vec.y;
   1.435 +}
   1.436 +
   1.437 +Vector2::Vector2(float x, float y) {
   1.438 +	this->x = x;
   1.439 +	this->y = y;
   1.440 +}
   1.441 +
   1.442 +float Vector2::DotProduct(const Vector2 &vec) const {
   1.443 +	return x * vec.x + y * vec.y;
   1.444 +}
   1.445 +
   1.446 +float DotProduct(const Vector2 &vec1, const Vector2 &vec2) {
   1.447 +	return vec1.x * vec2.x + vec1.y + vec2.y;
   1.448 +}
   1.449 +
   1.450 +Vector2 Vector2::operator +(const Vector2 &vec) const {
   1.451 +	return Vector2(x + vec.x, y + vec.y);
   1.452 +}
   1.453 +
   1.454 +Vector2 Vector2::operator -(const Vector2 &vec) const {
   1.455 +	return Vector2(x - vec.x, y - vec.y);
   1.456 +}
   1.457 +
   1.458 +Vector2 Vector2::operator *(float scalar) const {
   1.459 +	return Vector2(x * scalar, y * scalar);
   1.460 +}
   1.461 +
   1.462 +Vector2 Vector2::operator /(float scalar) const {
   1.463 +	return Vector2(x / scalar, y / scalar);
   1.464 +}
   1.465 +
   1.466 +void Vector2::operator +=(const Vector2 &vec) {
   1.467 +	x += vec.x;
   1.468 +	y += vec.y;
   1.469 +}
   1.470 +
   1.471 +void Vector2::operator -=(const Vector2 &vec) {
   1.472 +	x -= vec.x;
   1.473 +	y -= vec.y;
   1.474 +}
   1.475 +
   1.476 +void Vector2::operator *=(float scalar) {
   1.477 +	x *= scalar;
   1.478 +	y *= scalar;
   1.479 +}
   1.480 +
   1.481 +void Vector2::operator /=(float scalar) {
   1.482 +	x /= scalar;
   1.483 +	y /= scalar;
   1.484 +}
   1.485 +
   1.486 +Vector2 Vector2::operator -() const {
   1.487 +	return Vector2(-x, -y);
   1.488 +}
   1.489 +
   1.490 +bool Vector2::operator >(const Vector2 &vec) const {
   1.491 +	return LengthSq() > vec.LengthSq();
   1.492 +}
   1.493 +
   1.494 +bool Vector2::operator <(const Vector2 &vec) const {
   1.495 +	return LengthSq() < vec.LengthSq();
   1.496 +}
   1.497 +
   1.498 +bool Vector2::operator >(float len) const {
   1.499 +	return LengthSq() > len;
   1.500 +}
   1.501 +
   1.502 +bool Vector2::operator <(float len) const {
   1.503 +	return LengthSq() < len;
   1.504 +}
   1.505 +
   1.506 +bool Vector2::operator ==(const Vector2 &vec) const {
   1.507 +	return ((*this - vec).Length() < XSmallNumber);
   1.508 +}
   1.509 +
   1.510 +bool Vector2::operator ==(float len) const {
   1.511 +	return ((this->Length() - len) < XSmallNumber);
   1.512 +}
   1.513 +
   1.514 +Vector2::operator Vector3() const {
   1.515 +	return Vector3(x, y, 1.0f);
   1.516 +}
   1.517 +
   1.518 +float Vector2::Length() const {
   1.519 +	return (float)sqrt(x * x + y * y);
   1.520 +}
   1.521 +
   1.522 +float Vector2::LengthSq() const {
   1.523 +	return x * x + y * y;
   1.524 +}
   1.525 +	
   1.526 +void Vector2::Normalize() {
   1.527 +	float len = (float)sqrt(x * x + y * y);
   1.528 +	x /= len;
   1.529 +	y /= len;
   1.530 +}
   1.531 +
   1.532 +Vector2 Vector2::Normalized() const {
   1.533 +	float len = (float)sqrt(x * x + y * y);
   1.534 +	return Vector2(x / len, y / len);
   1.535 +}
   1.536 +
   1.537 +//Vector2 Vector2::Reflection(const Vector2 &normal) const;
   1.538 +//Vector2 Vector2::Refraction(const Vector2 &normal, float FromIOR, float ToIOR) const;
   1.539 +	
   1.540 +void Vector2::Transform(const Matrix3x3 &mat) {
   1.541 +	float nx = x * mat.m[0][0] + y * mat.m[1][0] + mat.m[2][0];
   1.542 +	y = x * mat.m[0][1] + y * mat.m[1][1] + mat.m[2][1];
   1.543 +	x = nx;
   1.544 +}
   1.545 +
   1.546 +void Vector2::Translate(float x, float y) {
   1.547 +	this->x += x;
   1.548 +	this->y += y;
   1.549 +}
   1.550 +
   1.551 +void Vector2::Rotate(float angle) {
   1.552 +	Matrix3x3 xform;
   1.553 +	xform.SetRotation(angle);
   1.554 +
   1.555 +	Transform(xform);
   1.556 +}
   1.557 +
   1.558 +void Vector2::Scale(float x, float y) {
   1.559 +	this->x *= x;
   1.560 +	this->y *= y;
   1.561 +}
   1.562 +
   1.563 +float &Vector2::operator [](int index) {
   1.564 +	return !index ? x : y;
   1.565 +}
   1.566 +
   1.567 +std::ostream &operator <<(std::ostream &out, const Vector2 &vec) {
   1.568 +	out << vec.x << ", " << vec.y;
   1.569 +	return out;
   1.570 +}
   1.571 +
   1.572 +
   1.573 +// --------------- Quaternion implementation ---------------
   1.574 +
   1.575 +Quaternion::Quaternion() {
   1.576 +	s = 1.0f;
   1.577 +	v.x = v.y = v.z = 0.0f;
   1.578 +}
   1.579 +
   1.580 +Quaternion::Quaternion(float s, float x, float y, float z) {
   1.581 +	v.x = x;
   1.582 +	v.y = y;
   1.583 +	v.z = z;
   1.584 +	this->s = s;
   1.585 +}
   1.586 +
   1.587 +Quaternion::Quaternion(float s, const Vector3 &v) {
   1.588 +	this->s = s;
   1.589 +	this->v = v;
   1.590 +}
   1.591 +
   1.592 +Quaternion Quaternion::operator +(const Quaternion &quat) const {
   1.593 +	return Quaternion(s + quat.s, v + quat.v);
   1.594 +}
   1.595 +
   1.596 +Quaternion Quaternion::operator -(const Quaternion &quat) const {
   1.597 +	return Quaternion(s - quat.s, v - quat.v);
   1.598 +}
   1.599 +
   1.600 +Quaternion Quaternion::operator -() const {
   1.601 +	return Quaternion(-s, -v);
   1.602 +}
   1.603 +
   1.604 +// Quaternion Multiplication:
   1.605 +// Q1*Q2 = [s1*s2 - v1.v2,  s1*v2 + s2*v1 + v1(x)v2]
   1.606 +Quaternion Quaternion::operator *(const Quaternion &quat) const {
   1.607 +	Quaternion newq;	
   1.608 +	newq.s = s * quat.s - DotProduct(v, quat.v);
   1.609 +	newq.v = quat.v * s + v * quat.s + CrossProduct(v, quat.v);	
   1.610 +	return newq;
   1.611 +}
   1.612 +
   1.613 +void Quaternion::operator +=(const Quaternion &quat) {
   1.614 +	*this = Quaternion(s + quat.s, v + quat.v);
   1.615 +}
   1.616 +
   1.617 +void Quaternion::operator -=(const Quaternion &quat) {
   1.618 +	*this = Quaternion(s - quat.s, v - quat.v);
   1.619 +}
   1.620 +
   1.621 +void Quaternion::operator *=(const Quaternion &quat) {
   1.622 +	*this = *this * quat;
   1.623 +}
   1.624 +
   1.625 +void Quaternion::ResetIdentity() {
   1.626 +	s = 1.0f;
   1.627 +	v.x = v.y = v.z = 0.0f;
   1.628 +}
   1.629 +
   1.630 +Quaternion Quaternion::Conjugate() const {
   1.631 +	return Quaternion(s, -v);
   1.632 +}
   1.633 +
   1.634 +float Quaternion::Length() const {
   1.635 +	return (float)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
   1.636 +}
   1.637 +
   1.638 +// Q * ~Q = ||Q||^2
   1.639 +float Quaternion::LengthSq() const {
   1.640 +	return v.x*v.x + v.y*v.y + v.z*v.z + s*s;
   1.641 +}
   1.642 +
   1.643 +void Quaternion::Normalize() {
   1.644 +	float len = (float)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
   1.645 +	v.x /= len;
   1.646 +	v.y /= len;
   1.647 +	v.z /= len;
   1.648 +	s /= len;
   1.649 +}
   1.650 +
   1.651 +Quaternion Quaternion::Normalized() const {
   1.652 +	Quaternion nq = *this;
   1.653 +	float len = (float)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
   1.654 +	nq.v.x /= len;
   1.655 +	nq.v.y /= len;
   1.656 +	nq.v.z /= len;
   1.657 +	nq.s /= len;
   1.658 +	return nq;
   1.659 +}
   1.660 +
   1.661 +// Quaternion Inversion: Q^-1 = ~Q / ||Q||^2
   1.662 +Quaternion Quaternion::Inverse() const {
   1.663 +	Quaternion inv = Conjugate();
   1.664 +	float lensq = LengthSq();
   1.665 +	inv.v /= lensq;
   1.666 +	inv.s /= lensq;
   1.667 +
   1.668 +	return inv;
   1.669 +}
   1.670 +
   1.671 +
   1.672 +void Quaternion::SetRotation(const Vector3 &axis, float angle) {
   1.673 +	float HalfAngle = angle / 2.0f;
   1.674 +	s = cosf(HalfAngle);
   1.675 +	v = axis * sinf(HalfAngle);
   1.676 +}
   1.677 +
   1.678 +void Quaternion::Rotate(const Vector3 &axis, float angle) {
   1.679 +	Quaternion q;
   1.680 +	float HalfAngle = angle / 2.0f;
   1.681 +	q.s = cosf(HalfAngle);
   1.682 +	q.v = axis * sinf(HalfAngle);
   1.683 +
   1.684 +	*this *= q;
   1.685 +}
   1.686 +
   1.687 +
   1.688 +Matrix3x3 Quaternion::GetRotationMatrix() const {
   1.689 +	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,
   1.690 +						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,
   1.691 +						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);
   1.692 +}
   1.693 +
   1.694 +
   1.695 +// ------------- Matrix implementation ---------------
   1.696 +
   1.697 +Matrix4x4::Matrix4x4() {
   1.698 +	memset(m, 0, 16*sizeof(float));
   1.699 +	m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0f;
   1.700 +}
   1.701 +
   1.702 +Matrix4x4::Matrix4x4(const Matrix4x4 &mat) {
   1.703 +	memcpy(m, mat.m, 16*sizeof(float));
   1.704 +}
   1.705 +
   1.706 +Matrix4x4::Matrix4x4(const Matrix3x3 &mat) {
   1.707 +	for(int i=0; i<3; i++) {
   1.708 +		for(int j=0; j<3; j++) {
   1.709 +			m[i][j] = mat.m[i][j];
   1.710 +		}
   1.711 +	}
   1.712 +	m[3][0] = m[3][1] = m[3][2] = m[0][3] = m[1][3] = m[2][3] = 0.0f;
   1.713 +	m[3][3] = 1.0f;
   1.714 +}
   1.715 +
   1.716 +Matrix4x4::Matrix4x4(	float m00, float m01, float m02, float m03,
   1.717 +						float m10, float m11, float m12, float m13,
   1.718 +						float m20, float m21, float m22, float m23,
   1.719 +						float m30, float m31, float m32, float m33 ) {
   1.720 +
   1.721 +	memcpy(m, &m00, 16*sizeof(float));	// arguments are adjacent in stack
   1.722 +}
   1.723 +
   1.724 +Matrix4x4 Matrix4x4::operator +(const Matrix4x4 &mat) const {
   1.725 +
   1.726 +	Matrix4x4 tmp;
   1.727 +
   1.728 +	const float *op1 = (float*)m;
   1.729 +	const float *op2 = (float*)mat.m;
   1.730 +	float *dst = (float*)tmp.m;
   1.731 +
   1.732 +	for(int i=0; i<16; i++) *dst++ = *op1++ + *op2++;
   1.733 +
   1.734 +	return tmp;
   1.735 +}
   1.736 +
   1.737 +Matrix4x4 Matrix4x4::operator -(const Matrix4x4 &mat) const {
   1.738 +
   1.739 +	Matrix4x4 tmp;
   1.740 +
   1.741 +	const float *op1 = (float*)m;
   1.742 +	const float *op2 = (float*)mat.m;
   1.743 +	float *dst = (float*)tmp.m;
   1.744 +
   1.745 +	for(int i=0; i<16; i++) *dst++ = *op1++ - *op2++;
   1.746 +
   1.747 +	return tmp;
   1.748 +}
   1.749 +
   1.750 +Matrix4x4 Matrix4x4::operator *(float scalar) const {
   1.751 +
   1.752 +	Matrix4x4 tmp;
   1.753 +
   1.754 +	const float *op1 = (float*)m;
   1.755 +	float *dst = (float*)tmp.m;
   1.756 +
   1.757 +	for(int i=0; i<16; i++) *dst++ = *op1++ * scalar;
   1.758 +
   1.759 +	return tmp;
   1.760 +}
   1.761 +
   1.762 +Matrix4x4 Matrix4x4::operator *(const Matrix4x4 &mat) const {
   1.763 +	Matrix4x4 tmp;
   1.764 +
   1.765 +	for(int i=0; i<4; i++) {
   1.766 +		for(int j=0; j<4; j++) {
   1.767 +			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];
   1.768 +		}
   1.769 +	}
   1.770 +
   1.771 +	return tmp;
   1.772 +}
   1.773 +
   1.774 +void Matrix4x4::operator +=(const Matrix4x4 &mat) {
   1.775 +
   1.776 +	const float *op2 = (float*)mat.m;
   1.777 +	float *dst = (float*)m;
   1.778 +
   1.779 +	for(int i=0; i<16; i++) *dst++ += *op2++;
   1.780 +}
   1.781 +
   1.782 +void Matrix4x4::operator -=(const Matrix4x4 &mat) {
   1.783 +
   1.784 +	const float *op2 = (float*)mat.m;
   1.785 +	float *dst = (float*)m;
   1.786 +
   1.787 +	for(int i=0; i<16; i++) *dst++ -= *op2++;
   1.788 +}
   1.789 +
   1.790 +void Matrix4x4::operator *=(float scalar) {
   1.791 +
   1.792 +	float *dst = (float*)m;
   1.793 +
   1.794 +	for(int i=0; i<16; i++) *dst++ *= scalar;
   1.795 +}
   1.796 +
   1.797 +void Matrix4x4::operator *=(const Matrix4x4 &mat) {
   1.798 +	Matrix4x4 tmp;
   1.799 +
   1.800 +	for(int i=0; i<4; i++) {
   1.801 +		for(int j=0; j<4; j++) {
   1.802 +			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];
   1.803 +		}
   1.804 +	}
   1.805 +
   1.806 +	memcpy(m, tmp.m, 16*sizeof(float));
   1.807 +}
   1.808 +
   1.809 +
   1.810 +void Matrix4x4::ResetIdentity() {
   1.811 +	memset(m, 0, 16*sizeof(float));
   1.812 +	m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0f;
   1.813 +}
   1.814 +
   1.815 +
   1.816 +// Transformations (assuming column vectors)
   1.817 +
   1.818 +void Matrix4x4::Translate(float x, float y, float z) {
   1.819 +	
   1.820 +	Matrix4x4 tmp(	1, 0, 0, 0,
   1.821 +					0, 1, 0, 0,
   1.822 +					0, 0, 1, 0,
   1.823 +					x, y, z, 1 );
   1.824 +	*this *= tmp;
   1.825 +}
   1.826 +
   1.827 +void Matrix4x4::Rotate(float x, float y, float z) {
   1.828 +
   1.829 +	*this *= Matrix4x4(	1,	0,			0,			0,
   1.830 +						0,	fcos(x),	fsin(x),	0,
   1.831 +						0,	-fsin(x),	fcos(x),	0,
   1.832 +						0,	0,			0,			1 );
   1.833 +	
   1.834 +	*this *= Matrix4x4(	fcos(y),	0,	-fsin(y),	0,
   1.835 +						0,			1,	0,			0,
   1.836 +						fsin(y),	0,	fcos(y),	0,
   1.837 +						0,			0,	0,			1 );
   1.838 +
   1.839 +	*this *= Matrix4x4(	fcos(z),	fsin(z),	0,	0,
   1.840 +						-fsin(z),	fcos(z),	0,	0,
   1.841 +						0,			0,			1,	0,
   1.842 +						0,			0,			0,	1 );
   1.843 +}
   1.844 +
   1.845 +void Matrix4x4::Rotate(const Vector3 &axis, float angle) {
   1.846 +
   1.847 +	float sina = fsin(angle);
   1.848 +	float cosa = fcos(angle);
   1.849 +	float invcosa = 1-cosa;
   1.850 +	float nxsq = axis.x * axis.x;
   1.851 +	float nysq = axis.y * axis.y;
   1.852 +	float nzsq = axis.z * axis.z;
   1.853 +
   1.854 +	Matrix4x4 xform;
   1.855 +	xform.m[0][0] = nxsq + (1-nxsq) * cosa;
   1.856 +	xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
   1.857 +	xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
   1.858 +	xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
   1.859 +	xform.m[1][1] = nysq + (1-nysq) * cosa;
   1.860 +	xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
   1.861 +	xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
   1.862 +	xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
   1.863 +	xform.m[2][2] = nzsq + (1-nzsq) * cosa;
   1.864 +
   1.865 +	*this *= xform;
   1.866 +}
   1.867 +
   1.868 +void Matrix4x4::Scale(float x, float y, float z) {
   1.869 +	
   1.870 +	Matrix4x4 xform(x, 0, 0, 0,
   1.871 +					0, y, 0, 0,
   1.872 +					0, 0, z, 0,
   1.873 +					0, 0, 0, 1 );
   1.874 +	*this *= xform;
   1.875 +}
   1.876 +
   1.877 +
   1.878 +//////////////////////////////
   1.879 +
   1.880 +void Matrix4x4::SetTranslation(float x, float y, float z) {
   1.881 +	
   1.882 +	*this = Matrix4x4(	1, 0, 0, 0,
   1.883 +						0, 1, 0, 0,
   1.884 +						0, 0, 1, 0,
   1.885 +						x, y, z, 1 );
   1.886 +}
   1.887 +
   1.888 +void Matrix4x4::SetRotation(float x, float y, float z) {
   1.889 +
   1.890 +	*this = Matrix4x4(	1,	0,			0,			0,
   1.891 +						0,	fcos(x),	fsin(x),	0,
   1.892 +						0,	-fsin(x),	fcos(x),	0,
   1.893 +						0,	0,			0,			1 );
   1.894 +	
   1.895 +	*this *= Matrix4x4(	fcos(y),	0,	-fsin(y),	0,
   1.896 +						0,			1,	0,			0,
   1.897 +						fsin(y),	0,	fcos(y),	0,
   1.898 +						0,			0,	0,			1 );
   1.899 +
   1.900 +	*this *= Matrix4x4(	fcos(z),	fsin(z),	0,	0,
   1.901 +						-fsin(z),	fcos(z),	0,	0,
   1.902 +						0,			0,			1,	0,
   1.903 +						0,			0,			0,	1 );
   1.904 +}
   1.905 +
   1.906 +void Matrix4x4::SetRotation(const Vector3 &axis, float angle) {
   1.907 +
   1.908 +	// caching of multiply used function results (opt)
   1.909 +	float sina = fsin(angle);
   1.910 +	float cosa = fcos(angle);
   1.911 +	float invcosa = 1-cosa;
   1.912 +	float nxsq = axis.x * axis.x;
   1.913 +	float nysq = axis.y * axis.y;
   1.914 +	float nzsq = axis.z * axis.z;
   1.915 +
   1.916 +	Matrix4x4 xform;
   1.917 +	xform.m[0][0] = nxsq + (1-nxsq) * cosa;
   1.918 +	xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
   1.919 +	xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
   1.920 +	xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
   1.921 +	xform.m[1][1] = nysq + (1-nysq) * cosa;
   1.922 +	xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
   1.923 +	xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
   1.924 +	xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
   1.925 +	xform.m[2][2] = nzsq + (1-nzsq) * cosa;
   1.926 +
   1.927 +	*this = xform;
   1.928 +}
   1.929 +
   1.930 +void Matrix4x4::SetScaling(float x, float y, float z) {
   1.931 +	
   1.932 +	Matrix4x4 xform(x, 0, 0, 0,
   1.933 +					0, y, 0, 0,
   1.934 +					0, 0, z, 0,
   1.935 +					0, 0, 0, 1 );
   1.936 +	*this = xform;
   1.937 +}
   1.938 +
   1.939 +void Matrix4x4::SetColumnVector(const Vector4 &vec, int columnindex) {
   1.940 +	
   1.941 +	m[0][columnindex] = vec.x;
   1.942 +	m[1][columnindex] = vec.y;
   1.943 +	m[2][columnindex] = vec.z;
   1.944 +	m[3][columnindex] = vec.w;
   1.945 +}
   1.946 +
   1.947 +void Matrix4x4::SetRowVector(const Vector4 &vec, int rowindex) {
   1.948 +
   1.949 +	m[rowindex][0] = vec.x;
   1.950 +	m[rowindex][1] = vec.y;
   1.951 +	m[rowindex][2] = vec.z;
   1.952 +	m[rowindex][3] = vec.w;
   1.953 +}
   1.954 +
   1.955 +Vector4 Matrix4x4::GetColumnVector(int columnindex) const {
   1.956 +
   1.957 +	return Vector4(m[0][columnindex], m[1][columnindex], m[2][columnindex], m[3][columnindex]);
   1.958 +}
   1.959 +
   1.960 +Vector4 Matrix4x4::GetRowVector(int rowindex) const {
   1.961 +
   1.962 +	return Vector4(m[rowindex][0], m[rowindex][1], m[rowindex][2], m[rowindex][3]);
   1.963 +}
   1.964 +
   1.965 +// other operations on matrices
   1.966 +
   1.967 +void Matrix4x4::Transpose() {
   1.968 +	Matrix4x4 mat = *this;
   1.969 +
   1.970 +	for(int i=0; i<4; i++) {
   1.971 +		for(int j=0; j<4; j++) {
   1.972 +			m[i][j] = mat.m[j][i];
   1.973 +		}
   1.974 +	}
   1.975 +}
   1.976 +
   1.977 +Matrix4x4 Matrix4x4::Transposed() const {
   1.978 +	Matrix4x4 mat = *this;
   1.979 +
   1.980 +	for(int i=0; i<4; i++) {
   1.981 +		for(int j=0; j<4; j++) {
   1.982 +			mat.m[i][j] = m[j][i];
   1.983 +		}
   1.984 +	}
   1.985 +
   1.986 +	return mat;
   1.987 +}
   1.988 +
   1.989 +
   1.990 +float Matrix4x4::Determinant() const {
   1.991 +
   1.992 +	float det11 =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.993 +					(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.994 +					(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.995 +
   1.996 +	float det12 =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.997 +					(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.998 +					(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.999 +
  1.1000 +	float det13 =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
  1.1001 +					(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
  1.1002 +					(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  1.1003 +
  1.1004 +	float det14 =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
  1.1005 +					(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
  1.1006 +					(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  1.1007 +
  1.1008 +	return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
  1.1009 +}
  1.1010 +
  1.1011 +
  1.1012 +Matrix4x4 Matrix4x4::Adjoint() const {
  1.1013 +
  1.1014 +	Matrix4x4 coef;
  1.1015 +
  1.1016 +	coef.m[0][0] =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
  1.1017 +					(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
  1.1018 +					(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
  1.1019 +	coef.m[0][1] =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
  1.1020 +					(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
  1.1021 +					(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
  1.1022 +	coef.m[0][2] =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
  1.1023 +					(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
  1.1024 +					(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  1.1025 +	coef.m[0][3] =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
  1.1026 +					(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
  1.1027 +					(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  1.1028 +
  1.1029 +	coef.m[1][0] =	(m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
  1.1030 +					(m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
  1.1031 +					(m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
  1.1032 +	coef.m[1][1] =	(m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
  1.1033 +					(m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
  1.1034 +					(m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
  1.1035 +	coef.m[1][2] =	(m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
  1.1036 +					(m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
  1.1037 +					(m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  1.1038 +	coef.m[1][3] =	(m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
  1.1039 +					(m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
  1.1040 +					(m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
  1.1041 +
  1.1042 +	coef.m[2][0] =	(m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
  1.1043 +					(m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
  1.1044 +					(m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
  1.1045 +	coef.m[2][1] =	(m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
  1.1046 +					(m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
  1.1047 +					(m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
  1.1048 +	coef.m[2][2] =	(m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
  1.1049 +					(m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
  1.1050 +					(m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
  1.1051 +	coef.m[2][3] =	(m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
  1.1052 +					(m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
  1.1053 +					(m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
  1.1054 +
  1.1055 +	coef.m[3][0] =	(m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
  1.1056 +					(m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
  1.1057 +					(m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
  1.1058 +	coef.m[3][1] =	(m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
  1.1059 +					(m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
  1.1060 +					(m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
  1.1061 +	coef.m[3][2] =	(m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
  1.1062 +					(m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
  1.1063 +					(m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
  1.1064 +	coef.m[3][3] =	(m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
  1.1065 +					(m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
  1.1066 +					(m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
  1.1067 +
  1.1068 +	coef.Transpose();
  1.1069 +
  1.1070 +	float *elem = (float*)coef.m;
  1.1071 +	for(int i=0; i<4; i++) {
  1.1072 +		for(int j=0; j<4; j++) {
  1.1073 +			coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
  1.1074 +			if(i%2) coef.m[i][j] = -coef.m[i][j];
  1.1075 +		}
  1.1076 +	}
  1.1077 +
  1.1078 +	return coef;
  1.1079 +}
  1.1080 +
  1.1081 +Matrix4x4 Matrix4x4::Inverse() const {
  1.1082 +
  1.1083 +	Matrix4x4 AdjMat = Adjoint();
  1.1084 +
  1.1085 +	return AdjMat * (1.0f / Determinant());
  1.1086 +}
  1.1087 +
  1.1088 +
  1.1089 +// --------- 3 by 3 matrices implementation --------------
  1.1090 +
  1.1091 +Matrix3x3::Matrix3x3() {
  1.1092 +	memset(m, 0, 9 * sizeof(float));
  1.1093 +	m[0][0] = m[1][1] = m[2][2] = 1.0f;
  1.1094 +}
  1.1095 +
  1.1096 +Matrix3x3::Matrix3x3(const Matrix3x3 &mat) {
  1.1097 +	memcpy(m, mat.m, 9 * sizeof(float));
  1.1098 +}
  1.1099 +
  1.1100 +Matrix3x3::Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22) {
  1.1101 +	memcpy(m, &m00, 9*sizeof(float));	// arguments are adjacent in stack
  1.1102 +}
  1.1103 +
  1.1104 +Matrix3x3 Matrix3x3::operator +(const Matrix3x3 &mat) const {
  1.1105 +	Matrix3x3 tmp;
  1.1106 +
  1.1107 +	const float *op1 = (float*)m;
  1.1108 +	const float *op2 = (float*)mat.m;
  1.1109 +	float *dst = (float*)tmp.m;
  1.1110 +
  1.1111 +	for(int i=0; i<9; i++) *dst++ = *op1++ + *op2++;
  1.1112 +
  1.1113 +	return tmp;
  1.1114 +}
  1.1115 +
  1.1116 +Matrix3x3 Matrix3x3::operator -(const Matrix3x3 &mat) const {
  1.1117 +	Matrix3x3 tmp;
  1.1118 +
  1.1119 +	const float *op1 = (float*)m;
  1.1120 +	const float *op2 = (float*)mat.m;
  1.1121 +	float *dst = (float*)tmp.m;
  1.1122 +
  1.1123 +	for(int i=0; i<9; i++) *dst++ = *op1++ - *op2++;
  1.1124 +
  1.1125 +	return tmp;
  1.1126 +}
  1.1127 +
  1.1128 +Matrix3x3 Matrix3x3::operator *(const Matrix3x3 &mat) const {
  1.1129 +	Matrix3x3 tmp;
  1.1130 +
  1.1131 +	for(int i=0; i<3; i++) {
  1.1132 +		for(int j=0; j<3; j++) {
  1.1133 +			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];
  1.1134 +		}
  1.1135 +	}
  1.1136 +
  1.1137 +	return tmp;
  1.1138 +}
  1.1139 +
  1.1140 +Matrix3x3 Matrix3x3::operator *(float scalar) const {
  1.1141 +	Matrix3x3 tmp;
  1.1142 +
  1.1143 +	const float *op1 = (float*)m;
  1.1144 +	float *dst = (float*)tmp.m;
  1.1145 +
  1.1146 +	for(int i=0; i<9; i++) *dst++ = *op1++ * scalar;
  1.1147 +
  1.1148 +	return tmp;
  1.1149 +}
  1.1150 +
  1.1151 +void Matrix3x3::operator +=(const Matrix3x3 &mat) {
  1.1152 +	const float *op = (float*)mat.m;
  1.1153 +	float *dst = (float*)m;
  1.1154 +
  1.1155 +	for(int i=0; i<9; i++) *dst++ += *op++;
  1.1156 +}
  1.1157 +
  1.1158 +void Matrix3x3::operator -=(const Matrix3x3 &mat) {
  1.1159 +	const float *op = (float*)mat.m;
  1.1160 +	float *dst = (float*)m;
  1.1161 +
  1.1162 +	for(int i=0; i<9; i++) *dst++ -= *op++;
  1.1163 +}
  1.1164 +
  1.1165 +void Matrix3x3::operator *=(const Matrix3x3 &mat) {
  1.1166 +	Matrix4x4 tmp;
  1.1167 +
  1.1168 +	for(int i=0; i<3; i++) {
  1.1169 +		for(int j=0; j<3; j++) {
  1.1170 +			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];
  1.1171 +		}
  1.1172 +	}
  1.1173 +
  1.1174 +	memcpy(m, tmp.m, 9*sizeof(float));
  1.1175 +}
  1.1176 +
  1.1177 +void Matrix3x3::operator *=(float scalar) {
  1.1178 +	float *dst = (float*)m;
  1.1179 +
  1.1180 +	for(int i=0; i<9; i++) *dst++ *= scalar;
  1.1181 +}
  1.1182 +
  1.1183 +
  1.1184 +void Matrix3x3::ResetIdentity() {
  1.1185 +	memset(m, 0, 9 * sizeof(float));
  1.1186 +	m[0][0] = m[1][1] = m[2][2] = 1.0f;
  1.1187 +}
  1.1188 +
  1.1189 +void Matrix3x3::Translate(float x, float y) {
  1.1190 +	Matrix3x3 tmp(	1, 0, 0,
  1.1191 +					0, 1, 0,
  1.1192 +					x, y, 1 );
  1.1193 +	*this *= tmp;	
  1.1194 +}
  1.1195 +
  1.1196 +void Matrix3x3::Rotate(float angle) {
  1.1197 +	Matrix3x3 tmp(	fcos(angle), fsin(angle),	0,
  1.1198 +					-fsin(angle), fcos(angle),	0,
  1.1199 +						0,				0,		1 );
  1.1200 +	*this *= tmp;
  1.1201 +}
  1.1202 +
  1.1203 +void Matrix3x3::Scale(float x, float y) {
  1.1204 +	Matrix3x3 tmp(	x, 0, 0,
  1.1205 +					0, y, 0,
  1.1206 +					0, 0, 1);
  1.1207 +
  1.1208 +	*this *= tmp;
  1.1209 +}
  1.1210 +
  1.1211 +void Matrix3x3::SetTranslation(float x, float y) {
  1.1212 +	Matrix3x3(	1, 0, 0,
  1.1213 +				0, 1, 0,
  1.1214 +				x, y, 1 );
  1.1215 +}
  1.1216 +
  1.1217 +void Matrix3x3::SetRotation(float angle) {
  1.1218 +	Matrix3x3(	fcos(angle),	fsin(angle),	0,
  1.1219 +				-fsin(angle),	fcos(angle),	0,
  1.1220 +					0,				0,			1 );
  1.1221 +}
  1.1222 +
  1.1223 +void Matrix3x3::SetScaling(float x, float y) {
  1.1224 +	Matrix3x3(	x, 0, 0,
  1.1225 +				0, y, 0,
  1.1226 +				0, 0, 1 );
  1.1227 +}
  1.1228 +
  1.1229 +void Matrix3x3::SetColumnVector(const Vector3 &vec, int columnindex) {
  1.1230 +	m[columnindex][0] = vec.x;
  1.1231 +	m[columnindex][1] = vec.y;
  1.1232 +	m[columnindex][2] = vec.z;
  1.1233 +}
  1.1234 +
  1.1235 +void Matrix3x3::SetRowVector(const Vector3 &vec, int rowindex) {
  1.1236 +	m[0][rowindex] = vec.x;
  1.1237 +	m[1][rowindex] = vec.y;
  1.1238 +	m[2][rowindex] = vec.z;
  1.1239 +}
  1.1240 +
  1.1241 +Vector3 Matrix3x3::GetColumnVector(int columnindex) const {
  1.1242 +	return Vector3(m[columnindex][0], m[columnindex][1], m[columnindex][2]);
  1.1243 +}
  1.1244 +
  1.1245 +Vector3 Matrix3x3::GetRowVector(int rowindex) const {
  1.1246 +	return Vector3(m[0][rowindex], m[1][rowindex], m[2][rowindex]);
  1.1247 +}
  1.1248 +
  1.1249 +void Matrix3x3::Transpose() {
  1.1250 +	Matrix3x3 mat = *this;
  1.1251 +
  1.1252 +	for(int i=0; i<3; i++) {
  1.1253 +		for(int j=0; j<3; j++) {
  1.1254 +			m[i][j] = mat.m[j][i];
  1.1255 +		}
  1.1256 +	}	
  1.1257 +}
  1.1258 +
  1.1259 +Matrix3x3 Matrix3x3::Transposed() const {
  1.1260 +	Matrix3x3 mat;
  1.1261 +
  1.1262 +	for(int i=0; i<3; i++) {
  1.1263 +		for(int j=0; j<3; j++) {
  1.1264 +			mat.m[i][j] = m[j][i];
  1.1265 +		}
  1.1266 +	}
  1.1267 +
  1.1268 +	return mat;
  1.1269 +}
  1.1270 +
  1.1271 +
  1.1272 +void Matrix3x3::OrthoNormalize() {
  1.1273 +	Vector3 i, j, k;
  1.1274 +	i = GetRowVector(0);
  1.1275 +	j = GetRowVector(1);
  1.1276 +	k = GetRowVector(2);
  1.1277 +
  1.1278 +	i = CrossProduct(j, k);
  1.1279 +	j = CrossProduct(k, i);
  1.1280 +	k = CrossProduct(i, j);
  1.1281 +
  1.1282 +	SetRowVector(i, 0);
  1.1283 +	SetRowVector(j, 1);
  1.1284 +	SetRowVector(k, 2);
  1.1285 +}
  1.1286 +
  1.1287 +Matrix3x3 Matrix3x3::OrthoNormalized() {
  1.1288 +	Vector3 i, j, k;
  1.1289 +	i = GetRowVector(0);
  1.1290 +	j = GetRowVector(1);
  1.1291 +	k = GetRowVector(2);
  1.1292 +
  1.1293 +	i = CrossProduct(j, k);
  1.1294 +	j = CrossProduct(k, i);
  1.1295 +	k = CrossProduct(i, j);
  1.1296 +
  1.1297 +	Matrix3x3 newmat;
  1.1298 +	newmat.SetRowVector(i, 0);
  1.1299 +	newmat.SetRowVector(j, 1);
  1.1300 +	newmat.SetRowVector(k, 2);
  1.1301 +
  1.1302 +	return newmat;
  1.1303 +}
  1.1304 +
  1.1305 +
  1.1306 +
  1.1307 +// ----------- Ray implementation --------------
  1.1308 +Ray::Ray() {
  1.1309 +	Origin = Vector3(0.0f, 0.0f, 0.0f);
  1.1310 +	Direction = Vector3(0.0f, 0.0f, 1.0f);
  1.1311 +	Energy = 1.0f;
  1.1312 +	CurrentIOR = 1.0f;
  1.1313 +}
  1.1314 +
  1.1315 +Ray::Ray(const Vector3 &origin, const Vector3 &direction) {
  1.1316 +	Origin = origin;
  1.1317 +	Direction = direction;
  1.1318 +}
  1.1319 +
  1.1320 +// ----------- Base implementation --------------
  1.1321 +Base::Base() {
  1.1322 +	i = Vector3(1, 0, 0);
  1.1323 +	j = Vector3(0, 1, 0);
  1.1324 +	k = Vector3(0, 0, 1);
  1.1325 +}
  1.1326 +
  1.1327 +Base::Base(const Vector3 &i, const Vector3 &j, const Vector3 &k) {
  1.1328 +	this->i = i;
  1.1329 +	this->j = j;
  1.1330 +	this->k = k;
  1.1331 +}
  1.1332 +
  1.1333 +Base::Base(const Vector3 &dir, bool LeftHanded) {
  1.1334 +	k = dir;
  1.1335 +	j = VECTOR3_J;
  1.1336 +	i = CrossProduct(j, k);
  1.1337 +	j = CrossProduct(k, i);
  1.1338 +}
  1.1339 +
  1.1340 +
  1.1341 +void Base::Rotate(float x, float y, float z) {
  1.1342 +	Matrix4x4 RotMat;
  1.1343 +	RotMat.SetRotation(x, y, z);
  1.1344 +	i.Transform(RotMat);
  1.1345 +	j.Transform(RotMat);
  1.1346 +	k.Transform(RotMat);
  1.1347 +}
  1.1348 +
  1.1349 +void Base::Rotate(const Vector3 &axis, float angle) {
  1.1350 +	Quaternion q;
  1.1351 +	q.SetRotation(axis, angle);
  1.1352 +	i.Transform(q);
  1.1353 +	j.Transform(q);
  1.1354 +	k.Transform(q);
  1.1355 +}
  1.1356 +
  1.1357 +void Base::Rotate(const Matrix4x4 &mat) {
  1.1358 +	i.Transform(mat);
  1.1359 +	j.Transform(mat);
  1.1360 +	k.Transform(mat);
  1.1361 +}
  1.1362 +
  1.1363 +void Base::Rotate(const Quaternion &quat) {
  1.1364 +	i.Transform(quat);
  1.1365 +	j.Transform(quat);
  1.1366 +	k.Transform(quat);
  1.1367 +}
  1.1368 +
  1.1369 +Matrix3x3 Base::CreateRotationMatrix() const {
  1.1370 +	return Matrix3x3(	i.x, i.y, i.z,
  1.1371 +						j.x, j.y, j.z,
  1.1372 +						k.x, k.y, k.z);
  1.1373 +}
  1.1374 \ No newline at end of file