nuclear@0: #include "n3dmath.h"
nuclear@0: 
nuclear@0: #define fsin (float)sin
nuclear@0: #define fcos (float)cos
nuclear@0: 
nuclear@0: float frand(float range) {
nuclear@0: 	return ((float)rand() / (float)RAND_MAX) * range;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector3::Vector3() {
nuclear@0: 	x = y = z = 0.0f;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector3::Vector3(float x, float y, float z) {
nuclear@0: 	this->x = x;
nuclear@0: 	this->y = y;
nuclear@0: 	this->z = z;
nuclear@0: }
nuclear@0: /*  inlined
nuclear@0: float Vector3::DotProduct(const Vector3 &vec) const {
nuclear@0: 	return x * vec.x + y * vec.y + z * vec.z;
nuclear@0: }
nuclear@0: 
nuclear@0: float DotProduct(const Vector3 &vec1, const Vector3 &vec2) {
nuclear@0: 	return vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector3 Vector3::CrossProduct(const Vector3 &vec) const {
nuclear@0: 	return Vector3(y * vec.z - z * vec.y,  z * vec.x - x * vec.z,  x * vec.y - y * vec.x);
nuclear@0: }
nuclear@0: 
nuclear@0: Vector3 CrossProduct(const Vector3 &vec1, const Vector3 &vec2) {
nuclear@0: 	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: }
nuclear@0: 
nuclear@0: Vector3 Vector3::operator +(const Vector3 &vec) const {
nuclear@0: 	return Vector3(x + vec.x, y + vec.y, z + vec.z);
nuclear@0: }
nuclear@0: 
nuclear@0: Vector3 Vector3::operator -(const Vector3 &vec) const {
nuclear@0: 	return Vector3(x - vec.x, y - vec.y, z - vec.z);
nuclear@0: }
nuclear@0: 
nuclear@0: Vector3 Vector3::operator *(float scalar) const {
nuclear@0: 	return Vector3(x * scalar, y * scalar, z * scalar);
nuclear@0: }
nuclear@0: 
nuclear@0: Vector3 Vector3::operator /(float scalar) const {
nuclear@0: 	return Vector3(x / scalar, y / scalar, z / scalar);
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector3::operator +=(const Vector3 &vec) {
nuclear@0: 	x += vec.x;
nuclear@0: 	y += vec.y;
nuclear@0: 	z += vec.z;
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector3::operator -=(const Vector3 &vec) {
nuclear@0: 	x -= vec.x;
nuclear@0: 	y -= vec.y;
nuclear@0: 	z -= vec.z;
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector3::operator *=(float scalar) {
nuclear@0: 	x *= scalar;
nuclear@0: 	y *= scalar;
nuclear@0: 	z *= scalar;
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector3::operator /=(float scalar) {
nuclear@0: 	x /= scalar;
nuclear@0: 	y /= scalar;
nuclear@0: 	z /= scalar;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector3 Vector3::operator -() const {
nuclear@0: 	return Vector3(-x, -y, -z);
nuclear@0: }
nuclear@0: 
nuclear@0: bool Vector3::operator >(const Vector3 &vec) const {
nuclear@0: 	return LengthSq() > vec.LengthSq();
nuclear@0: }
nuclear@0: 
nuclear@0: bool Vector3::operator <(const Vector3 &vec) const {
nuclear@0: 	return LengthSq() < vec.LengthSq();
nuclear@0: }
nuclear@0: 
nuclear@0: bool Vector3::operator >(float len) const {
nuclear@0: 	return LengthSq() > len;
nuclear@0: }
nuclear@0: 
nuclear@0: bool Vector3::operator <(float len) const {
nuclear@0: 	return LengthSq() < len;
nuclear@0: }
nuclear@0: 
nuclear@0: bool Vector3::operator ==(const Vector3 &vec) const {
nuclear@0: 	return ((*this - vec).Length() < XSmallNumber);
nuclear@0: }
nuclear@0: 
nuclear@0: bool Vector3::operator ==(float len) const {
nuclear@0: 	return ((this->Length() - len) < XSmallNumber);
nuclear@0: }
nuclear@0: 
nuclear@0: Vector3::operator Vector2() const {
nuclear@0: 	return Vector2(x, y);
nuclear@0: }
nuclear@0: 
nuclear@0: Vector3::operator Vector4() const {
nuclear@0: 	return Vector4(x, y, z, 1.0f);
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: float Vector3::Length() const {
nuclear@0: 	return (float)sqrt(x*x + y*y + z*z);
nuclear@0: }
nuclear@0: 
nuclear@0: float Vector3::LengthSq() const {
nuclear@0: 	return x*x + y*y + z*z;
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector3::Normalize() {
nuclear@0: 	float len = (float)sqrt(x*x + y*y + z*z);
nuclear@0: 	x /= len;
nuclear@0: 	y /= len;
nuclear@0: 	z /= len;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector3 Vector3::Normalized() const {
nuclear@0: 	float len = (float)sqrt(x*x + y*y + z*z);
nuclear@0: 	return Vector3(x / len, y / len, z / len);
nuclear@0: }
nuclear@0: 
nuclear@0: Vector3 Vector3::Reflection(const Vector3 &normal) const {
nuclear@0: 	return normal * this->DotProduct(normal) * 2.0f - *this;
nuclear@0: }
nuclear@0: */
nuclear@0: Vector3 Vector3::Refraction(const Vector3 &normal, float FromIOR, float ToIOR) const {
nuclear@0: 	float m = FromIOR / ToIOR;
nuclear@0: 	Vector3 dir = *this;
nuclear@0: 	dir.Normalize();
nuclear@0: 	float CosAngleIncoming = dir.DotProduct(normal);
nuclear@0: 	float CosAngleRefr = (1.0f / (m*m)) * (float)sqrt(1.0f - m*m * (1 - CosAngleIncoming * CosAngleIncoming));
nuclear@0: 
nuclear@0: 	return dir * m - normal * (CosAngleRefr + m * CosAngleIncoming);
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector3::Transform(const Matrix4x4 &mat) {
nuclear@0: 	// assume row vectors
nuclear@0: 	float nx = x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0] + mat.m[3][0];
nuclear@0: 	float ny = x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1] + mat.m[3][1];
nuclear@0: 	z  = x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] + mat.m[3][2];
nuclear@0: 	x = nx;
nuclear@0: 	y = ny;
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector3::Transform(const Quaternion &quat) {
nuclear@0: 	Quaternion vq(0.0f, *this);
nuclear@0: 	vq = quat * vq * quat.Inverse();
nuclear@0: 	*this = vq.v;
nuclear@0: }
nuclear@0: 
nuclear@0: // direct transformations
nuclear@0: 
nuclear@0: void Vector3::Translate(float x, float y, float z) {
nuclear@0: 	this->x += x;
nuclear@0: 	this->y += y;
nuclear@0: 	this->z += z;
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector3::Rotate(float x, float y, float z) {
nuclear@0: 	
nuclear@0: 	Matrix4x4 xform;
nuclear@0: 	xform.SetRotation(x, y, z);
nuclear@0: 	
nuclear@0: 	Transform(xform);
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector3::Rotate(const Vector3 &axis, float angle) {
nuclear@0: 
nuclear@0: 	Matrix4x4 xform;
nuclear@0: 	xform.SetRotation(axis, angle);
nuclear@0: 
nuclear@0: 	Transform(xform);
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector3::Scale(float x, float y, float z) {
nuclear@0: 	this->x *= x;
nuclear@0: 	this->y *= y;
nuclear@0: 	this->z *= z;
nuclear@0: }
nuclear@0: 
nuclear@0: float &Vector3::operator [](int index) {
nuclear@0: 	return !index ? x : index == 1 ? y : z;
nuclear@0: }
nuclear@0: 
nuclear@0: std::ostream &operator <<(std::ostream &out, const Vector3 &vec) {
nuclear@0: 	out << vec.x << ", " << vec.y << ", " << vec.z;
nuclear@0: 	return out;
nuclear@0: }
nuclear@0: 
nuclear@0: // ------------- Vector4 implementation ---------------
nuclear@0: 
nuclear@0: Vector4::Vector4() {
nuclear@0: 	x = y = z = 0.0f;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector4::Vector4(const Vector4 &vec) {
nuclear@0: 	x = vec.x;
nuclear@0: 	y = vec.y;
nuclear@0: 	z = vec.z;
nuclear@0: 	w = vec.w;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector4::Vector4(const Vector3 &vec) {
nuclear@0: 	x = vec.x;
nuclear@0: 	y = vec.y;
nuclear@0: 	z = vec.z;
nuclear@0: 	w = 1.0f;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector4::Vector4(float x, float y, float z, float w) {
nuclear@0: 	this->x = x;
nuclear@0: 	this->y = y;
nuclear@0: 	this->z = z;
nuclear@0: 	this->w = w;
nuclear@0: }
nuclear@0: 
nuclear@0: float Vector4::DotProduct(const Vector4 &vec) const {
nuclear@0: 	return x * vec.x + y * vec.y + z * vec.z + w * vec.w;
nuclear@0: }
nuclear@0: 
nuclear@0: float DotProduct(const Vector4 &vec1, const Vector4 &vec2) {
nuclear@0: 	return vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z + vec1.w * vec2.w;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector4 Vector4::CrossProduct(const Vector4 &vec1, const Vector4 &vec2) const {
nuclear@0:     float  A, B, C, D, E, F;       // Intermediate Values
nuclear@0:     Vector4 result;
nuclear@0: 
nuclear@0:     // Calculate intermediate values.
nuclear@0:     A = (vec1.x * vec2.y) - (vec1.y * vec2.x);
nuclear@0:     B = (vec1.x * vec2.z) - (vec1.z * vec2.x);
nuclear@0:     C = (vec1.x * vec2.w) - (vec1.w * vec2.x);
nuclear@0:     D = (vec1.y * vec2.z) - (vec1.z * vec2.y);
nuclear@0:     E = (vec1.y * vec2.w) - (vec1.w * vec2.y);
nuclear@0:     F = (vec1.z * vec2.w) - (vec1.w * vec2.z);
nuclear@0: 
nuclear@0:     // Calculate the result-vector components.
nuclear@0:     result.x =   (y * F) - (z * E) + (w * D);
nuclear@0:     result.y = - (x * F) + (z * C) - (w * B);
nuclear@0:     result.z =   (x * E) - (y * C) + (w * A);
nuclear@0:     result.w = - (x * D) + (y * B) - (z * A);
nuclear@0:     return result;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector4 CrossProduct(const Vector4 &vec1, const Vector4 &vec2, const Vector4 &vec3) {
nuclear@0:     float  A, B, C, D, E, F;       // Intermediate Values
nuclear@0:     Vector4 result;
nuclear@0: 
nuclear@0:     // Calculate intermediate values.
nuclear@0:     A = (vec2.x * vec3.y) - (vec2.y * vec3.x);
nuclear@0:     B = (vec2.x * vec3.z) - (vec2.z * vec3.x);
nuclear@0:     C = (vec2.x * vec3.w) - (vec2.w * vec3.x);
nuclear@0:     D = (vec2.y * vec3.z) - (vec2.z * vec3.y);
nuclear@0:     E = (vec2.y * vec3.w) - (vec2.w * vec3.y);
nuclear@0:     F = (vec2.z * vec3.w) - (vec2.w * vec3.z);
nuclear@0: 
nuclear@0:     // Calculate the result-vector components.
nuclear@0:     result.x =   (vec1.y * F) - (vec1.z * E) + (vec1.w * D);
nuclear@0:     result.y = - (vec1.x * F) + (vec1.z * C) - (vec1.w * B);
nuclear@0:     result.z =   (vec1.x * E) - (vec1.y * C) + (vec1.w * A);
nuclear@0:     result.w = - (vec1.x * D) + (vec1.y * B) - (vec1.z * A);
nuclear@0:     return result;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector4 Vector4::operator +(const Vector4 &vec) const {
nuclear@0: 	return Vector4(x + vec.x, y + vec.y, z + vec.z, w + vec.w);
nuclear@0: }
nuclear@0: 
nuclear@0: Vector4 Vector4::operator -(const Vector4 &vec) const {
nuclear@0: 	return Vector4(x - vec.x, y - vec.y, z - vec.z, w - vec.w);
nuclear@0: }
nuclear@0: 
nuclear@0: Vector4 Vector4::operator *(float scalar) const {
nuclear@0: 	return Vector4(x * scalar, y * scalar, z * scalar, w * scalar);
nuclear@0: }
nuclear@0: 
nuclear@0: Vector4 Vector4::operator /(float scalar) const {
nuclear@0: 	return Vector4(x / scalar, y / scalar, z / scalar, w / scalar);
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector4::operator +=(const Vector4 &vec) {
nuclear@0: 	x += vec.x;
nuclear@0: 	y += vec.y;
nuclear@0: 	z += vec.z;
nuclear@0: 	w += vec.w;
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector4::operator -=(const Vector4 &vec) {
nuclear@0: 	x -= vec.x;
nuclear@0: 	y -= vec.y;
nuclear@0: 	z -= vec.z;
nuclear@0: 	w -= vec.w;
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector4::operator *=(float scalar) {
nuclear@0: 	x *= scalar;
nuclear@0: 	y *= scalar;
nuclear@0: 	z *= scalar;
nuclear@0: 	w *= scalar;
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector4::operator /=(float scalar) {
nuclear@0: 	x /= scalar;
nuclear@0: 	y /= scalar;
nuclear@0: 	z /= scalar;
nuclear@0: 	w /= scalar;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector4 Vector4::operator -() const {
nuclear@0: 	return Vector4(-x, -y, -z, -w);
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: bool Vector4::operator >(const Vector4 &vec) const {
nuclear@0: 	return LengthSq() > vec.LengthSq();
nuclear@0: }
nuclear@0: 
nuclear@0: bool Vector4::operator <(const Vector4 &vec) const {
nuclear@0: 	return LengthSq() < vec.LengthSq();
nuclear@0: }
nuclear@0: 
nuclear@0: bool Vector4::operator >(float len) const {
nuclear@0: 	return LengthSq() > len;
nuclear@0: }
nuclear@0: 
nuclear@0: bool Vector4::operator <(float len) const {
nuclear@0: 	return LengthSq() < len;
nuclear@0: }
nuclear@0: 
nuclear@0: bool Vector4::operator ==(const Vector4 &vec) const {
nuclear@0: 	return ((*this - vec).Length() < XSmallNumber);
nuclear@0: }
nuclear@0: 
nuclear@0: bool Vector4::operator ==(float len) const {
nuclear@0: 	return ((this->Length() - len) < XSmallNumber);
nuclear@0: }
nuclear@0: 
nuclear@0: Vector4::operator Vector3() const {
nuclear@0: 	return Vector3(x, y, z);
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: float Vector4::Length() const {
nuclear@0: 	return (float)sqrt(x*x + y*y + z*z + w*w);
nuclear@0: }
nuclear@0: 
nuclear@0: float Vector4::LengthSq() const {
nuclear@0: 	return x*x + y*y + z*z + w*w;
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector4::Normalize() {
nuclear@0: 	float len = (float)sqrt(x*x + y*y + z*z + w*w);
nuclear@0: 	x /= len;
nuclear@0: 	y /= len;
nuclear@0: 	z /= len;
nuclear@0: 	w /= len;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector4 Vector4::Normalized() const {
nuclear@0: 	float len = (float)sqrt(x*x + y*y + z*z + w*w);
nuclear@0: 	return Vector4(x / len, y / len, z / len, w / len);
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector4::Transform(const Matrix4x4 &mat) {
nuclear@0: 	// assume row vectors
nuclear@0: 	float nx = x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0] + w * mat.m[3][0];
nuclear@0: 	float ny = x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1] + w * mat.m[3][1];
nuclear@0: 	float nz = x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] + w * mat.m[3][2];
nuclear@0: 	w = x * mat.m[0][3] + y * mat.m[1][3] + z * mat.m[2][3] + w * mat.m[3][3];
nuclear@0: 	x = nx;
nuclear@0: 	y = ny;
nuclear@0: 	z = nz;
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: // Direct transformations on the vector
nuclear@0: void Vector4::Translate(float x, float y, float z, float w) {
nuclear@0: 	x += x;
nuclear@0: 	y += y;
nuclear@0: 	z += z;
nuclear@0: 	w += w;
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector4::Rotate(float x, float y, float z) {
nuclear@0: 	Matrix4x4 xform;
nuclear@0: 	xform.SetRotation(x, y, z);
nuclear@0: 	Transform(xform);
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector4::Rotate(const Vector3 &axis, float angle) {
nuclear@0: 	Matrix4x4 xform;
nuclear@0: 	xform.SetRotation(axis, angle);
nuclear@0: 	Transform(xform);
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector4::Scale(float x, float y, float z, float w) {
nuclear@0: 	this->x *= x;
nuclear@0: 	this->y *= y;
nuclear@0: 	this->z *= z;
nuclear@0: 	this->w *= w;
nuclear@0: }
nuclear@0: 
nuclear@0: float &Vector4::operator [](int index) {
nuclear@0: 	return !index ? x : index == 1 ? y : index == 2 ? z : w;
nuclear@0: }
nuclear@0: 
nuclear@0: std::ostream &operator <<(std::ostream &out, const Vector4 &vec) {
nuclear@0: 	out << vec.x << ", " << vec.y << ", " << vec.z << ", " << vec.w;
nuclear@0: 	return out;
nuclear@0: }
nuclear@0: 
nuclear@0: // ------------- Vector2 implementation ---------------
nuclear@0: 
nuclear@0: Vector2::Vector2() {
nuclear@0: 	x = y = 0.0f;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector2::Vector2(const Vector2 &vec) {
nuclear@0: 	x = vec.x;
nuclear@0: 	y = vec.y;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector2::Vector2(float x, float y) {
nuclear@0: 	this->x = x;
nuclear@0: 	this->y = y;
nuclear@0: }
nuclear@0: 
nuclear@0: float Vector2::DotProduct(const Vector2 &vec) const {
nuclear@0: 	return x * vec.x + y * vec.y;
nuclear@0: }
nuclear@0: 
nuclear@0: float DotProduct(const Vector2 &vec1, const Vector2 &vec2) {
nuclear@0: 	return vec1.x * vec2.x + vec1.y + vec2.y;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector2 Vector2::operator +(const Vector2 &vec) const {
nuclear@0: 	return Vector2(x + vec.x, y + vec.y);
nuclear@0: }
nuclear@0: 
nuclear@0: Vector2 Vector2::operator -(const Vector2 &vec) const {
nuclear@0: 	return Vector2(x - vec.x, y - vec.y);
nuclear@0: }
nuclear@0: 
nuclear@0: Vector2 Vector2::operator *(float scalar) const {
nuclear@0: 	return Vector2(x * scalar, y * scalar);
nuclear@0: }
nuclear@0: 
nuclear@0: Vector2 Vector2::operator /(float scalar) const {
nuclear@0: 	return Vector2(x / scalar, y / scalar);
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector2::operator +=(const Vector2 &vec) {
nuclear@0: 	x += vec.x;
nuclear@0: 	y += vec.y;
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector2::operator -=(const Vector2 &vec) {
nuclear@0: 	x -= vec.x;
nuclear@0: 	y -= vec.y;
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector2::operator *=(float scalar) {
nuclear@0: 	x *= scalar;
nuclear@0: 	y *= scalar;
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector2::operator /=(float scalar) {
nuclear@0: 	x /= scalar;
nuclear@0: 	y /= scalar;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector2 Vector2::operator -() const {
nuclear@0: 	return Vector2(-x, -y);
nuclear@0: }
nuclear@0: 
nuclear@0: bool Vector2::operator >(const Vector2 &vec) const {
nuclear@0: 	return LengthSq() > vec.LengthSq();
nuclear@0: }
nuclear@0: 
nuclear@0: bool Vector2::operator <(const Vector2 &vec) const {
nuclear@0: 	return LengthSq() < vec.LengthSq();
nuclear@0: }
nuclear@0: 
nuclear@0: bool Vector2::operator >(float len) const {
nuclear@0: 	return LengthSq() > len;
nuclear@0: }
nuclear@0: 
nuclear@0: bool Vector2::operator <(float len) const {
nuclear@0: 	return LengthSq() < len;
nuclear@0: }
nuclear@0: 
nuclear@0: bool Vector2::operator ==(const Vector2 &vec) const {
nuclear@0: 	return ((*this - vec).Length() < XSmallNumber);
nuclear@0: }
nuclear@0: 
nuclear@0: bool Vector2::operator ==(float len) const {
nuclear@0: 	return ((this->Length() - len) < XSmallNumber);
nuclear@0: }
nuclear@0: 
nuclear@0: Vector2::operator Vector3() const {
nuclear@0: 	return Vector3(x, y, 1.0f);
nuclear@0: }
nuclear@0: 
nuclear@0: float Vector2::Length() const {
nuclear@0: 	return (float)sqrt(x * x + y * y);
nuclear@0: }
nuclear@0: 
nuclear@0: float Vector2::LengthSq() const {
nuclear@0: 	return x * x + y * y;
nuclear@0: }
nuclear@0: 	
nuclear@0: void Vector2::Normalize() {
nuclear@0: 	float len = (float)sqrt(x * x + y * y);
nuclear@0: 	x /= len;
nuclear@0: 	y /= len;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector2 Vector2::Normalized() const {
nuclear@0: 	float len = (float)sqrt(x * x + y * y);
nuclear@0: 	return Vector2(x / len, y / len);
nuclear@0: }
nuclear@0: 
nuclear@0: //Vector2 Vector2::Reflection(const Vector2 &normal) const;
nuclear@0: //Vector2 Vector2::Refraction(const Vector2 &normal, float FromIOR, float ToIOR) const;
nuclear@0: 	
nuclear@0: void Vector2::Transform(const Matrix3x3 &mat) {
nuclear@0: 	float nx = x * mat.m[0][0] + y * mat.m[1][0] + mat.m[2][0];
nuclear@0: 	y = x * mat.m[0][1] + y * mat.m[1][1] + mat.m[2][1];
nuclear@0: 	x = nx;
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector2::Translate(float x, float y) {
nuclear@0: 	this->x += x;
nuclear@0: 	this->y += y;
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector2::Rotate(float angle) {
nuclear@0: 	Matrix3x3 xform;
nuclear@0: 	xform.SetRotation(angle);
nuclear@0: 
nuclear@0: 	Transform(xform);
nuclear@0: }
nuclear@0: 
nuclear@0: void Vector2::Scale(float x, float y) {
nuclear@0: 	this->x *= x;
nuclear@0: 	this->y *= y;
nuclear@0: }
nuclear@0: 
nuclear@0: float &Vector2::operator [](int index) {
nuclear@0: 	return !index ? x : y;
nuclear@0: }
nuclear@0: 
nuclear@0: std::ostream &operator <<(std::ostream &out, const Vector2 &vec) {
nuclear@0: 	out << vec.x << ", " << vec.y;
nuclear@0: 	return out;
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: // --------------- Quaternion implementation ---------------
nuclear@0: 
nuclear@0: Quaternion::Quaternion() {
nuclear@0: 	s = 1.0f;
nuclear@0: 	v.x = v.y = v.z = 0.0f;
nuclear@0: }
nuclear@0: 
nuclear@0: Quaternion::Quaternion(float s, float x, float y, float z) {
nuclear@0: 	v.x = x;
nuclear@0: 	v.y = y;
nuclear@0: 	v.z = z;
nuclear@0: 	this->s = s;
nuclear@0: }
nuclear@0: 
nuclear@0: Quaternion::Quaternion(float s, const Vector3 &v) {
nuclear@0: 	this->s = s;
nuclear@0: 	this->v = v;
nuclear@0: }
nuclear@0: 
nuclear@0: Quaternion Quaternion::operator +(const Quaternion &quat) const {
nuclear@0: 	return Quaternion(s + quat.s, v + quat.v);
nuclear@0: }
nuclear@0: 
nuclear@0: Quaternion Quaternion::operator -(const Quaternion &quat) const {
nuclear@0: 	return Quaternion(s - quat.s, v - quat.v);
nuclear@0: }
nuclear@0: 
nuclear@0: Quaternion Quaternion::operator -() const {
nuclear@0: 	return Quaternion(-s, -v);
nuclear@0: }
nuclear@0: 
nuclear@0: // Quaternion Multiplication:
nuclear@0: // Q1*Q2 = [s1*s2 - v1.v2,  s1*v2 + s2*v1 + v1(x)v2]
nuclear@0: Quaternion Quaternion::operator *(const Quaternion &quat) const {
nuclear@0: 	Quaternion newq;	
nuclear@0: 	newq.s = s * quat.s - DotProduct(v, quat.v);
nuclear@0: 	newq.v = quat.v * s + v * quat.s + CrossProduct(v, quat.v);	
nuclear@0: 	return newq;
nuclear@0: }
nuclear@0: 
nuclear@0: void Quaternion::operator +=(const Quaternion &quat) {
nuclear@0: 	*this = Quaternion(s + quat.s, v + quat.v);
nuclear@0: }
nuclear@0: 
nuclear@0: void Quaternion::operator -=(const Quaternion &quat) {
nuclear@0: 	*this = Quaternion(s - quat.s, v - quat.v);
nuclear@0: }
nuclear@0: 
nuclear@0: void Quaternion::operator *=(const Quaternion &quat) {
nuclear@0: 	*this = *this * quat;
nuclear@0: }
nuclear@0: 
nuclear@0: void Quaternion::ResetIdentity() {
nuclear@0: 	s = 1.0f;
nuclear@0: 	v.x = v.y = v.z = 0.0f;
nuclear@0: }
nuclear@0: 
nuclear@0: Quaternion Quaternion::Conjugate() const {
nuclear@0: 	return Quaternion(s, -v);
nuclear@0: }
nuclear@0: 
nuclear@0: float Quaternion::Length() const {
nuclear@0: 	return (float)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
nuclear@0: }
nuclear@0: 
nuclear@0: // Q * ~Q = ||Q||^2
nuclear@0: float Quaternion::LengthSq() const {
nuclear@0: 	return v.x*v.x + v.y*v.y + v.z*v.z + s*s;
nuclear@0: }
nuclear@0: 
nuclear@0: void Quaternion::Normalize() {
nuclear@0: 	float len = (float)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
nuclear@0: 	v.x /= len;
nuclear@0: 	v.y /= len;
nuclear@0: 	v.z /= len;
nuclear@0: 	s /= len;
nuclear@0: }
nuclear@0: 
nuclear@0: Quaternion Quaternion::Normalized() const {
nuclear@0: 	Quaternion nq = *this;
nuclear@0: 	float len = (float)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
nuclear@0: 	nq.v.x /= len;
nuclear@0: 	nq.v.y /= len;
nuclear@0: 	nq.v.z /= len;
nuclear@0: 	nq.s /= len;
nuclear@0: 	return nq;
nuclear@0: }
nuclear@0: 
nuclear@0: // Quaternion Inversion: Q^-1 = ~Q / ||Q||^2
nuclear@0: Quaternion Quaternion::Inverse() const {
nuclear@0: 	Quaternion inv = Conjugate();
nuclear@0: 	float lensq = LengthSq();
nuclear@0: 	inv.v /= lensq;
nuclear@0: 	inv.s /= lensq;
nuclear@0: 
nuclear@0: 	return inv;
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: void Quaternion::SetRotation(const Vector3 &axis, float angle) {
nuclear@0: 	float HalfAngle = angle / 2.0f;
nuclear@0: 	s = cosf(HalfAngle);
nuclear@0: 	v = axis * sinf(HalfAngle);
nuclear@0: }
nuclear@0: 
nuclear@0: void Quaternion::Rotate(const Vector3 &axis, float angle) {
nuclear@0: 	Quaternion q;
nuclear@0: 	float HalfAngle = angle / 2.0f;
nuclear@0: 	q.s = cosf(HalfAngle);
nuclear@0: 	q.v = axis * sinf(HalfAngle);
nuclear@0: 
nuclear@0: 	*this *= q;
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: Matrix3x3 Quaternion::GetRotationMatrix() const {
nuclear@0: 	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: 						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: 						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: }
nuclear@0: 
nuclear@0: 
nuclear@0: // ------------- Matrix implementation ---------------
nuclear@0: 
nuclear@0: Matrix4x4::Matrix4x4() {
nuclear@0: 	memset(m, 0, 16*sizeof(float));
nuclear@0: 	m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0f;
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix4x4::Matrix4x4(const Matrix4x4 &mat) {
nuclear@0: 	memcpy(m, mat.m, 16*sizeof(float));
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix4x4::Matrix4x4(const Matrix3x3 &mat) {
nuclear@0: 	for(int i=0; i<3; i++) {
nuclear@0: 		for(int j=0; j<3; j++) {
nuclear@0: 			m[i][j] = mat.m[i][j];
nuclear@0: 		}
nuclear@0: 	}
nuclear@0: 	m[3][0] = m[3][1] = m[3][2] = m[0][3] = m[1][3] = m[2][3] = 0.0f;
nuclear@0: 	m[3][3] = 1.0f;
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix4x4::Matrix4x4(	float m00, float m01, float m02, float m03,
nuclear@0: 						float m10, float m11, float m12, float m13,
nuclear@0: 						float m20, float m21, float m22, float m23,
nuclear@0: 						float m30, float m31, float m32, float m33 ) {
nuclear@0: 
nuclear@0: 	memcpy(m, &m00, 16*sizeof(float));	// arguments are adjacent in stack
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix4x4 Matrix4x4::operator +(const Matrix4x4 &mat) const {
nuclear@0: 
nuclear@0: 	Matrix4x4 tmp;
nuclear@0: 
nuclear@0: 	const float *op1 = (float*)m;
nuclear@0: 	const float *op2 = (float*)mat.m;
nuclear@0: 	float *dst = (float*)tmp.m;
nuclear@0: 
nuclear@0: 	for(int i=0; i<16; i++) *dst++ = *op1++ + *op2++;
nuclear@0: 
nuclear@0: 	return tmp;
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix4x4 Matrix4x4::operator -(const Matrix4x4 &mat) const {
nuclear@0: 
nuclear@0: 	Matrix4x4 tmp;
nuclear@0: 
nuclear@0: 	const float *op1 = (float*)m;
nuclear@0: 	const float *op2 = (float*)mat.m;
nuclear@0: 	float *dst = (float*)tmp.m;
nuclear@0: 
nuclear@0: 	for(int i=0; i<16; i++) *dst++ = *op1++ - *op2++;
nuclear@0: 
nuclear@0: 	return tmp;
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix4x4 Matrix4x4::operator *(float scalar) const {
nuclear@0: 
nuclear@0: 	Matrix4x4 tmp;
nuclear@0: 
nuclear@0: 	const float *op1 = (float*)m;
nuclear@0: 	float *dst = (float*)tmp.m;
nuclear@0: 
nuclear@0: 	for(int i=0; i<16; i++) *dst++ = *op1++ * scalar;
nuclear@0: 
nuclear@0: 	return tmp;
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix4x4 Matrix4x4::operator *(const Matrix4x4 &mat) const {
nuclear@0: 	Matrix4x4 tmp;
nuclear@0: 
nuclear@0: 	for(int i=0; i<4; i++) {
nuclear@0: 		for(int j=0; j<4; j++) {
nuclear@0: 			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: 		}
nuclear@0: 	}
nuclear@0: 
nuclear@0: 	return tmp;
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix4x4::operator +=(const Matrix4x4 &mat) {
nuclear@0: 
nuclear@0: 	const float *op2 = (float*)mat.m;
nuclear@0: 	float *dst = (float*)m;
nuclear@0: 
nuclear@0: 	for(int i=0; i<16; i++) *dst++ += *op2++;
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix4x4::operator -=(const Matrix4x4 &mat) {
nuclear@0: 
nuclear@0: 	const float *op2 = (float*)mat.m;
nuclear@0: 	float *dst = (float*)m;
nuclear@0: 
nuclear@0: 	for(int i=0; i<16; i++) *dst++ -= *op2++;
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix4x4::operator *=(float scalar) {
nuclear@0: 
nuclear@0: 	float *dst = (float*)m;
nuclear@0: 
nuclear@0: 	for(int i=0; i<16; i++) *dst++ *= scalar;
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix4x4::operator *=(const Matrix4x4 &mat) {
nuclear@0: 	Matrix4x4 tmp;
nuclear@0: 
nuclear@0: 	for(int i=0; i<4; i++) {
nuclear@0: 		for(int j=0; j<4; j++) {
nuclear@0: 			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: 		}
nuclear@0: 	}
nuclear@0: 
nuclear@0: 	memcpy(m, tmp.m, 16*sizeof(float));
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: void Matrix4x4::ResetIdentity() {
nuclear@0: 	memset(m, 0, 16*sizeof(float));
nuclear@0: 	m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0f;
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: // Transformations (assuming column vectors)
nuclear@0: 
nuclear@0: void Matrix4x4::Translate(float x, float y, float z) {
nuclear@0: 	
nuclear@0: 	Matrix4x4 tmp(	1, 0, 0, 0,
nuclear@0: 					0, 1, 0, 0,
nuclear@0: 					0, 0, 1, 0,
nuclear@0: 					x, y, z, 1 );
nuclear@0: 	*this *= tmp;
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix4x4::Rotate(float x, float y, float z) {
nuclear@0: 
nuclear@0: 	*this *= Matrix4x4(	1,	0,			0,			0,
nuclear@0: 						0,	fcos(x),	fsin(x),	0,
nuclear@0: 						0,	-fsin(x),	fcos(x),	0,
nuclear@0: 						0,	0,			0,			1 );
nuclear@0: 	
nuclear@0: 	*this *= Matrix4x4(	fcos(y),	0,	-fsin(y),	0,
nuclear@0: 						0,			1,	0,			0,
nuclear@0: 						fsin(y),	0,	fcos(y),	0,
nuclear@0: 						0,			0,	0,			1 );
nuclear@0: 
nuclear@0: 	*this *= Matrix4x4(	fcos(z),	fsin(z),	0,	0,
nuclear@0: 						-fsin(z),	fcos(z),	0,	0,
nuclear@0: 						0,			0,			1,	0,
nuclear@0: 						0,			0,			0,	1 );
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix4x4::Rotate(const Vector3 &axis, float angle) {
nuclear@0: 
nuclear@0: 	float sina = fsin(angle);
nuclear@0: 	float cosa = fcos(angle);
nuclear@0: 	float invcosa = 1-cosa;
nuclear@0: 	float nxsq = axis.x * axis.x;
nuclear@0: 	float nysq = axis.y * axis.y;
nuclear@0: 	float nzsq = axis.z * axis.z;
nuclear@0: 
nuclear@0: 	Matrix4x4 xform;
nuclear@0: 	xform.m[0][0] = nxsq + (1-nxsq) * cosa;
nuclear@0: 	xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
nuclear@0: 	xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
nuclear@0: 	xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
nuclear@0: 	xform.m[1][1] = nysq + (1-nysq) * cosa;
nuclear@0: 	xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
nuclear@0: 	xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
nuclear@0: 	xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
nuclear@0: 	xform.m[2][2] = nzsq + (1-nzsq) * cosa;
nuclear@0: 
nuclear@0: 	*this *= xform;
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix4x4::Scale(float x, float y, float z) {
nuclear@0: 	
nuclear@0: 	Matrix4x4 xform(x, 0, 0, 0,
nuclear@0: 					0, y, 0, 0,
nuclear@0: 					0, 0, z, 0,
nuclear@0: 					0, 0, 0, 1 );
nuclear@0: 	*this *= xform;
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: //////////////////////////////
nuclear@0: 
nuclear@0: void Matrix4x4::SetTranslation(float x, float y, float z) {
nuclear@0: 	
nuclear@0: 	*this = Matrix4x4(	1, 0, 0, 0,
nuclear@0: 						0, 1, 0, 0,
nuclear@0: 						0, 0, 1, 0,
nuclear@0: 						x, y, z, 1 );
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix4x4::SetRotation(float x, float y, float z) {
nuclear@0: 
nuclear@0: 	*this = Matrix4x4(	1,	0,			0,			0,
nuclear@0: 						0,	fcos(x),	fsin(x),	0,
nuclear@0: 						0,	-fsin(x),	fcos(x),	0,
nuclear@0: 						0,	0,			0,			1 );
nuclear@0: 	
nuclear@0: 	*this *= Matrix4x4(	fcos(y),	0,	-fsin(y),	0,
nuclear@0: 						0,			1,	0,			0,
nuclear@0: 						fsin(y),	0,	fcos(y),	0,
nuclear@0: 						0,			0,	0,			1 );
nuclear@0: 
nuclear@0: 	*this *= Matrix4x4(	fcos(z),	fsin(z),	0,	0,
nuclear@0: 						-fsin(z),	fcos(z),	0,	0,
nuclear@0: 						0,			0,			1,	0,
nuclear@0: 						0,			0,			0,	1 );
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix4x4::SetRotation(const Vector3 &axis, float angle) {
nuclear@0: 
nuclear@0: 	// caching of multiply used function results (opt)
nuclear@0: 	float sina = fsin(angle);
nuclear@0: 	float cosa = fcos(angle);
nuclear@0: 	float invcosa = 1-cosa;
nuclear@0: 	float nxsq = axis.x * axis.x;
nuclear@0: 	float nysq = axis.y * axis.y;
nuclear@0: 	float nzsq = axis.z * axis.z;
nuclear@0: 
nuclear@0: 	Matrix4x4 xform;
nuclear@0: 	xform.m[0][0] = nxsq + (1-nxsq) * cosa;
nuclear@0: 	xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
nuclear@0: 	xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
nuclear@0: 	xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
nuclear@0: 	xform.m[1][1] = nysq + (1-nysq) * cosa;
nuclear@0: 	xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
nuclear@0: 	xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
nuclear@0: 	xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
nuclear@0: 	xform.m[2][2] = nzsq + (1-nzsq) * cosa;
nuclear@0: 
nuclear@0: 	*this = xform;
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix4x4::SetScaling(float x, float y, float z) {
nuclear@0: 	
nuclear@0: 	Matrix4x4 xform(x, 0, 0, 0,
nuclear@0: 					0, y, 0, 0,
nuclear@0: 					0, 0, z, 0,
nuclear@0: 					0, 0, 0, 1 );
nuclear@0: 	*this = xform;
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix4x4::SetColumnVector(const Vector4 &vec, int columnindex) {
nuclear@0: 	
nuclear@0: 	m[0][columnindex] = vec.x;
nuclear@0: 	m[1][columnindex] = vec.y;
nuclear@0: 	m[2][columnindex] = vec.z;
nuclear@0: 	m[3][columnindex] = vec.w;
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix4x4::SetRowVector(const Vector4 &vec, int rowindex) {
nuclear@0: 
nuclear@0: 	m[rowindex][0] = vec.x;
nuclear@0: 	m[rowindex][1] = vec.y;
nuclear@0: 	m[rowindex][2] = vec.z;
nuclear@0: 	m[rowindex][3] = vec.w;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector4 Matrix4x4::GetColumnVector(int columnindex) const {
nuclear@0: 
nuclear@0: 	return Vector4(m[0][columnindex], m[1][columnindex], m[2][columnindex], m[3][columnindex]);
nuclear@0: }
nuclear@0: 
nuclear@0: Vector4 Matrix4x4::GetRowVector(int rowindex) const {
nuclear@0: 
nuclear@0: 	return Vector4(m[rowindex][0], m[rowindex][1], m[rowindex][2], m[rowindex][3]);
nuclear@0: }
nuclear@0: 
nuclear@0: // other operations on matrices
nuclear@0: 
nuclear@0: void Matrix4x4::Transpose() {
nuclear@0: 	Matrix4x4 mat = *this;
nuclear@0: 
nuclear@0: 	for(int i=0; i<4; i++) {
nuclear@0: 		for(int j=0; j<4; j++) {
nuclear@0: 			m[i][j] = mat.m[j][i];
nuclear@0: 		}
nuclear@0: 	}
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix4x4 Matrix4x4::Transposed() const {
nuclear@0: 	Matrix4x4 mat = *this;
nuclear@0: 
nuclear@0: 	for(int i=0; i<4; i++) {
nuclear@0: 		for(int j=0; j<4; j++) {
nuclear@0: 			mat.m[i][j] = m[j][i];
nuclear@0: 		}
nuclear@0: 	}
nuclear@0: 
nuclear@0: 	return mat;
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: float Matrix4x4::Determinant() const {
nuclear@0: 
nuclear@0: 	float det11 =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@0: 					(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@0: 					(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@0: 
nuclear@0: 	float det12 =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@0: 					(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@0: 					(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@0: 
nuclear@0: 	float det13 =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@0: 					(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@0: 					(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@0: 
nuclear@0: 	float det14 =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@0: 					(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@0: 					(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@0: 
nuclear@0: 	return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: Matrix4x4 Matrix4x4::Adjoint() const {
nuclear@0: 
nuclear@0: 	Matrix4x4 coef;
nuclear@0: 
nuclear@0: 	coef.m[0][0] =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@0: 					(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@0: 					(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@0: 	coef.m[0][1] =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@0: 					(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@0: 					(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@0: 	coef.m[0][2] =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@0: 					(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@0: 					(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@0: 	coef.m[0][3] =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@0: 					(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@0: 					(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@0: 
nuclear@0: 	coef.m[1][0] =	(m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@0: 					(m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@0: 					(m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@0: 	coef.m[1][1] =	(m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@0: 					(m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@0: 					(m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@0: 	coef.m[1][2] =	(m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@0: 					(m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@0: 					(m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@0: 	coef.m[1][3] =	(m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@0: 					(m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@0: 					(m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@0: 
nuclear@0: 	coef.m[2][0] =	(m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
nuclear@0: 					(m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
nuclear@0: 					(m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
nuclear@0: 	coef.m[2][1] =	(m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
nuclear@0: 					(m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
nuclear@0: 					(m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
nuclear@0: 	coef.m[2][2] =	(m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
nuclear@0: 					(m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
nuclear@0: 					(m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
nuclear@0: 	coef.m[2][3] =	(m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
nuclear@0: 					(m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
nuclear@0: 					(m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
nuclear@0: 
nuclear@0: 	coef.m[3][0] =	(m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
nuclear@0: 					(m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
nuclear@0: 					(m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
nuclear@0: 	coef.m[3][1] =	(m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
nuclear@0: 					(m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
nuclear@0: 					(m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
nuclear@0: 	coef.m[3][2] =	(m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
nuclear@0: 					(m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
nuclear@0: 					(m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
nuclear@0: 	coef.m[3][3] =	(m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
nuclear@0: 					(m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
nuclear@0: 					(m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
nuclear@0: 
nuclear@0: 	coef.Transpose();
nuclear@0: 
nuclear@0: 	float *elem = (float*)coef.m;
nuclear@0: 	for(int i=0; i<4; i++) {
nuclear@0: 		for(int j=0; j<4; j++) {
nuclear@0: 			coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
nuclear@0: 			if(i%2) coef.m[i][j] = -coef.m[i][j];
nuclear@0: 		}
nuclear@0: 	}
nuclear@0: 
nuclear@0: 	return coef;
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix4x4 Matrix4x4::Inverse() const {
nuclear@0: 
nuclear@0: 	Matrix4x4 AdjMat = Adjoint();
nuclear@0: 
nuclear@0: 	return AdjMat * (1.0f / Determinant());
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: // --------- 3 by 3 matrices implementation --------------
nuclear@0: 
nuclear@0: Matrix3x3::Matrix3x3() {
nuclear@0: 	memset(m, 0, 9 * sizeof(float));
nuclear@0: 	m[0][0] = m[1][1] = m[2][2] = 1.0f;
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix3x3::Matrix3x3(const Matrix3x3 &mat) {
nuclear@0: 	memcpy(m, mat.m, 9 * sizeof(float));
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix3x3::Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22) {
nuclear@0: 	memcpy(m, &m00, 9*sizeof(float));	// arguments are adjacent in stack
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix3x3 Matrix3x3::operator +(const Matrix3x3 &mat) const {
nuclear@0: 	Matrix3x3 tmp;
nuclear@0: 
nuclear@0: 	const float *op1 = (float*)m;
nuclear@0: 	const float *op2 = (float*)mat.m;
nuclear@0: 	float *dst = (float*)tmp.m;
nuclear@0: 
nuclear@0: 	for(int i=0; i<9; i++) *dst++ = *op1++ + *op2++;
nuclear@0: 
nuclear@0: 	return tmp;
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix3x3 Matrix3x3::operator -(const Matrix3x3 &mat) const {
nuclear@0: 	Matrix3x3 tmp;
nuclear@0: 
nuclear@0: 	const float *op1 = (float*)m;
nuclear@0: 	const float *op2 = (float*)mat.m;
nuclear@0: 	float *dst = (float*)tmp.m;
nuclear@0: 
nuclear@0: 	for(int i=0; i<9; i++) *dst++ = *op1++ - *op2++;
nuclear@0: 
nuclear@0: 	return tmp;
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix3x3 Matrix3x3::operator *(const Matrix3x3 &mat) const {
nuclear@0: 	Matrix3x3 tmp;
nuclear@0: 
nuclear@0: 	for(int i=0; i<3; i++) {
nuclear@0: 		for(int j=0; j<3; j++) {
nuclear@0: 			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: 		}
nuclear@0: 	}
nuclear@0: 
nuclear@0: 	return tmp;
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix3x3 Matrix3x3::operator *(float scalar) const {
nuclear@0: 	Matrix3x3 tmp;
nuclear@0: 
nuclear@0: 	const float *op1 = (float*)m;
nuclear@0: 	float *dst = (float*)tmp.m;
nuclear@0: 
nuclear@0: 	for(int i=0; i<9; i++) *dst++ = *op1++ * scalar;
nuclear@0: 
nuclear@0: 	return tmp;
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix3x3::operator +=(const Matrix3x3 &mat) {
nuclear@0: 	const float *op = (float*)mat.m;
nuclear@0: 	float *dst = (float*)m;
nuclear@0: 
nuclear@0: 	for(int i=0; i<9; i++) *dst++ += *op++;
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix3x3::operator -=(const Matrix3x3 &mat) {
nuclear@0: 	const float *op = (float*)mat.m;
nuclear@0: 	float *dst = (float*)m;
nuclear@0: 
nuclear@0: 	for(int i=0; i<9; i++) *dst++ -= *op++;
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix3x3::operator *=(const Matrix3x3 &mat) {
nuclear@0: 	Matrix4x4 tmp;
nuclear@0: 
nuclear@0: 	for(int i=0; i<3; i++) {
nuclear@0: 		for(int j=0; j<3; j++) {
nuclear@0: 			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: 		}
nuclear@0: 	}
nuclear@0: 
nuclear@0: 	memcpy(m, tmp.m, 9*sizeof(float));
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix3x3::operator *=(float scalar) {
nuclear@0: 	float *dst = (float*)m;
nuclear@0: 
nuclear@0: 	for(int i=0; i<9; i++) *dst++ *= scalar;
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: void Matrix3x3::ResetIdentity() {
nuclear@0: 	memset(m, 0, 9 * sizeof(float));
nuclear@0: 	m[0][0] = m[1][1] = m[2][2] = 1.0f;
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix3x3::Translate(float x, float y) {
nuclear@0: 	Matrix3x3 tmp(	1, 0, 0,
nuclear@0: 					0, 1, 0,
nuclear@0: 					x, y, 1 );
nuclear@0: 	*this *= tmp;	
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix3x3::Rotate(float angle) {
nuclear@0: 	Matrix3x3 tmp(	fcos(angle), fsin(angle),	0,
nuclear@0: 					-fsin(angle), fcos(angle),	0,
nuclear@0: 						0,				0,		1 );
nuclear@0: 	*this *= tmp;
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix3x3::Scale(float x, float y) {
nuclear@0: 	Matrix3x3 tmp(	x, 0, 0,
nuclear@0: 					0, y, 0,
nuclear@0: 					0, 0, 1);
nuclear@0: 
nuclear@0: 	*this *= tmp;
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix3x3::SetTranslation(float x, float y) {
nuclear@0: 	Matrix3x3(	1, 0, 0,
nuclear@0: 				0, 1, 0,
nuclear@0: 				x, y, 1 );
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix3x3::SetRotation(float angle) {
nuclear@0: 	Matrix3x3(	fcos(angle),	fsin(angle),	0,
nuclear@0: 				-fsin(angle),	fcos(angle),	0,
nuclear@0: 					0,				0,			1 );
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix3x3::SetScaling(float x, float y) {
nuclear@0: 	Matrix3x3(	x, 0, 0,
nuclear@0: 				0, y, 0,
nuclear@0: 				0, 0, 1 );
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix3x3::SetColumnVector(const Vector3 &vec, int columnindex) {
nuclear@0: 	m[columnindex][0] = vec.x;
nuclear@0: 	m[columnindex][1] = vec.y;
nuclear@0: 	m[columnindex][2] = vec.z;
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix3x3::SetRowVector(const Vector3 &vec, int rowindex) {
nuclear@0: 	m[0][rowindex] = vec.x;
nuclear@0: 	m[1][rowindex] = vec.y;
nuclear@0: 	m[2][rowindex] = vec.z;
nuclear@0: }
nuclear@0: 
nuclear@0: Vector3 Matrix3x3::GetColumnVector(int columnindex) const {
nuclear@0: 	return Vector3(m[columnindex][0], m[columnindex][1], m[columnindex][2]);
nuclear@0: }
nuclear@0: 
nuclear@0: Vector3 Matrix3x3::GetRowVector(int rowindex) const {
nuclear@0: 	return Vector3(m[0][rowindex], m[1][rowindex], m[2][rowindex]);
nuclear@0: }
nuclear@0: 
nuclear@0: void Matrix3x3::Transpose() {
nuclear@0: 	Matrix3x3 mat = *this;
nuclear@0: 
nuclear@0: 	for(int i=0; i<3; i++) {
nuclear@0: 		for(int j=0; j<3; j++) {
nuclear@0: 			m[i][j] = mat.m[j][i];
nuclear@0: 		}
nuclear@0: 	}	
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix3x3 Matrix3x3::Transposed() const {
nuclear@0: 	Matrix3x3 mat;
nuclear@0: 
nuclear@0: 	for(int i=0; i<3; i++) {
nuclear@0: 		for(int j=0; j<3; j++) {
nuclear@0: 			mat.m[i][j] = m[j][i];
nuclear@0: 		}
nuclear@0: 	}
nuclear@0: 
nuclear@0: 	return mat;
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: void Matrix3x3::OrthoNormalize() {
nuclear@0: 	Vector3 i, j, k;
nuclear@0: 	i = GetRowVector(0);
nuclear@0: 	j = GetRowVector(1);
nuclear@0: 	k = GetRowVector(2);
nuclear@0: 
nuclear@0: 	i = CrossProduct(j, k);
nuclear@0: 	j = CrossProduct(k, i);
nuclear@0: 	k = CrossProduct(i, j);
nuclear@0: 
nuclear@0: 	SetRowVector(i, 0);
nuclear@0: 	SetRowVector(j, 1);
nuclear@0: 	SetRowVector(k, 2);
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix3x3 Matrix3x3::OrthoNormalized() {
nuclear@0: 	Vector3 i, j, k;
nuclear@0: 	i = GetRowVector(0);
nuclear@0: 	j = GetRowVector(1);
nuclear@0: 	k = GetRowVector(2);
nuclear@0: 
nuclear@0: 	i = CrossProduct(j, k);
nuclear@0: 	j = CrossProduct(k, i);
nuclear@0: 	k = CrossProduct(i, j);
nuclear@0: 
nuclear@0: 	Matrix3x3 newmat;
nuclear@0: 	newmat.SetRowVector(i, 0);
nuclear@0: 	newmat.SetRowVector(j, 1);
nuclear@0: 	newmat.SetRowVector(k, 2);
nuclear@0: 
nuclear@0: 	return newmat;
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: 
nuclear@0: // ----------- Ray implementation --------------
nuclear@0: Ray::Ray() {
nuclear@0: 	Origin = Vector3(0.0f, 0.0f, 0.0f);
nuclear@0: 	Direction = Vector3(0.0f, 0.0f, 1.0f);
nuclear@0: 	Energy = 1.0f;
nuclear@0: 	CurrentIOR = 1.0f;
nuclear@0: }
nuclear@0: 
nuclear@0: Ray::Ray(const Vector3 &origin, const Vector3 &direction) {
nuclear@0: 	Origin = origin;
nuclear@0: 	Direction = direction;
nuclear@0: }
nuclear@0: 
nuclear@0: // ----------- Base implementation --------------
nuclear@0: Base::Base() {
nuclear@0: 	i = Vector3(1, 0, 0);
nuclear@0: 	j = Vector3(0, 1, 0);
nuclear@0: 	k = Vector3(0, 0, 1);
nuclear@0: }
nuclear@0: 
nuclear@0: Base::Base(const Vector3 &i, const Vector3 &j, const Vector3 &k) {
nuclear@0: 	this->i = i;
nuclear@0: 	this->j = j;
nuclear@0: 	this->k = k;
nuclear@0: }
nuclear@0: 
nuclear@0: Base::Base(const Vector3 &dir, bool LeftHanded) {
nuclear@0: 	k = dir;
nuclear@0: 	j = VECTOR3_J;
nuclear@0: 	i = CrossProduct(j, k);
nuclear@0: 	j = CrossProduct(k, i);
nuclear@0: }
nuclear@0: 
nuclear@0: 
nuclear@0: void Base::Rotate(float x, float y, float z) {
nuclear@0: 	Matrix4x4 RotMat;
nuclear@0: 	RotMat.SetRotation(x, y, z);
nuclear@0: 	i.Transform(RotMat);
nuclear@0: 	j.Transform(RotMat);
nuclear@0: 	k.Transform(RotMat);
nuclear@0: }
nuclear@0: 
nuclear@0: void Base::Rotate(const Vector3 &axis, float angle) {
nuclear@0: 	Quaternion q;
nuclear@0: 	q.SetRotation(axis, angle);
nuclear@0: 	i.Transform(q);
nuclear@0: 	j.Transform(q);
nuclear@0: 	k.Transform(q);
nuclear@0: }
nuclear@0: 
nuclear@0: void Base::Rotate(const Matrix4x4 &mat) {
nuclear@0: 	i.Transform(mat);
nuclear@0: 	j.Transform(mat);
nuclear@0: 	k.Transform(mat);
nuclear@0: }
nuclear@0: 
nuclear@0: void Base::Rotate(const Quaternion &quat) {
nuclear@0: 	i.Transform(quat);
nuclear@0: 	j.Transform(quat);
nuclear@0: 	k.Transform(quat);
nuclear@0: }
nuclear@0: 
nuclear@0: Matrix3x3 Base::CreateRotationMatrix() const {
nuclear@0: 	return Matrix3x3(	i.x, i.y, i.z,
nuclear@0: 						j.x, j.y, j.z,
nuclear@0: 						k.x, k.y, k.z);
nuclear@0: }