BlitzNext/Runtime/blitz3d/geom.hpp

#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

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