BlitzNext/blitz3d/geom.h

#ifndef GEOM_H
#define GEOM_H

#include <math.h>

class Vector;
class Line;
class Plane;
class Matrix;
class Transform;

static float PI = 3.14159265359f;		//180 degrees
static float TWOPI = PI*2.0f;			//360 degrees
static float HALFPI = PI*.5f;			//90  degrees
static float QUARTERPI = PI*.25f;		//45  degrees
//static float FLT_EPSILON=.000001f;		//small value
//static float INFINITY=10000000.0f;	//big value

class Vector {
public:
	float x, y, z;

	Vector() :x(0), y(0), z(0) {
	}
	Vector(float x, float y, float z) :x(x), y(y), z(z) {
	}
	operator float*() {
		return &x;
	}
	operator const float *() {
		return &x;
	}
	float &operator[](int n) {
		return (&x)[n];
	}
	float operator[](int n)const {
		return (&x)[n];
	}
	Vector operator-()const {
		return Vector(-x, -y, -z);
	}
	Vector operator*(float scale)const {
		return Vector(x*scale, y*scale, z*scale);
	}
	Vector operator*(const Vector &q)const {
		return Vector(x*q.x, y*q.y, z*q.z);
	}
	Vector operator/(float scale)const {
		return Vector(x / scale, y / scale, z / scale);
	}
	Vector operator/(const Vector &q)const {
		return Vector(x / q.x, y / q.y, z / q.z);
	}
	Vector operator+(const Vector &q)const {
		return Vector(x + q.x, y + q.y, z + q.z);
	}
	Vector operator-(const Vector &q)const {
		return Vector(x - q.x, y - q.y, z - q.z);
	}
	Vector &operator*=(float scale) {
		x *= scale; y *= scale; z *= scale; return *this;
	}
	Vector &operator*=(const Vector &q) {
		x *= q.x; y *= q.y; z *= q.z; return *this;
	}
	Vector &operator/=(float scale) {
		x /= scale; y /= scale; z /= scale; return *this;
	}
	Vector &operator/=(const Vector &q) {
		x /= q.x; y /= q.y; z /= q.z; return *this;
	}
	Vector &operator+=(const Vector &q) {
		x += q.x; y += q.y; z += q.z; return *this;
	}
	Vector &operator-=(const Vector &q) {
		x -= q.x; y -= q.y; z -= q.z; return *this;
	}
	bool operator<(const Vector &q)const {
		if (fabs(x - q.x) > FLT_EPSILON) return x < q.x ? true : false;
		if (fabs(y - q.y) > FLT_EPSILON) return y < q.y ? true : false;
		return fabs(z - q.z) > FLT_EPSILON && z < q.z;
	}
	bool operator==(const Vector &q)const {
		return fabs(x - q.x) <= FLT_EPSILON && fabs(y - q.y) <= FLT_EPSILON && fabs(z - q.z) <= FLT_EPSILON;
	}
	bool operator!=(const Vector &q)const {
		return fabs(x - q.x) > FLT_EPSILON || fabs(y - q.y) > FLT_EPSILON || fabs(z - q.z) > FLT_EPSILON;
	}
	float dot(const Vector &q)const {
		return x*q.x + y*q.y + z*q.z;
	}
	Vector cross(const Vector &q)const {
		return Vector(y*q.z - z*q.y, z*q.x - x*q.z, x*q.y - y*q.x);
	}
	float length()const {
		return sqrtf(x*x + y*y + z*z);
	}
	float distance(const Vector &q)const {
		float dx = x - q.x, dy = y - q.y, dz = z - q.z; return sqrtf(dx*dx + dy*dy + dz*dz);
	}
	Vector normalized()const {
		float l = length(); return Vector(x / l, y / l, z / l);
	}
	void normalize() {
		float l = length(); x /= l; y /= l; z /= l;
	}
	float yaw()const {
		return -atan2f(x, z);
	}
	float pitch()const {
		return -atan2f(y, sqrtf(x*x + z*z));
	}
	void clear() {
		x = y = z = 0;
	}
};

class Line {
public:
	Vector o, d;
	Line() {
	}
	Line(const Vector &o, const Vector &d) :o(o), d(d) {
	}
	Line operator+(const Vector &q)const {
		return Line(o + q, d);
	}
	Line operator-(const Vector &q)const {
		return Line(o - q, d);
	}
	Vector operator*(float q)const {
		return o + d*q;
	}
	Vector nearest(const Vector &q)const {
		return o + d*(d.dot(q - o) / d.dot(d));
	}
};

class Plane {
public:
	Vector n;
	float d;

	Plane() :d(0) {
	}
	//normal/offset form
	Plane(const Vector &n, float d) :n(n), d(d) {
	}
	//point/normal form
	Plane(const Vector &p, const Vector &n) :n(n), d(-n.dot(p)) {
	}
	//create plane from tri
	Plane(const Vector &v0, const Vector &v1, const Vector &v2) {
		n = (v1 - v0).cross(v2 - v0).normalized(); d = -n.dot(v0);
	}
	Plane operator-()const {
		return Plane(-n, -d);
	}
	float t_intersect(const Line &q)const {
		return -distance(q.o) / n.dot(q.d);
	}
	Vector intersect(const Line &q)const {
		return q*t_intersect(q);
	}
	Line intersect(const Plane &q)const {
		Vector lv = n.cross(q.n).normalized();
		return Line(q.intersect(Line(nearest(n*-d), n.cross(lv))), lv);
	}
	Vector nearest(const Vector &q)const {
		return q - n*distance(q);
	}
	void negate() {
		n = -n; d = -d;
	}
	float distance(const Vector &q)const {
		return n.dot(q) + d;
	}
};

struct Quat {
	float w;
	Vector v;
	Quat() :w(1) {
	}
	Quat(float w, const Vector &v) :w(w), v(v) {
	}
	Quat operator-()const {
		return Quat(w, -v);
	}
	Quat operator+(const Quat &q)const {
		return Quat(w + q.w, v + q.v);
	}
	Quat operator-(const Quat &q)const {
		return Quat(w - q.w, v - q.v);
	}
	Quat operator*(const Quat &q)const {
		return Quat(w*q.w - v.dot(q.v), q.v.cross(v) + q.v*w + v*q.w);
	}
	Vector operator*(const Vector &q)const {
		return (*this * Quat(0, q) * -*this).v;
	}
	Quat operator*(float q)const {
		return Quat(w*q, v*q);
	}
	Quat operator/(float q)const {
		return Quat(w / q, v / q);
	}
	float dot(const Quat &q)const {
		return v.x*q.v.x + v.y*q.v.y + v.z*q.v.z + w*q.w;
	}
	float length()const {
		return sqrtf(w*w + v.x*v.x + v.y*v.y + v.z*v.z);
	}
	void normalize() {
		*this = *this / length();
	}
	Quat normalized()const {
		return *this / length();
	}
	Quat slerpTo(const Quat &q, float a)const {
		Quat t = q;
		float d = dot(q), b = 1 - a;
		if (d < 0) { t.w = -t.w; t.v = -t.v; d = -d; }
		if (d < 1 - FLT_EPSILON) {
			float om = acosf(d);
			float si = sinf(om);
			a = sinf(a*om) / si;
			b = sinf(b*om) / si;
		}
		return *this*b + t*a;
	}
	Vector i()const {
		float xz = v.x*v.z, wy = w*v.y;
		float xy = v.x*v.y, wz = w*v.z;
		float yy = v.y*v.y, zz = v.z*v.z;
		return Vector(1 - 2 * (yy + zz), 2 * (xy - wz), 2 * (xz + wy));
	}
	Vector j()const {
		float yz = v.y*v.z, wx = w*v.x;
		float xy = v.x*v.y, wz = w*v.z;
		float xx = v.x*v.x, zz = v.z*v.z;
		return Vector(2 * (xy + wz), 1 - 2 * (xx + zz), 2 * (yz - wx));
	}
	Vector k()const {
		float xz = v.x*v.z, wy = w*v.y;
		float yz = v.y*v.z, wx = w*v.x;
		float xx = v.x*v.x, yy = v.y*v.y;
		return Vector(2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx + yy));
	}
};

class Matrix {
	static Matrix tmps[64];
	static Matrix &alloc_tmp() { static int tmp = 0; return tmps[tmp++ & 63]; }
	friend class Transform;
public:
	Vector i, j, k;

	Matrix() :i(Vector(1, 0, 0)), j(Vector(0, 1, 0)), k(Vector(0, 0, 1)) {
	}
	Matrix(const Vector &i, const Vector &j, const Vector &k) :i(i), j(j), k(k) {
	}
	Matrix(const Quat &q) {
		float xx = q.v.x*q.v.x, yy = q.v.y*q.v.y, zz = q.v.z*q.v.z;
		float xy = q.v.x*q.v.y, xz = q.v.x*q.v.z, yz = q.v.y*q.v.z;
		float wx = q.w*q.v.x, wy = q.w*q.v.y, wz = q.w*q.v.z;
		i = Vector(1 - 2 * (yy + zz), 2 * (xy - wz), 2 * (xz + wy)),
			j = Vector(2 * (xy + wz), 1 - 2 * (xx + zz), 2 * (yz - wx)),
			k = Vector(2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx + yy));
	}
	Matrix(float angle, const Vector &axis) {
		const Vector &u = axis;
		float c = cosf(angle), s = sinf(angle);
		float x2 = axis.x*axis.x, y2 = axis.y*axis.y, z2 = axis.z*axis.z;
		i = Vector(x2 + c*(1 - x2), u.x*u.y*(1 - c) - u.z*s, u.z*u.x*(1 - c) + u.y*s);
		j = Vector(u.x*u.y*(1 - c) + u.z*s, y2 + c*(1 - y2), u.y*u.z*(1 - c) - u.x*s);
		k = Vector(u.z*u.x*(1 - c) - u.y*s, u.y*u.z*(1 - c) + u.x*s, z2 + c*(1 - z2));
	}
	Vector &operator[](int n) {
		return (&i)[n];
	}
	const Vector &operator[](int n)const {
		return (&i)[n];
	}
	Matrix &operator~()const {
		Matrix &m = alloc_tmp();
		m.i.x = i.x; m.i.y = j.x; m.i.z = k.x;
		m.j.x = i.y; m.j.y = j.y; m.j.z = k.y;
		m.k.x = i.z; m.k.y = j.z; m.k.z = k.z;
		return m;
	}
	float determinant()const {
		return i.x*(j.y*k.z - j.z*k.y) - i.y*(j.x*k.z - j.z*k.x) + i.z*(j.x*k.y - j.y*k.x);
	}
	Matrix &operator-()const {
		Matrix &m = alloc_tmp();
		float t = 1.0f / determinant();
		m.i.x = t*(j.y*k.z - j.z*k.y); m.i.y = -t*(i.y*k.z - i.z*k.y); m.i.z = t*(i.y*j.z - i.z*j.y);
		m.j.x = -t*(j.x*k.z - j.z*k.x); m.j.y = t*(i.x*k.z - i.z*k.x); m.j.z = -t*(i.x*j.z - i.z*j.x);
		m.k.x = t*(j.x*k.y - j.y*k.x); m.k.y = -t*(i.x*k.y - i.y*k.x); m.k.z = t*(i.x*j.y - i.y*j.x);
		return m;
	}
	Matrix &cofactor()const {
		Matrix &m = alloc_tmp();
		m.i.x = (j.y*k.z - j.z*k.y); m.i.y = -(j.x*k.z - j.z*k.x); m.i.z = (j.x*k.y - j.y*k.x);
		m.j.x = -(i.y*k.z - i.z*k.y); m.j.y = (i.x*k.z - i.z*k.x); m.j.z = -(i.x*k.y - i.y*k.x);
		m.k.x = (i.y*j.z - i.z*j.y); m.k.y = -(i.x*j.z - i.z*j.x); m.k.z = (i.x*j.y - i.y*j.x);
		return m;
	}
	bool operator==(const Matrix &q)const {
		return i == q.i && j == q.j && k == q.k;
	}
	bool operator!=(const Matrix &q)const {
		return i != q.i || j != q.j || k != q.k;
	}
	Vector operator*(const Vector &q)const {
		return Vector(i.x*q.x + j.x*q.y + k.x*q.z, i.y*q.x + j.y*q.y + k.y*q.z, i.z*q.x + j.z*q.y + k.z*q.z);
	}
	Matrix &operator*(const Matrix &q)const {
		Matrix &m = alloc_tmp();
		m.i.x = i.x*q.i.x + j.x*q.i.y + k.x*q.i.z; m.i.y = i.y*q.i.x + j.y*q.i.y + k.y*q.i.z; m.i.z = i.z*q.i.x + j.z*q.i.y + k.z*q.i.z;
		m.j.x = i.x*q.j.x + j.x*q.j.y + k.x*q.j.z; m.j.y = i.y*q.j.x + j.y*q.j.y + k.y*q.j.z; m.j.z = i.z*q.j.x + j.z*q.j.y + k.z*q.j.z;
		m.k.x = i.x*q.k.x + j.x*q.k.y + k.x*q.k.z; m.k.y = i.y*q.k.x + j.y*q.k.y + k.y*q.k.z; m.k.z = i.z*q.k.x + j.z*q.k.y + k.z*q.k.z;
		return m;
	}
	void orthogonalize() {
		k.normalize();
		i = j.cross(k).normalized();
		j = k.cross(i);
	}
	Matrix &orthogonalized()const {
		Matrix &m = alloc_tmp();
		m = *this; m.orthogonalize();
		return m;
	}
};

class Box {
public:
	Vector a, b;
	Box() :a(Vector(INFINITY, INFINITY, INFINITY)), b(Vector(-INFINITY, -INFINITY, -INFINITY)) {
	}
	Box(const Vector &q) :a(q), b(q) {
	}
	Box(const Vector &a, const Vector &b) :a(a), b(b) {
	}
	Box(const Line &l) :a(l.o), b(l.o) {
		update(l.o + l.d);
	}
	void clear() {
		a.x = a.y = a.z = INFINITY;
		b.x = b.y = b.z = -INFINITY;
	}
	bool empty()const {
		return b.x < a.x || b.y < a.y || b.z < a.z;
	}
	Vector centre()const {
		return Vector((a.x + b.x)*.5f, (a.y + b.y)*.5f, (a.z + b.z)*.5f);
	}
	Vector corner(int n)const {
		return Vector(((n & 1) ? b : a).x, ((n & 2) ? b : a).y, ((n & 4) ? b : a).z);
	}
	void update(const Vector &q) {
		if (q.x < a.x) a.x = q.x; if (q.y < a.y) a.y = q.y; if (q.z < a.z) a.z = q.z;
		if (q.x > b.x) b.x = q.x; if (q.y > b.y) b.y = q.y; if (q.z > b.z) b.z = q.z;
	}
	void update(const Box &q) {
		if (q.a.x < a.x) a.x = q.a.x; if (q.a.y < a.y) a.y = q.a.y; if (q.a.z < a.z) a.z = q.a.z;
		if (q.b.x > b.x) b.x = q.b.x; if (q.b.y > b.y) b.y = q.b.y; if (q.b.z > b.z) b.z = q.b.z;
	}
	bool overlaps(const Box &q)const {
		return
			(b.x < q.b.x ? b.x : q.b.x) >= (a.x > q.a.x ? a.x : q.a.x) &&
			(b.y < q.b.y ? b.y : q.b.y) >= (a.y > q.a.y ? a.y : q.a.y) &&
			(b.z < q.b.z ? b.z : q.b.z) >= (a.z > q.a.z ? a.z : q.a.z);
	}
	void expand(float n) {
		a.x -= n; a.y -= n; a.z -= n; b.x += n; b.y += n; b.z += n;
	}
	float width()const {
		return b.x - a.x;
	}
	float height()const {
		return b.y - a.y;
	}
	float depth()const {
		return b.z - a.z;
	}
	bool contains(const Vector &q) {
		return q.x >= a.x && q.x <= b.x && q.y >= a.y && q.y <= b.y && q.z >= a.z && q.z <= b.z;
	}
};

class Transform {
	static Transform tmps[64];
	static Transform &alloc_tmp() { static int tmp = 0; return tmps[tmp++ & 63]; }
public:
	Matrix m;
	Vector v;

	Transform() {
	}
	Transform(const Matrix &m) :m(m) {
	}
	Transform(const Vector &v) :v(v) {
	}
	Transform(const Matrix &m, const Vector &v) :m(m), v(v) {
	}
	Transform &operator-()const {
		Transform &t = alloc_tmp();
		t.m = -m; t.v = t.m*-v;
		return t;
	}
	Transform &operator~()const {
		Transform &t = alloc_tmp();
		t.m = ~m; t.v = t.m*-v;
		return t;
	}
	Vector operator*(const Vector &q)const {
		return m*q + v;
	}
	Line operator*(const Line &q)const {
		Vector t = (*this)*q.o;
		return Line(t, (*this)*(q.o + q.d) - t);
	}
	Box operator*(const Box &q)const {
		Box t((*this*q.corner(0)));
		for (int k = 1; k < 8; ++k) t.update(*this*q.corner(k));
		return t;
	}
	Transform &operator*(const Transform &q)const {
		Transform &t = alloc_tmp();
		t.m = m*q.m; t.v = m*q.v + v;
		return t;
	}
	bool operator==(const Transform &q)const {
		return m == q.m && v == q.v;
	}
	bool operator!=(const Transform &q)const {
		return !operator==(q);
	}
};

inline float transformRadius(float r, const Matrix &t) {
	static const float sq_3 = sqrtf(1.0f / 3.0f);
	return (t * Vector(sq_3, sq_3, sq_3)).length()*r;
}

inline Matrix pitchMatrix(float q) {
	return Matrix(Vector(1, 0, 0), Vector(0, cosf(q), sinf(q)), Vector(0, -sinf(q), cosf(q)));
}

inline Matrix yawMatrix(float q) {
	return Matrix(Vector(cosf(q), 0, sinf(q)), Vector(0, 1, 0), Vector(-sinf(q), 0, cosf(q)));
}

inline Matrix rollMatrix(float q) {
	return Matrix(Vector(cosf(q), sinf(q), 0), Vector(-sinf(q), cosf(q), 0), Vector(0, 0, 1));
}

inline float matrixPitch(const Matrix &m) {
	return m.k.pitch();
	//	return asinf( -m.k.y );
}

inline float matrixYaw(const Matrix &m) {
	return m.k.yaw();
	//return atan2f( -m.k.x,m.k.z );
}

inline float matrixRoll(const Matrix &m) {
	return atan2f(m.i.y, m.j.y);
	//Matrix t=pitchMatrix( -matrixPitch(m) )*yawMatrix( -matrixYaw(m) )*m;
	//return atan2f( t.i.y,t.i.x );
}

inline Matrix scaleMatrix(float x, float y, float z) {
	return Matrix(Vector(x, 0, 0), Vector(0, y, 0), Vector(0, 0, z));
}

inline Matrix scaleMatrix(const Vector &scale) {
	return Matrix(Vector(scale.x, 0, 0), Vector(0, scale.y, 0), Vector(0, 0, scale.z));
}

inline Quat pitchQuat(float p) {
	return Quat(cosf(p / -2), Vector(sinf(p / -2), 0, 0));
}

inline Quat yawQuat(float y) {
	return Quat(cosf(y / 2), Vector(0, sinf(y / 2), 0));
}

inline Quat rollQuat(float r) {
	return Quat(cosf(r / -2), Vector(0, 0, sinf(r / -2)));
}

//inline Quat rotationQuat( float p,float y,float r ){
//	return yawQuat(y)*pitchQuat(p)*rollQuat(r);
//}

Quat rotationQuat(float p, float y, float r);

inline Matrix rotationMatrix(float p, float y, float r) {
	return yawMatrix(y)*pitchMatrix(p)*rollMatrix(r);
}

inline Matrix rotationMatrix(const Vector &rot) {
	return yawMatrix(rot.y)*pitchMatrix(rot.x)*rollMatrix(rot.z);
}

inline float quatPitch(const Quat &q) {
	return q.k().pitch();
}

inline float quatYaw(const Quat &q) {
	return q.k().yaw();
}

inline float quatRoll(const Quat &q) {
	//	Vector i=q.i(),j=q.j();
	//	return atan2f( i.y,j.y );
	return matrixRoll(q);
}

inline Quat matrixQuat(const Matrix &p) {
	Matrix m = p;
	m.orthogonalize();
	float t = m.i.x + m.j.y + m.k.z, w, x, y, z;
	if (t > FLT_EPSILON) {
		t = sqrtf(t + 1) * 2;
		x = (m.k.y - m.j.z) / t;
		y = (m.i.z - m.k.x) / t;
		z = (m.j.x - m.i.y) / t;
		w = t / 4;
	} else if (m.i.x > m.j.y && m.i.x > m.k.z) {
		t = sqrtf(m.i.x - m.j.y - m.k.z + 1) * 2;
		x = t / 4;
		y = (m.j.x + m.i.y) / t;
		z = (m.i.z + m.k.x) / t;
		w = (m.k.y - m.j.z) / t;
	} else if (m.j.y > m.k.z) {
		t = sqrtf(m.j.y - m.k.z - m.i.x + 1) * 2;
		x = (m.j.x + m.i.y) / t;
		y = t / 4;
		z = (m.k.y + m.j.z) / t;
		w = (m.i.z - m.k.x) / t;
	} else {
		t = sqrtf(m.k.z - m.j.y - m.i.x + 1) * 2;
		x = (m.i.z + m.k.x) / t;
		y = (m.k.y + m.j.z) / t;
		z = t / 4;
		w = (m.j.x - m.i.y) / t;
	}
	return Quat(w, Vector(x, y, z));
}

#endif