bullet3/examples/Experiments/ImplicitCloth/stan/vecmath.cpp

//
//
//  Typical 3d vector math code.
//  By S Melax 1998-2008
//
//
//

#include "vecmath.h"
#include <memory.h>  // for memcpy
#include <float.h>

float squared(float a) { return a * a; }
float clamp(float a, const float minval, const float maxval) { return Min(maxval, Max(minval, a)); }
int clamp(int a, const int minval, const int maxval) { return Min(maxval, Max(minval, a)); }

float Round(float a, float precision)
{
	return floorf(0.5f + a / precision) * precision;
}

float Interpolate(const float &f0, const float &f1, float alpha)
{
	return f0 * (1 - alpha) + f1 * alpha;
}

int argmin(const float a[], int n)
{
	int r = 0;
	for (int i = 1; i < n; i++)
	{
		if (a[i] < a[r])
		{
			r = i;
		}
	}
	return r;
}

int argmax(const float a[], int n)
{
	int r = 0;
	for (int i = 1; i < n; i++)
	{
		if (a[i] > a[r])
		{
			r = i;
		}
	}
	return r;
}

//------------ float3 (3D) --------------

float3 vabs(const float3 &v)
{
	return float3(fabsf(v.x), fabsf(v.y), fabsf(v.z));
}

float3 safenormalize(const float3 &v)
{
	if (magnitude(v) <= 0.0f)
	{
		return float3(1, 0, 0);
	}
	return normalize(v);
}

float3 Round(const float3 &a, float precision)
{
	return float3(Round(a.x, precision), Round(a.y, precision), Round(a.z, precision));
}

float3 Interpolate(const float3 &v0, const float3 &v1, float alpha)
{
	return v0 * (1 - alpha) + v1 * alpha;
}

float3 Orth(const float3 &v)
{
	float3 absv = vabs(v);
	float3 u(1, 1, 1);
	u[argmax(&absv[0], 3)] = 0.0f;
	return normalize(cross(u, v));
}

void BoxLimits(const float3 *verts, int verts_count, float3 &bmin, float3 &bmax)
{
	bmin = float3(FLT_MAX, FLT_MAX, FLT_MAX);
	bmax = float3(-FLT_MAX, -FLT_MAX, -FLT_MAX);
	for (int i = 0; i < verts_count; i++)
	{
		bmin = VectorMin(bmin, verts[i]);
		bmax = VectorMax(bmax, verts[i]);
	}
}
void BoxLimits(const float4 *verts, int verts_count, float3 &bmin, float3 &bmax)
{
	bmin = float3(FLT_MAX, FLT_MAX, FLT_MAX);
	bmax = float3(-FLT_MAX, -FLT_MAX, -FLT_MAX);
	for (int i = 0; i < verts_count; i++)
	{
		bmin = VectorMin(bmin, verts[i].xyz());
		bmax = VectorMax(bmax, verts[i].xyz());
	}
}
int overlap(const float3 &bmina, const float3 &bmaxa, const float3 &bminb, const float3 &bmaxb)
{
	for (int j = 0; j < 3; j++)
	{
		if (bmina[j] > bmaxb[j]) return 0;
		if (bminb[j] > bmaxa[j]) return 0;
	}
	return 1;
}

// the statement v1*v2 is ambiguous since there are 3 types
// of vector multiplication
//  - componantwise (for example combining colors)
//  - dot product
//  - cross product
// Therefore we never declare/implement this function.
// So we will never see:  float3 operator*(float3 a,float3 b)

//------------ float3x3 ---------------
float Determinant(const float3x3 &m)
{
	return m.x.x * m.y.y * m.z.z + m.y.x * m.z.y * m.x.z + m.z.x * m.x.y * m.y.z - m.x.x * m.z.y * m.y.z - m.y.x * m.x.y * m.z.z - m.z.x * m.y.y * m.x.z;
}

float3x3 Inverse(const float3x3 &a)
{
	float3x3 b;
	float d = Determinant(a);
	assert(d != 0);
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			int i1 = (i + 1) % 3;
			int i2 = (i + 2) % 3;
			int j1 = (j + 1) % 3;
			int j2 = (j + 2) % 3;
			// reverse indexs i&j to take transpose
			b[j][i] = (a[i1][j1] * a[i2][j2] - a[i1][j2] * a[i2][j1]) / d;
		}
	}
	// Matrix check=a*b; // Matrix 'check' should be the identity (or close to it)
	return b;
}

float3x3 Transpose(const float3x3 &m)
{
	return float3x3(float3(m.x.x, m.y.x, m.z.x),
					float3(m.x.y, m.y.y, m.z.y),
					float3(m.x.z, m.y.z, m.z.z));
}

float3 operator*(const float3 &v, const float3x3 &m)
{
	return float3((m.x.x * v.x + m.y.x * v.y + m.z.x * v.z),
				  (m.x.y * v.x + m.y.y * v.y + m.z.y * v.z),
				  (m.x.z * v.x + m.y.z * v.y + m.z.z * v.z));
}
float3 operator*(const float3x3 &m, const float3 &v)
{
	return float3(dot(m.x, v), dot(m.y, v), dot(m.z, v));
}

float3x3 operator*(const float3x3 &a, const float3x3 &b)
{
	return float3x3(a.x * b, a.y * b, a.z * b);
}

float3x3 operator*(const float3x3 &a, const float &s)
{
	return float3x3(a.x * s, a.y * s, a.z * s);
}
float3x3 operator/(const float3x3 &a, const float &s)
{
	float t = 1 / s;
	return float3x3(a.x * t, a.y * t, a.z * t);
}
float3x3 operator+(const float3x3 &a, const float3x3 &b)
{
	return float3x3(a.x + b.x, a.y + b.y, a.z + b.z);
}
float3x3 operator-(const float3x3 &a, const float3x3 &b)
{
	return float3x3(a.x - b.x, a.y - b.y, a.z - b.z);
}
float3x3 &operator+=(float3x3 &a, const float3x3 &b)
{
	a.x += b.x;
	a.y += b.y;
	a.z += b.z;
	return a;
}
float3x3 &operator-=(float3x3 &a, const float3x3 &b)
{
	a.x -= b.x;
	a.y -= b.y;
	a.z -= b.z;
	return a;
}
float3x3 &operator*=(float3x3 &a, const float &s)
{
	a.x *= s;
	a.y *= s;
	a.z *= s;
	return a;
}

float3x3 outerprod(const float3 &a, const float3 &b)
{
	return float3x3(a.x * b, a.y * b, a.z * b);  // a is a column vector b is a row vector
}

//--------------- 4D ----------------

float4 operator*(const float4 &v, const float4x4 &m)
{
	return v.x * m.x + v.y * m.y + v.z * m.z + v.w * m.w;  // yes this actually works
}

//  Dont implement m*v for now, since that might confuse us
//  All our transforms are based on multiplying the "row" vector on the left
//float4   operator*(const float4x4& m , const float4&   v )
//{
//	return float4(dot(v,m.x),dot(v,m.y),dot(v,m.z),dot(v,m.w));
//}

float4x4 operator*(const float4x4 &a, const float4x4 &b)
{
	return float4x4(a.x * b, a.y * b, a.z * b, a.w * b);
}

float4x4 MatrixTranspose(const float4x4 &m)
{
	return float4x4(
		m.x.x, m.y.x, m.z.x, m.w.x,
		m.x.y, m.y.y, m.z.y, m.w.y,
		m.x.z, m.y.z, m.z.z, m.w.z,
		m.x.w, m.y.w, m.z.w, m.w.w);
}

float4x4 MatrixRigidInverse(const float4x4 &m)
{
	float4x4 trans_inverse = MatrixTranslation(-m.w.xyz());
	float4x4 rot = m;
	rot.w = float4(0, 0, 0, 1);
	return trans_inverse * MatrixTranspose(rot);
}

float4x4 MatrixPerspectiveFov(float fovy, float aspect, float zn, float zf)
{
	float h = 1.0f / tanf(fovy / 2.0f);  // view space height
	float w = h / aspect;                // view space width
	return float4x4(
		w, 0, 0, 0,
		0, h, 0, 0,
		0, 0, zf / (zn - zf), -1,
		0, 0, zn * zf / (zn - zf), 0);
}

float4x4 MatrixLookAt(const float3 &eye, const float3 &at, const float3 &up)
{
	float4x4 m;
	m.w.w = 1.0f;
	m.w.xyz() = eye;
	m.z.xyz() = normalize(eye - at);
	m.x.xyz() = normalize(cross(up, m.z.xyz()));
	m.y.xyz() = cross(m.z.xyz(), m.x.xyz());
	return MatrixRigidInverse(m);
}

float4x4 MatrixTranslation(const float3 &t)
{
	return float4x4(
		1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0,
		t.x, t.y, t.z, 1);
}

float4x4 MatrixRotationZ(const float angle_radians)
{
	float s = sinf(angle_radians);
	float c = cosf(angle_radians);
	return float4x4(
		c, s, 0, 0,
		-s, c, 0, 0,
		0, 0, 1, 0,
		0, 0, 0, 1);
}

int operator==(const float4x4 &a, const float4x4 &b)
{
	return (a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w);
}

float4x4 Inverse(const float4x4 &m)
{
	float4x4 d;
	float *dst = &d.x.x;
	float tmp[12]; /* temp array for pairs */
	float src[16]; /* array of transpose source matrix */
	float det;     /* determinant */
	/* transpose matrix */
	for (int i = 0; i < 4; i++)
	{
		src[i] = m(i, 0);
		src[i + 4] = m(i, 1);
		src[i + 8] = m(i, 2);
		src[i + 12] = m(i, 3);
	}
	/* calculate pairs for first 8 elements (cofactors) */
	tmp[0] = src[10] * src[15];
	tmp[1] = src[11] * src[14];
	tmp[2] = src[9] * src[15];
	tmp[3] = src[11] * src[13];
	tmp[4] = src[9] * src[14];
	tmp[5] = src[10] * src[13];
	tmp[6] = src[8] * src[15];
	tmp[7] = src[11] * src[12];
	tmp[8] = src[8] * src[14];
	tmp[9] = src[10] * src[12];
	tmp[10] = src[8] * src[13];
	tmp[11] = src[9] * src[12];
	/* calculate first 8 elements (cofactors) */
	dst[0] = tmp[0] * src[5] + tmp[3] * src[6] + tmp[4] * src[7];
	dst[0] -= tmp[1] * src[5] + tmp[2] * src[6] + tmp[5] * src[7];
	dst[1] = tmp[1] * src[4] + tmp[6] * src[6] + tmp[9] * src[7];
	dst[1] -= tmp[0] * src[4] + tmp[7] * src[6] + tmp[8] * src[7];
	dst[2] = tmp[2] * src[4] + tmp[7] * src[5] + tmp[10] * src[7];
	dst[2] -= tmp[3] * src[4] + tmp[6] * src[5] + tmp[11] * src[7];
	dst[3] = tmp[5] * src[4] + tmp[8] * src[5] + tmp[11] * src[6];
	dst[3] -= tmp[4] * src[4] + tmp[9] * src[5] + tmp[10] * src[6];
	dst[4] = tmp[1] * src[1] + tmp[2] * src[2] + tmp[5] * src[3];
	dst[4] -= tmp[0] * src[1] + tmp[3] * src[2] + tmp[4] * src[3];
	dst[5] = tmp[0] * src[0] + tmp[7] * src[2] + tmp[8] * src[3];
	dst[5] -= tmp[1] * src[0] + tmp[6] * src[2] + tmp[9] * src[3];
	dst[6] = tmp[3] * src[0] + tmp[6] * src[1] + tmp[11] * src[3];
	dst[6] -= tmp[2] * src[0] + tmp[7] * src[1] + tmp[10] * src[3];
	dst[7] = tmp[4] * src[0] + tmp[9] * src[1] + tmp[10] * src[2];
	dst[7] -= tmp[5] * src[0] + tmp[8] * src[1] + tmp[11] * src[2];
	/* calculate pairs for second 8 elements (cofactors) */
	tmp[0] = src[2] * src[7];
	tmp[1] = src[3] * src[6];
	tmp[2] = src[1] * src[7];
	tmp[3] = src[3] * src[5];
	tmp[4] = src[1] * src[6];
	tmp[5] = src[2] * src[5];
	tmp[6] = src[0] * src[7];
	tmp[7] = src[3] * src[4];
	tmp[8] = src[0] * src[6];
	tmp[9] = src[2] * src[4];
	tmp[10] = src[0] * src[5];
	tmp[11] = src[1] * src[4];
	/* calculate second 8 elements (cofactors) */
	dst[8] = tmp[0] * src[13] + tmp[3] * src[14] + tmp[4] * src[15];
	dst[8] -= tmp[1] * src[13] + tmp[2] * src[14] + tmp[5] * src[15];
	dst[9] = tmp[1] * src[12] + tmp[6] * src[14] + tmp[9] * src[15];
	dst[9] -= tmp[0] * src[12] + tmp[7] * src[14] + tmp[8] * src[15];
	dst[10] = tmp[2] * src[12] + tmp[7] * src[13] + tmp[10] * src[15];
	dst[10] -= tmp[3] * src[12] + tmp[6] * src[13] + tmp[11] * src[15];
	dst[11] = tmp[5] * src[12] + tmp[8] * src[13] + tmp[11] * src[14];
	dst[11] -= tmp[4] * src[12] + tmp[9] * src[13] + tmp[10] * src[14];
	dst[12] = tmp[2] * src[10] + tmp[5] * src[11] + tmp[1] * src[9];
	dst[12] -= tmp[4] * src[11] + tmp[0] * src[9] + tmp[3] * src[10];
	dst[13] = tmp[8] * src[11] + tmp[0] * src[8] + tmp[7] * src[10];
	dst[13] -= tmp[6] * src[10] + tmp[9] * src[11] + tmp[1] * src[8];
	dst[14] = tmp[6] * src[9] + tmp[11] * src[11] + tmp[3] * src[8];
	dst[14] -= tmp[10] * src[11] + tmp[2] * src[8] + tmp[7] * src[9];
	dst[15] = tmp[10] * src[10] + tmp[4] * src[8] + tmp[9] * src[9];
	dst[15] -= tmp[8] * src[9] + tmp[11] * src[10] + tmp[5] * src[8];
	/* calculate determinant */
	det = src[0] * dst[0] + src[1] * dst[1] + src[2] * dst[2] + src[3] * dst[3];
	/* calculate matrix inverse */
	det = 1 / det;
	for (int j = 0; j < 16; j++)
		dst[j] *= det;
	return d;
}

//--------- Quaternion --------------

template <>
void Quaternion::Normalize()
{
	float m = sqrtf(squared(w) + squared(x) + squared(y) + squared(z));
	if (m < 0.000000001f)
	{
		w = 1.0f;
		x = y = z = 0.0f;
		return;
	}
	(*this) *= (1.0f / m);
}

float3 rotate(const Quaternion &q, const float3 &v)
{
	// The following is equivalent to:
	//return (q.getmatrix() * v);
	float qx2 = q.x * q.x;
	float qy2 = q.y * q.y;
	float qz2 = q.z * q.z;

	float qxqy = q.x * q.y;
	float qxqz = q.x * q.z;
	float qxqw = q.x * q.w;
	float qyqz = q.y * q.z;
	float qyqw = q.y * q.w;
	float qzqw = q.z * q.w;
	return float3(
		(1 - 2 * (qy2 + qz2)) * v.x + (2 * (qxqy - qzqw)) * v.y + (2 * (qxqz + qyqw)) * v.z,
		(2 * (qxqy + qzqw)) * v.x + (1 - 2 * (qx2 + qz2)) * v.y + (2 * (qyqz - qxqw)) * v.z,
		(2 * (qxqz - qyqw)) * v.x + (2 * (qyqz + qxqw)) * v.y + (1 - 2 * (qx2 + qy2)) * v.z);
}

Quaternion slerp(const Quaternion &_a, const Quaternion &b, float interp)
{
	Quaternion a = _a;
	if (dot(a, b) < 0.0)
	{
		a.w = -a.w;
		a.x = -a.x;
		a.y = -a.y;
		a.z = -a.z;
	}
	float d = dot(a, b);
	if (d >= 1.0)
	{
		return a;
	}
	float theta = acosf(d);
	if (theta == 0.0f)
	{
		return (a);
	}
	return a * (sinf(theta - interp * theta) / sinf(theta)) + b * (sinf(interp * theta) / sinf(theta));
}

Quaternion Interpolate(const Quaternion &q0, const Quaternion &q1, float alpha)
{
	return slerp(q0, q1, alpha);
}

Quaternion YawPitchRoll(float yaw, float pitch, float roll)
{
	return QuatFromAxisAngle(float3(0.0f, 0.0f, 1.0f), DegToRad(yaw)) * QuatFromAxisAngle(float3(1.0f, 0.0f, 0.0f), DegToRad(pitch)) * QuatFromAxisAngle(float3(0.0f, 1.0f, 0.0f), DegToRad(roll));
}

float Yaw(const Quaternion &q)
{
	static float3 v;
	v = q.ydir();
	return (v.y == 0.0 && v.x == 0.0) ? 0.0f : RadToDeg(atan2f(-v.x, v.y));
}

float Pitch(const Quaternion &q)
{
	static float3 v;
	v = q.ydir();
	return RadToDeg(atan2f(v.z, sqrtf(squared(v.x) + squared(v.y))));
}

float Roll(const Quaternion &_q)
{
	Quaternion q = _q;
	q = QuatFromAxisAngle(float3(0.0f, 0.0f, 1.0f), -DegToRad(Yaw(q))) * q;
	q = QuatFromAxisAngle(float3(1.0f, 0.0f, 0.0f), -DegToRad(Pitch(q))) * q;
	return RadToDeg(atan2f(-q.xdir().z, q.xdir().x));
}

float Yaw(const float3 &v)
{
	return (v.y == 0.0 && v.x == 0.0) ? 0.0f : RadToDeg(atan2f(-v.x, v.y));
}

float Pitch(const float3 &v)
{
	return RadToDeg(atan2f(v.z, sqrtf(squared(v.x) + squared(v.y))));
}

//--------- utility functions -------------

//        RotationArc()
// Given two vectors v0 and v1 this function
// returns quaternion q where q*v0==v1.
// Routine taken from game programming gems.
Quaternion RotationArc(float3 v0, float3 v1)
{
	static Quaternion q;
	v0 = normalize(v0);  // Comment these two lines out if you know its not needed.
	v1 = normalize(v1);  // If vector is already unit length then why do it again?
	float3 c = cross(v0, v1);
	float d = dot(v0, v1);
	if (d <= -1.0f)
	{
		float3 a = Orth(v0);
		return Quaternion(a.x, a.y, a.z, 0);
	}  // 180 about any orthogonal axis axis
	float s = sqrtf((1 + d) * 2);
	q.x = c.x / s;
	q.y = c.y / s;
	q.z = c.z / s;
	q.w = s / 2.0f;
	return q;
}

float4x4 MatrixFromQuatVec(const Quaternion &q, const float3 &v)
{
	// builds a 4x4 transformation matrix based on orientation q and translation v
	float qx2 = q.x * q.x;
	float qy2 = q.y * q.y;
	float qz2 = q.z * q.z;

	float qxqy = q.x * q.y;
	float qxqz = q.x * q.z;
	float qxqw = q.x * q.w;
	float qyqz = q.y * q.z;
	float qyqw = q.y * q.w;
	float qzqw = q.z * q.w;

	return float4x4(
		1 - 2 * (qy2 + qz2),
		2 * (qxqy + qzqw),
		2 * (qxqz - qyqw),
		0,
		2 * (qxqy - qzqw),
		1 - 2 * (qx2 + qz2),
		2 * (qyqz + qxqw),
		0,
		2 * (qxqz + qyqw),
		2 * (qyqz - qxqw),
		1 - 2 * (qx2 + qy2),
		0,
		v.x,
		v.y,
		v.z,
		1.0f);
}

float3 PlaneLineIntersection(const float3 &normal, const float dist, const float3 &p0, const float3 &p1)
{
	// returns the point where the line p0-p1 intersects the plane n&d
	float3 dif;
	dif = p1 - p0;
	float dn = dot(normal, dif);
	float t = -(dist + dot(normal, p0)) / dn;
	return p0 + (dif * t);
}

float3 LineProject(const float3 &p0, const float3 &p1, const float3 &a)
{
	// project point a on segment [p0,p1]
	float3 d = p1 - p0;
	float t = dot(d, (a - p0)) / dot(d, d);
	return p0 + d * t;
}

float LineProjectTime(const float3 &p0, const float3 &p1, const float3 &a)
{
	// project point a on segment [p0,p1]
	float3 d = p1 - p0;
	float t = dot(d, (a - p0)) / dot(d, d);
	return t;
}

float3 TriNormal(const float3 &v0, const float3 &v1, const float3 &v2)
{
	// return the normal of the triangle
	// inscribed by v0, v1, and v2
	float3 cp = cross(v1 - v0, v2 - v1);
	float m = magnitude(cp);
	if (m == 0) return float3(1, 0, 0);
	return cp * (1.0f / m);
}

int BoxInside(const float3 &p, const float3 &bmin, const float3 &bmax)
{
	return (p.x >= bmin.x && p.x <= bmax.x &&
			p.y >= bmin.y && p.y <= bmax.y &&
			p.z >= bmin.z && p.z <= bmax.z);
}

int BoxIntersect(const float3 &v0, const float3 &v1, const float3 &bmin, const float3 &bmax, float3 *impact)
{
	if (BoxInside(v0, bmin, bmax))
	{
		*impact = v0;
		return 1;
	}
	if (v0.x <= bmin.x && v1.x >= bmin.x)
	{
		float a = (bmin.x - v0.x) / (v1.x - v0.x);
		//v.x = bmin.x;
		float vy = (1 - a) * v0.y + a * v1.y;
		float vz = (1 - a) * v0.z + a * v1.z;
		if (vy >= bmin.y && vy <= bmax.y && vz >= bmin.z && vz <= bmax.z)
		{
			impact->x = bmin.x;
			impact->y = vy;
			impact->z = vz;
			return 1;
		}
	}
	else if (v0.x >= bmax.x && v1.x <= bmax.x)
	{
		float a = (bmax.x - v0.x) / (v1.x - v0.x);
		//v.x = bmax.x;
		float vy = (1 - a) * v0.y + a * v1.y;
		float vz = (1 - a) * v0.z + a * v1.z;
		if (vy >= bmin.y && vy <= bmax.y && vz >= bmin.z && vz <= bmax.z)
		{
			impact->x = bmax.x;
			impact->y = vy;
			impact->z = vz;
			return 1;
		}
	}
	if (v0.y <= bmin.y && v1.y >= bmin.y)
	{
		float a = (bmin.y - v0.y) / (v1.y - v0.y);
		float vx = (1 - a) * v0.x + a * v1.x;
		//v.y = bmin.y;
		float vz = (1 - a) * v0.z + a * v1.z;
		if (vx >= bmin.x && vx <= bmax.x && vz >= bmin.z && vz <= bmax.z)
		{
			impact->x = vx;
			impact->y = bmin.y;
			impact->z = vz;
			return 1;
		}
	}
	else if (v0.y >= bmax.y && v1.y <= bmax.y)
	{
		float a = (bmax.y - v0.y) / (v1.y - v0.y);
		float vx = (1 - a) * v0.x + a * v1.x;
		// vy = bmax.y;
		float vz = (1 - a) * v0.z + a * v1.z;
		if (vx >= bmin.x && vx <= bmax.x && vz >= bmin.z && vz <= bmax.z)
		{
			impact->x = vx;
			impact->y = bmax.y;
			impact->z = vz;
			return 1;
		}
	}
	if (v0.z <= bmin.z && v1.z >= bmin.z)
	{
		float a = (bmin.z - v0.z) / (v1.z - v0.z);
		float vx = (1 - a) * v0.x + a * v1.x;
		float vy = (1 - a) * v0.y + a * v1.y;
		// v.z = bmin.z;
		if (vy >= bmin.y && vy <= bmax.y && vx >= bmin.x && vx <= bmax.x)
		{
			impact->x = vx;
			impact->y = vy;
			impact->z = bmin.z;
			return 1;
		}
	}
	else if (v0.z >= bmax.z && v1.z <= bmax.z)
	{
		float a = (bmax.z - v0.z) / (v1.z - v0.z);
		float vx = (1 - a) * v0.x + a * v1.x;
		float vy = (1 - a) * v0.y + a * v1.y;
		// v.z = bmax.z;
		if (vy >= bmin.y && vy <= bmax.y && vx >= bmin.x && vx <= bmax.x)
		{
			impact->x = vx;
			impact->y = vy;
			impact->z = bmax.z;
			return 1;
		}
	}
	return 0;
}

float DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint, float3 *vpoint)
{
	static float3 cp;
	cp = normalize(cross(udir, vdir));

	float distu = -dot(cp, ustart);
	float distv = -dot(cp, vstart);
	float dist = (float)fabs(distu - distv);
	if (upoint)
	{
		float3 normal = normalize(cross(vdir, cp));
		*upoint = PlaneLineIntersection(normal, -dot(normal, vstart), ustart, ustart + udir);
	}
	if (vpoint)
	{
		float3 normal = normalize(cross(udir, cp));
		*vpoint = PlaneLineIntersection(normal, -dot(normal, ustart), vstart, vstart + vdir);
	}
	return dist;
}

Quaternion VirtualTrackBall(const float3 &cop, const float3 &cor, const float3 &dir1, const float3 &dir2)
{
	// routine taken from game programming gems.
	// Implement track ball functionality to spin stuf on the screen
	//  cop   center of projection
	//  cor   center of rotation
	//  dir1  old mouse direction
	//  dir2  new mouse direction
	// pretend there is a sphere around cor.  Then find the points
	// where dir1 and dir2 intersect that sphere.  Find the
	// rotation that takes the first point to the second.
	float m;
	// compute plane
	float3 nrml = cor - cop;
	float fudgefactor = 1.0f / (magnitude(nrml) * 0.25f);  // since trackball proportional to distance from cop
	nrml = normalize(nrml);
	float dist = -dot(nrml, cor);
	float3 u = PlaneLineIntersection(nrml, dist, cop, cop + dir1);
	u = u - cor;
	u = u * fudgefactor;
	m = magnitude(u);
	if (m > 1)
	{
		u /= m;
	}
	else
	{
		u = u - (nrml * sqrtf(1 - m * m));
	}
	float3 v = PlaneLineIntersection(nrml, dist, cop, cop + dir2);
	v = v - cor;
	v = v * fudgefactor;
	m = magnitude(v);
	if (m > 1)
	{
		v /= m;
	}
	else
	{
		v = v - (nrml * sqrtf(1 - m * m));
	}
	return RotationArc(u, v);
}

int countpolyhit = 0;
int HitCheckPoly(const float3 *vert, const int n, const float3 &v0, const float3 &v1, float3 *impact, float3 *normal)
{
	countpolyhit++;
	int i;
	float3 nrml(0, 0, 0);
	for (i = 0; i < n; i++)
	{
		int i1 = (i + 1) % n;
		int i2 = (i + 2) % n;
		nrml = nrml + cross(vert[i1] - vert[i], vert[i2] - vert[i1]);
	}

	float m = magnitude(nrml);
	if (m == 0.0)
	{
		return 0;
	}
	nrml = nrml * (1.0f / m);
	float dist = -dot(nrml, vert[0]);
	float d0, d1;
	if ((d0 = dot(v0, nrml) + dist) < 0 || (d1 = dot(v1, nrml) + dist) > 0)
	{
		return 0;
	}

	static float3 the_point;
	// By using the cached plane distances d0 and d1
	// we can optimize the following:
	//     the_point = planelineintersection(nrml,dist,v0,v1);
	float a = d0 / (d0 - d1);
	the_point = v0 * (1 - a) + v1 * a;

	int inside = 1;
	for (int j = 0; inside && j < n; j++)
	{
		// let inside = 0 if outside
		float3 pp1, pp2, side;
		pp1 = vert[j];
		pp2 = vert[(j + 1) % n];
		side = cross((pp2 - pp1), (the_point - pp1));
		inside = (dot(nrml, side) >= 0.0);
	}
	if (inside)
	{
		if (normal)
		{
			*normal = nrml;
		}
		if (impact)
		{
			*impact = the_point;
		}
	}
	return inside;
}

int SolveQuadratic(float a, float b, float c, float *ta, float *tb)  // if true returns roots ta,tb where ta<=tb
{
	assert(ta);
	assert(tb);
	float d = b * b - 4.0f * a * c;  // discriminant
	if (d < 0.0f) return 0;
	float sqd = sqrtf(d);
	*ta = (-b - sqd) / (2.0f * a);
	*tb = (-b + sqd) / (2.0f * a);
	return 1;
}

int HitCheckRaySphere(const float3 &sphereposition, float radius, const float3 &_v0, const float3 &_v1, float3 *impact, float3 *normal)
{
	assert(impact);
	assert(normal);
	float3 dv = _v1 - _v0;
	float3 v0 = _v0 - sphereposition;            // solve in coord system of the sphere
	if (radius <= 0.0f || _v0 == _v1) return 0;  // only true if point moves from outside to inside sphere.
	float a = dot(dv, dv);
	float b = 2.0f * dot(dv, v0);
	float c = dot(v0, v0) - radius * radius;
	if (c < 0.0f) return 0;  // we are already inside the sphere.

	float ta, tb;
	int doesIntersect = SolveQuadratic(a, b, c, &ta, &tb);

	if (!doesIntersect) return 0;

	if (ta >= 0.0f && ta <= 1.0f && (ta <= tb || tb <= 0.0f))
	{
		*impact = _v0 + dv * ta;
		*normal = (v0 + dv * ta) / radius;
		return 1;
	}
	if (tb >= 0.0f && tb <= 1.0f)
	{
		assert(tb <= ta || ta <= 0.0f);  // tb must be better than ta
		*impact = _v0 + dv * tb;
		*normal = (v0 + dv * tb) / radius;
		return 1;
	}
	return 0;
}

int HitCheckRayCylinder(const float3 &p0, const float3 &p1, float radius, const float3 &_v0, const float3 &_v1, float3 *impact, float3 *normal)
{
	assert(impact);
	assert(normal);
	// only concerned about hitting the sides, not the caps for now
	float3x3 m = RotationArc(p1 - p0, float3(0, 0, 1.0f)).getmatrix();
	float h = ((p1 - p0) * m).z;
	float3 v0 = (_v0 - p0) * m;
	float3 v1 = (_v1 - p0) * m;
	if (v0.z <= 0.0f && v1.z <= 0.0f) return 0;                                  // entirely below cylinder
	if (v0.z >= h && v1.z >= h) return 0;                                        // ray is above cylinder
	if (v0.z < 0.0f) v0 = PlaneLineIntersection(float3(0, 0, 1.0f), 0, v0, v1);  // crop to cylinder range
	if (v1.z < 0.0f) v1 = PlaneLineIntersection(float3(0, 0, 1.0f), 0, v0, v1);
	if (v0.z > h) v0 = PlaneLineIntersection(float3(0, 0, 1.0f), -h, v0, v1);
	if (v1.z > h) v1 = PlaneLineIntersection(float3(0, 0, 1.0f), -h, v0, v1);
	if (v0.x == v1.x && v0.y == v1.y) return 0;
	float3 dv = v1 - v0;

	float a = dv.x * dv.x + dv.y * dv.y;
	float b = 2.0f * (dv.x * v0.x + dv.y * v0.y);
	float c = (v0.x * v0.x + v0.y * v0.y) - radius * radius;
	if (c < 0.0f) return 0;  // we are already inside the cylinder .

	float ta, tb;
	int doesIntersect = SolveQuadratic(a, b, c, &ta, &tb);

	if (!doesIntersect) return 0;

	if (ta >= 0.0f && ta <= 1.0f && (ta <= tb || tb <= 0.0f))
	{
		*impact = (v0 + dv * ta) * Transpose(m) + p0;
		*normal = (float3(v0.x, v0.y, 0.0f) + float3(dv.x, dv.y, 0) * ta) / radius * Transpose(m);
		return 1;
	}
	if (tb >= 0.0f && tb <= 1.0f)
	{
		assert(tb <= ta || ta <= 0.0f);                // tb must be better than ta
		*impact = (v0 + dv * tb) * Transpose(m) + p0;  // compute intersection in original space
		*normal = (float3(v0.x, v0.y, 0.0f) + float3(dv.x, dv.y, 0) * tb) / radius * Transpose(m);
		return 1;
	}
	return 0;
}

int HitCheckSweptSphereTri(const float3 &p0, const float3 &p1, const float3 &p2, float radius, const float3 &v0, const float3 &_v1, float3 *impact, float3 *normal)
{
	float3 unused;
	if (!normal) normal = &unused;
	float3 v1 = _v1;  // so we can update v1 after each sub intersection test if necessary
	int hit = 0;
	float3 cp = cross(p1 - p0, p2 - p0);
	if (dot(cp, v1 - v0) >= 0.0f) return 0;  // coming from behind and/or moving away
	float3 n = normalize(cp);
	float3 tv[3];
	tv[0] = p0 + n * radius;
	tv[1] = p1 + n * radius;
	tv[2] = p2 + n * radius;
	hit += HitCheckPoly(tv, 3, v0, v1, &v1, normal);
	hit += HitCheckRayCylinder(p0, p1, radius, v0, v1, &v1, normal);
	hit += HitCheckRayCylinder(p1, p2, radius, v0, v1, &v1, normal);
	hit += HitCheckRayCylinder(p2, p0, radius, v0, v1, &v1, normal);
	hit += HitCheckRaySphere(p0, radius, v0, v1, &v1, normal);
	hit += HitCheckRaySphere(p1, radius, v0, v1, &v1, normal);
	hit += HitCheckRaySphere(p2, radius, v0, v1, &v1, normal);
	if (hit && impact) *impact = v1 + *normal * 0.001f;
	return hit;
}

float3 PlanesIntersection(const Plane &p0, const Plane &p1, const Plane &p2)
{
	float3x3 mp = Transpose(float3x3(p0.normal(), p1.normal(), p2.normal()));
	float3x3 mi = Inverse(mp);
	float3 b(p0.dist(), p1.dist(), p2.dist());
	return -b * mi;
}

float3 PlanesIntersection(const Plane *planes, int planes_count, const float3 &seed)
{
	int i;
	float3x3 A;  // gets initilized to 0 matrix
	float3 b(0, 0, 0);
	for (i = 0; i < planes_count; i++)
	{
		const Plane &p = planes[i];
		A += outerprod(p.normal(), p.normal());
		b += p.normal() * -p.dist();
	}
	float3x3 evecs = Diagonalizer(A).getmatrix();           // eigenvectors
	float3 evals = Diagonal(evecs * A * Transpose(evecs));  // eigenvalues
	for (i = 0; i < 3; i++)
	{
		if (fabsf(evals[i]) < 1.0f)  // not sure if they are necessarily positive
		{
			Plane p;
			p.normal() = evecs[i] * squared(1.0f - evals[i]);
			p.dist() = -dot(seed, p.normal());
			A += outerprod(p.normal(), p.normal());
			b += p.normal() * -p.dist();
		}
	}
	return Inverse(A) * b;
}

Plane Transform(const Plane &p, const float3 &translation, const Quaternion &rotation)
{
	//   Transforms the plane by the given translation/rotation.
	float3 newnormal = rotate(rotation, p.normal());
	return Plane(newnormal, p.dist() - dot(newnormal, translation));
}

float3 PlaneProject(const Plane &plane, const float3 &point)
{
	return point - plane.normal() * (dot(point, plane.normal()) + plane.dist());
}
float3 PlaneLineIntersection(const Plane &plane, const float3 &p0, const float3 &p1)
{
	// returns the point where the line p0-p1 intersects the plane n&d
	float3 dif;
	dif = p1 - p0;
	float dn = dot(plane.normal(), dif);
	float t = -(plane.dist() + dot(plane.normal(), p0)) / dn;
	return p0 + (dif * t);
}

int Clip(const float3 &plane_normal, float plane_dist, const float3 *verts_in, int count_in, float3 *verts_out)
{
	// clips a polygon specified by the non-indexed vertex list verts_in.
	// verts_out must be preallocated with a size >= count+1
	assert(verts_out);
	int n = 0;
	int prev_status = (dot(plane_normal, verts_in[count_in - 1]) + plane_dist > 0);
	for (int i = 0; i < count_in; i++)
	{
		int status = (dot(plane_normal, verts_in[i]) + plane_dist > 0);
		if (status != prev_status)
		{
			verts_out[n++] = PlaneLineIntersection(plane_normal, plane_dist, verts_in[(i == 0) ? count_in - 1 : i - 1], verts_in[i]);
		}
		if (status == 0)  // under
		{
			verts_out[n++] = verts_in[i];
		}
	}
	assert(n <= count_in + 1);  // remove if intention to use this routine on convex polygons
	return n;
}

int ClipPolyPoly(const float3 &normal, const float3 *clipper, int clipper_count, const float3 *verts_in, int in_count, float3 *scratch)
{
	// clips polys against each other.
	// requires sufficiently allocated temporary memory in scratch buffer
	// function returns final number of vertices in clipped polygon.
	// Resulting vertices are returned in the scratch buffer.
	// if the arrays are the same &verts_in==&scratch the routine should still work anyways.
	// the first argument (normal) is the normal of polygon clipper.
	// its generally assumed both are convex polygons.
	assert(scratch);  // size should be >= 2*(clipper_count+in_count)
	int i;
	int bsize = clipper_count + in_count;
	int count = in_count;
	for (i = 0; i < clipper_count; i++)
	{
		int i1 = (i + 1) % clipper_count;
		float3 n = cross(clipper[i1] - clipper[i], normal);
		if (n == float3(0, 0, 0)) continue;
		n = normalize(n);
		count = Clip(n, -dot(clipper[i], n), (i == 0) ? verts_in : (i % 2) ? scratch : scratch + bsize, count, (i % 2) ? scratch + bsize : scratch);
		assert(count < bsize);
	}
	if (clipper_count % 2) memcpy(scratch, scratch + bsize, count * sizeof(float3));
	return count;
}

float Volume(const float3 *vertices, const int3 *tris, const int count)
{
	// count is the number of triangles (tris)
	float volume = 0;
	for (int i = 0; i < count; i++)  // for each triangle
	{
		volume += Determinant(float3x3(vertices[tris[i][0]], vertices[tris[i][1]], vertices[tris[i][2]]));  //divide by 6 later for efficiency
	}
	return volume / 6.0f;  // since the determinant give 6 times tetra volume
}

float3 CenterOfMass(const float3 *vertices, const int3 *tris, const int count)
{
	// count is the number of triangles (tris)
	float3 com(0, 0, 0);
	float volume = 0;                // actually accumulates the volume*6
	for (int i = 0; i < count; i++)  // for each triangle
	{
		float3x3 A(vertices[tris[i][0]], vertices[tris[i][1]], vertices[tris[i][2]]);
		float vol = Determinant(A);      // dont bother to divide by 6
		com += vol * (A.x + A.y + A.z);  // divide by 4 at end
		volume += vol;
	}
	com /= volume * 4.0f;
	return com;
}
float3x3 Inertia(const float3 *vertices, const int3 *tris, const int count, const float3 &com /* =float3(0,0,0) */)
{
	// count is the number of triangles (tris)
	// The moments are calculated based on the center of rotation (com) which defaults to [0,0,0] if unsupplied
	// assume mass==1.0  you can multiply by mass later.
	// for improved accuracy the next 3 variables, the determinant d, and its calculation should be changed to double
	float volume = 0;                // technically this variable accumulates the volume times 6
	float3 diag(0, 0, 0);            // accumulate matrix main diagonal integrals [x*x, y*y, z*z]
	float3 offd(0, 0, 0);            // accumulate matrix off-diagonal  integrals [y*z, x*z, x*y]
	for (int i = 0; i < count; i++)  // for each triangle
	{
		float3x3 A(vertices[tris[i][0]] - com, vertices[tris[i][1]] - com, vertices[tris[i][2]] - com);  // matrix trick for volume calc by taking determinant
		float d = Determinant(A);                                                                        // vol of tiny parallelapiped= d * dr * ds * dt (the 3 partials of my tetral triple integral equasion)
		volume += d;                                                                                     // add vol of current tetra (note it could be negative - that's ok we need that sometimes)
		for (int j = 0; j < 3; j++)
		{
			int j1 = (j + 1) % 3;
			int j2 = (j + 2) % 3;
			diag[j] += (A[0][j] * A[1][j] + A[1][j] * A[2][j] + A[2][j] * A[0][j] +
						A[0][j] * A[0][j] + A[1][j] * A[1][j] + A[2][j] * A[2][j]) *
					   d;  // divide by 60.0f later;
			offd[j] += (A[0][j1] * A[1][j2] + A[1][j1] * A[2][j2] + A[2][j1] * A[0][j2] +
						A[0][j1] * A[2][j2] + A[1][j1] * A[0][j2] + A[2][j1] * A[1][j2] +
						A[0][j1] * A[0][j2] * 2 + A[1][j1] * A[1][j2] * 2 + A[2][j1] * A[2][j2] * 2) *
					   d;  // divide by 120.0f later
		}
	}
	diag /= volume * (60.0f / 6.0f);  // divide by total volume (vol/6) since density=1/volume
	offd /= volume * (120.0f / 6.0f);
	return float3x3(diag.y + diag.z, -offd.z, -offd.y,
					-offd.z, diag.x + diag.z, -offd.x,
					-offd.y, -offd.x, diag.x + diag.y);
}

float3x3 ShapeInertiaContrib(const float3 &cor, const float3 &position, const Quaternion &orientation,
							 const float3 &shape_com, const float3x3 &shape_inertia, float shape_mass)
{
	// transforms 3x3 inertia tensor from local reference frame to a more global one.
	// essentially returns the contribution of a subshape to the inertia of a larger rigid body
	// typical usage:
	//         foreach shape s { totalinertia += InertiaContribution(...); }
	// cor - new center of rotation that we are translating to.
	// This could be the center of mass of the compound object.
	// Another application is when an object is attached to something (nail-joint) that is static, in which
	// one easy way to implement this is to lock the center of rotation and adjust the inertia accordingly.
	// position & orientation - is the current pose or transform of the shape.
	// Obviously position, orientation and cor are all described wrt the same reference frame.
	// shape_com and shape_inertia are the center of mass and the inertia of the shape in the local coordinate system of that shape.
	// To clarify, if a shape happened to be located somewhere else then position or orientation would be different, but
	// com and inertia would be the same.
	float3x3 Identity(1.0f, 0, 0, 0, 1.0f, 0, 0, 0, 1.0f);
	float3x3 R = orientation.getmatrix();
	float3 r = (shape_com * R + position) - cor;
	return Transpose(R) * shape_inertia * R + (Identity * dot(r, r) - outerprod(r, r)) * shape_mass;
}

Quaternion Diagonalizer(const float3x3 &A)
{
	// A must be a symmetric matrix.
	// returns quaternion q such that its corresponding matrix Q
	// can be used to Diagonalize A
	// Diagonal matrix D = Q * A * Transpose(Q);  and  A = QT*D*Q
	// The rows of q are the eigenvectors D's diagonal is the eigenvalues
	// As per 'row' convention if float3x3 Q = q.getmatrix(); then v*Q = q*v*conj(q)
	int maxsteps = 24;  // certainly wont need that many.
	int i;
	Quaternion q(0, 0, 0, 1);
	for (i = 0; i < maxsteps; i++)
	{
		float3x3 Q = q.getmatrix();                                        // v*Q == q*v*conj(q)
		float3x3 D = Q * A * Transpose(Q);                                 // A = Q^T*D*Q
		float3 offdiag(D[1][2], D[0][2], D[0][1]);                         // elements not on the diagonal
		float3 om(fabsf(offdiag.x), fabsf(offdiag.y), fabsf(offdiag.z));   // mag of each offdiag elem
		int k = (om.x > om.y && om.x > om.z) ? 0 : (om.y > om.z) ? 1 : 2;  // index of largest element of offdiag
		int k1 = (k + 1) % 3;
		int k2 = (k + 2) % 3;
		if (offdiag[k] == 0.0f) break;  // diagonal already
		float thet = (D[k2][k2] - D[k1][k1]) / (2.0f * offdiag[k]);
		float sgn = (thet > 0.0f) ? 1.0f : -1.0f;
		thet *= sgn;                                                                     // make it positive
		float t = sgn / (thet + ((thet < 1.E6f) ? sqrtf(squared(thet) + 1.0f) : thet));  // sign(T)/(|T|+sqrt(T^2+1))
		float c = 1.0f / sqrtf(squared(t) + 1.0f);                                       //  c= 1/(t^2+1) , t=s/c
		if (c == 1.0f) break;                                                            // no room for improvement - reached machine precision.
		Quaternion jr(0, 0, 0, 0);                                                       // jacobi rotation for this iteration.
		jr[k] = sgn * sqrtf((1.0f - c) / 2.0f);                                          // using 1/2 angle identity sin(a/2) = sqrt((1-cos(a))/2)
		jr[k] *= -1.0f;                                                                  // since our quat-to-matrix convention was for v*M instead of M*v
		jr.w = sqrtf(1.0f - squared(jr[k]));
		if (jr.w == 1.0f) break;  // reached limits of floating point precision
		q = q * jr;
		q.Normalize();
	}
	return q;
}

float3 Diagonal(const float3x3 &M)
{
	return float3(M[0][0], M[1][1], M[2][2]);
}