fix a lot of issues, add gyroscopic force integrator, add contact polygon winding, add some quaternion constraints, add more tests

include/bounce/common/draw.h

@@ -54,7 +54,8 @@ public :
 		e_contactPointsFlag = 0x0008,
 		e_contactNormalsFlag = 0x0010,
 		e_contactTangentsFlag = 0x0020,
-		e_aabbsFlag = 0x0040,
+		e_contactAreasFlag = 0x0040,
+		e_aabbsFlag = 0x0080,
 	};
 
 	b3Draw()

include/bounce/common/geometry.h

@@ -75,13 +75,6 @@ inline float32 b3Distance(const b3Vec3& P, const b3Plane& plane)
 	return b3Dot(plane.normal, P) - plane.offset;
 }
 
-// Project a point onto a normal plane.
-inline b3Vec3 b3Project(const b3Vec3& P, const b3Plane& plane)
-{
-	float32 fraction = b3Distance(P, plane);
-	return P - fraction * plane.normal;
-}
-
 // Compute barycentric coordinates (u, v) for point Q to segment AB.
 // The last output value is the divisor.
 inline void b3Barycentric(float32 out[3], 
@@ -150,4 +143,33 @@ inline void b3Barycentric(float32 out[5],
 	out[4] = sign * divisor;
 }
 
+// Project a point onto a normal plane.
+inline b3Vec3 b3ClosestPointOnPlane(const b3Vec3& P, const b3Plane& plane)
+{
+	float32 fraction = b3Distance(P, plane);
+	return P - fraction * plane.normal;
+}
+
+// Project a point onto a segment AB.
+inline b3Vec3 b3ClosestPointOnSegment(const b3Vec3& P, const b3Vec3& A, const b3Vec3& B)
+{
+	float32 wAB[3];
+	b3Barycentric(wAB, A, B, P);
+
+	if (wAB[1] <= 0.0f)
+	{
+		return A;
+	}
+
+	if (wAB[0] <= 0.0f)
+	{
+		return B;
+	}
+
+	float32 s = 1.0f / wAB[2];
+	float32 wA = s * wAB[0];
+	float32 wB = s * wAB[1];
+	return wA * A + wB * B;
+}
+
 #endif

include/bounce/common/math/mat.h

@@ -20,69 +20,350 @@
 #define B3_MAT_H
 
 #include <bounce/common/math/math.h>
+#include <bounce/common/math/mat22.h>
+#include <bounce/common/math/mat33.h>
 
-// A vector stored in column-major order.
-template<u32 n>
-struct b3Vec
+struct b3Mat23
 {
-	b3Vec() { }
+	b3Mat23() { }
 
-	const float32& operator[](u32 i) const
+	b3Mat23(const b3Vec2& _x, const b3Vec2& _y, const b3Vec2& _z)
 	{
-		return e[i];
-	}
-	
-	float32& operator[](u32 i)
-	{
-		return e[i];
-	}
-	
-	void operator+=(const b3Vec<n>& v)
-	{
-		for (u32 i = 0; i < n; ++i)
-		{
-			e[i] += v[i];
-		}
+		x = _x;
+		y = _y;
+		z = _z;
 	}
 
-	float32 e[n];
+	void SetZero()
+	{
+		x.SetZero();
+		y.SetZero();
+		z.SetZero();
+	}
+
+	b3Vec2 x, y, z;
 };
 
-template<u32 n>
-inline b3Vec<n> operator-(const b3Vec<n>& v)
+struct b3Mat24
 {
-	b3Vec<n> result;
-	for (u32 i = 0; i < n; ++i)
+	b3Mat24() { }
+
+	b3Mat24(const b3Vec2& _x, const b3Vec2& _y, const b3Vec2& _z, const b3Vec2& _w)
 	{
-		result[i] = -v[i];
+		x = _x;
+		y = _y;
+		z = _z;
+		w = _w;
 	}
+
+	void SetZero()
+	{
+		x.SetZero();
+		y.SetZero();
+		z.SetZero();
+		w.SetZero();
+	}
+
+	b3Vec2 x, y, z, w;
+};
+
+struct b3Mat32
+{
+	b3Mat32() { }
+
+	b3Mat32(const b3Vec3& _x, const b3Vec3& _y)
+	{
+		x = _x;
+		y = _y;
+	}
+
+	void SetZero()
+	{
+		x.SetZero();
+		y.SetZero();
+	}
+
+	b3Vec3 x, y;
+};
+
+struct b3Mat34
+{
+	b3Mat34() { }
+
+	b3Mat34(const b3Vec3& _x, const b3Vec3& _y, const b3Vec3& _z, const b3Vec3& _w)
+	{
+		x = _x;
+		y = _y;
+		z = _z;
+		w = _w;
+	}
+
+	void SetZero()
+	{
+		x.SetZero();
+		y.SetZero();
+		z.SetZero();
+		w.SetZero();
+	}
+
+	b3Vec3 x, y, z, w;
+};
+
+struct b3Vec4
+{
+	b3Vec4() { }
+
+	b3Vec4(float32 _x, float32 _y, float32 _z, float32 _w)
+	{
+		x = _x;
+		y = _y;
+		z = _z;
+		w = _w;
+	}
+
+	void SetZero()
+	{
+		x = 0.0f;
+		y = 0.0f;
+		z = 0.0f;
+		w = 0.0f;
+	}
+
+	float32 x, y, z, w;
+};
+
+struct b3Mat43
+{
+	b3Mat43() { }
+
+	b3Mat43(const b3Vec4& _x, const b3Vec4& _y, const b3Vec4& _z)
+	{
+		x = _x;
+		y = _y;
+		z = _z;
+	}
+
+	void SetZero()
+	{
+		x.SetZero();
+		y.SetZero();
+		z.SetZero();
+	}
+
+	b3Vec4 x, y, z;
+};
+
+struct b3Mat44
+{
+	b3Mat44() { }
+
+	b3Mat44(const b3Vec4& _x, const b3Vec4& _y, const b3Vec4& _z, const b3Vec4& _w)
+	{
+		x = _x;
+		y = _y;
+		z = _z;
+		w = _w;
+	}
+
+	void SetZero()
+	{
+		x.SetZero();
+		y.SetZero();
+		z.SetZero();
+		w.SetZero();
+	}
+
+	b3Vec4 x, y, z, w;
+};
+
+inline b3Vec4 operator+(const b3Vec4& a, const b3Vec4& b)
+{
+	return b3Vec4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
+}
+
+inline b3Vec4 operator-(const b3Vec4& a, const b3Vec4& b)
+{
+	return b3Vec4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
+}
+
+// 1x1 * 1x4 = 1x1
+inline b3Vec4 operator*(float32 s, const b3Vec4& v)
+{
+	return b3Vec4(s * v.x, s * v.y, s * v.z, s * v.w);
+}
+
+// a * 4x4 = 4x4
+inline b3Mat44 operator*(float32 s, const b3Mat44& A)
+{
+	return b3Mat44(s * A.x, s * A.y, s * A.z, s * A.w);
+}
+
+// 4x4 * 4x1 = 4x1
+inline b3Vec4 operator*(const b3Mat44& A, const b3Vec4& v)
+{
+	return v.x * A.x + v.y * A.y + v.z * A.z + v.w * A.w;
+}
+
+// 4x4 * 4x4 = 4x4
+inline b3Mat44 operator*(const b3Mat44& A, const b3Mat44& B)
+{
+	return b3Mat44(A * B.x, A * B.y, A * B.z, A * B.w);
+}
+
+// a * 3x4 = 3x4
+inline b3Mat34 operator*(float32 s, const b3Mat34& A)
+{
+	return b3Mat34(s * A.x, s * A.y, s * A.z, s * A.w);
+}
+
+// 4x3 * 3x1 = 4x1
+inline b3Vec4 operator*(const b3Mat43& A, const b3Vec3& v)
+{
+	return v.x * A.x + v.y * A.y + v.z * A.z;
+}
+
+// 3x4 * 4x1 = 3x1
+inline b3Vec3 operator*(const b3Mat34& A, const b3Vec4& v)
+{
+	return v.x * A.x + v.y * A.y + v.z * A.z + v.w * A.w;
+}
+
+// 1x4 * 4x1 = 1x1
+inline float32 operator*(const b3Vec4& A, const b3Vec4& B)
+{
+	return A.x * B.x + A.y * B.y + A.z * B.z + A.w * B.w;
+}
+
+// 3x1 * 1x1 = 3x1
+inline b3Vec3 operator*(const b3Vec3& v, float32 s)
+{
+	return s * v;
+}
+
+// 2x1 * 1x1 = 2x1
+inline b3Vec2 operator*(const b3Vec2& v, float32 s)
+{
+	return s * v;
+}
+
+// 1x3 * 3x1 = 1x1
+inline float32 operator*(const b3Vec3& A, const b3Vec3& B)
+{
+	return A.x * B.x + A.y * B.y + A.z * B.z;
+}
+
+// 1x3 * 3x3 = 1x3
+inline b3Vec3 operator*(const b3Vec3& A, const b3Mat33& B)
+{
+	return b3Vec3(A * B.x, A * B.y, A * B.z);
+}
+
+// 1x4 * 4x4 = 1x4
+inline b3Vec4 operator*(const b3Vec4& A, const b3Mat44& B)
+{
+	return b3Vec4(A * B.x, A * B.y, A * B.z, A * B.w);
+}
+
+// 1x4 * 4x3 = 1x3
+inline b3Vec3 operator*(const b3Vec4& A, const b3Mat43& B)
+{
+	return b3Vec3(A * B.x, A * B.y, A * B.z);
+}
+
+// 3x2 * 2x1 = 3x1
+inline b3Vec3 operator*(const b3Mat32& A, const b3Vec2& B)
+{
+	return B.x * A.x + B.y * A.y;
+}
+
+// 2x3 * 2x1 = 2x1
+inline b3Vec2 operator*(const b3Mat23& A, const b3Vec3& B)
+{
+	return B.x * A.x + B.y * A.y + B.z * A.z;
+}
+
+// 2x3 * 2x2 = 2x2
+inline b3Mat22 operator*(const b3Mat23& A, const b3Mat32& B)
+{
+	return b3Mat22(A * B.x, A * B.y);
+}
+
+// 2x3 * 3x3 = 2x3
+inline b3Mat23 operator*(const b3Mat23& A, const b3Mat33& B)
+{
+	return b3Mat23(A * B.x, A * B.y, A * B.z);
+}
+
+// 3x4 * 4x3 = 3x3
+inline b3Mat33 operator*(const b3Mat34& A, const b3Mat43& B)
+{
+	return b3Mat33(A * B.x, A * B.y, A * B.z);
+}
+
+// 3x4 * 4x4 = 3x3
+inline b3Mat34 operator*(const b3Mat34& A, const b3Mat44& B)
+{
+	return b3Mat34(A * B.x, A * B.y, A * B.z, A * B.w);
+}
+
+// 2x4 * 4x1 = 2x1
+inline b3Vec2 operator*(const b3Mat24& A, const b3Vec4& B)
+{
+	return B.x * A.x + B.y * A.y + B.z * A.z + B.w * A.w;
+}
+
+// 2x4 * 4x3 = 4x3
+inline b3Mat23 operator*(const b3Mat24& A, const b3Mat43& B)
+{
+	return b3Mat23(A * B.x, A * B.y, A * B.z);
+}
+
+// 2x4 * 2x4 = 2x4
+inline b3Mat24 operator*(const b3Mat24& A, const b3Mat44& B)
+{
+	return b3Mat24(A * B.x, A * B.y, A * B.z, A * B.w);
+}
+
+// 4x4 * 4x3 = 4x3
+inline b3Mat43 operator*(const b3Mat44& A, const b3Mat43& B)
+{
+	return b3Mat43(A * B.x, A * B.y, A * B.z);
+}
+
+inline b3Mat23 b3Transpose(const b3Mat32& A)
+{
+	b3Mat23 result;
+	result.x = b3Vec2(A.x.x, A.y.x);
+	result.y = b3Vec2(A.x.y, A.y.y);
+	result.z = b3Vec2(A.x.z, A.y.z);
 	return result;
 }
 
-// A matrix stored in column-major order.
-template<u32 n, u32 m>
-struct b3Mat
+inline b3Mat32 b3Transpose(const b3Mat23& A)
 {
-	b3Mat() { }
+	b3Mat32 result;
+	result.x = b3Vec3(A.x.x, A.y.x, A.z.x);
+	result.y = b3Vec3(A.x.y, A.y.y, A.z.y);
+	return result;
+}
 
-	const float32& operator()(u32 i, u32 j) const
-	{
-		return e[i + n * j];
-	}
+inline b3Mat34 b3Transpose(const b3Mat43& A)
+{
+	b3Mat34 result;
+	result.x = b3Vec3(A.x.x, A.y.x, A.z.x);
+	result.y = b3Vec3(A.x.y, A.y.y, A.z.y);
+	result.z = b3Vec3(A.x.z, A.y.z, A.z.z);
+	result.w = b3Vec3(A.x.w, A.y.w, A.z.w);
+	return result;
+}
 
-	float32& operator()(u32 i, u32 j)
-	{
-		return e[i + n * j];
-	}
-
-	float32 e[n * m];
-};
-
-// Solve Ax = b.
-// It doesn't compute the inverse. 
-// Therefore, is more efficient.
-// Returns false if the matrix is singular.
-// Warning: Make sure to pass a copy of the original matrix to the function. A will be invalidated.
-bool b3Solve(float32* b, float32* A, u32 n);
+inline b3Mat43 b3Transpose(const b3Mat34& A)
+{
+	b3Mat43 result;
+	result.x = b3Vec4(A.x.x, A.y.x, A.z.x, A.w.x);
+	result.y = b3Vec4(A.x.y, A.y.y, A.z.y, A.w.y);
+	result.z = b3Vec4(A.x.z, A.y.z, A.z.z, A.w.z);
+	return result;
+}
 
 #endif

include/bounce/common/math/mat22.h

@@ -30,6 +30,13 @@ struct b3Mat22
 	// Set this matrix from two vectors.
 	b3Mat22(const b3Vec2& _x, const b3Vec2& _y) : x(_x), y(_y) { }
 
+	// Set this matrix to the zero matrix.
+	void SetZero()
+	{
+		x.SetZero();
+		y.SetZero();
+	}
+
 	// Solve Ax = b. 
 	// It doesn't compute the inverse. 
 	// Therefore, is more efficient.
@@ -45,6 +52,12 @@ inline b3Vec2 operator*(const b3Mat22& A, const b3Vec2& v)
 	return v.x * A.x + v.y * A.y;
 }
 
+// Add two matrices.
+inline b3Mat22 operator+(const b3Mat22& A, const b3Mat22& B)
+{
+	return b3Mat22(A.x + B.x, A.y + B.y);
+}
+
 // Multiply a matrix times a vector.
 inline b3Vec2 b3Mul(const b3Mat22& A, const b3Vec2& v)
 {
@@ -54,6 +67,6 @@ inline b3Vec2 b3Mul(const b3Mat22& A, const b3Vec2& v)
 // Invert a matrix.
 // If the matrix determinant is zero this returns 
 // the zero matrix.
-inline b3Mat22 b3Inverse(const b3Mat22& A);
+b3Mat22 b3Inverse(const b3Mat22& A);
 
 #endif

include/bounce/common/math/quat.h

@@ -37,7 +37,19 @@ struct b3Quat
 	{
 		Set(axis, angle);
 	}
+	
+	// Write an indexed value to this quaternion.
+	float32& operator[](u32 i)
+	{
+		return (&x)[i];
+	}
 
+	// Read an indexed value from this quaternion.
+	float32 operator[](u32 i) const
+	{
+		return (&x)[i];
+	}
+	
 	// Add a quaternion to this quaternion.
 	void operator+=(const b3Quat& q)
 	{
@@ -203,6 +215,77 @@ inline b3Vec3 b3Mul(const b3Quat& q, const b3Vec3& v)
 	return v + qs * t + b3Cross(qv, t);
 }
 
+// Inverse rotate a vector by an orientation quaternion.
+inline b3Vec3 b3MulT(const b3Quat& q, const b3Vec3& v)
+{
+	return b3Mul(b3Conjugate(q), v);
+}
+
+// Convert a 3-by-3 rotation matrix to an orientation quaternion.
+inline b3Quat b3ConvertRotToQuat(const b3Mat33& m)
+{
+	// Check the diagonal.
+	float32 trace = m[0][0] + m[1][1] + m[2][2];
+	
+	if (trace > 0.0f) 
+	{
+		b3Quat result;
+		
+		float32 s = b3Sqrt(trace + 1.0f);
+		result.w = 0.5f * s;
+				
+		float32 t = 0.5f / s;
+		result.x = t * (m[1][2] - m[2][1]);
+		result.y = t * (m[2][0] - m[0][2]);
+		result.z = t * (m[0][1] - m[1][0]);
+		return result;
+	}
+
+	// Diagonal is negative.
+	const i32 next[3] = { 1, 2, 0 };
+	
+	i32 i = 0;
+	
+	if (m[1][1] > m[0][0])
+	{
+		i = 1;
+	}
+
+	if (m[2][2] > m[i][i])
+	{
+		i = 2;
+	}
+
+	i32 j = next[i];
+	i32 k = next[j];
+
+	float32 s = sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1.0f);
+	
+	float32 q[4];
+	q[i] = s * 0.5f;
+	
+	float32 t;
+	if (s != 0.0f) 
+	{
+		t = 0.5f / s;
+	}
+	else
+	{
+		t = s;
+	}
+
+	q[3] = t * (m[j][k] - m[k][j]);
+	q[j] = t * (m[i][j] + m[j][i]);
+	q[k] = t * (m[i][k] + m[k][i]);
+		
+	b3Quat result;
+	result.x = q[0];
+	result.y = q[1];
+	result.z = q[2];
+	result.w = q[3];
+	return result;
+}
+
 // Convert an orientation quaternion to a 3-by-3 rotation matrix.
 inline b3Mat33 b3ConvertQuatToRot(const b3Quat& q)
 {
@@ -218,43 +301,4 @@ inline b3Mat33 b3ConvertQuatToRot(const b3Quat& q)
 		b3Vec3(         xz + wy,          yz - wx, 1.0f - (xx + yy)));
 }
 
-// Perform a linear interpolation between two quaternions.
-inline b3Quat b3Lerp(const b3Quat& a, const b3Quat& b, float32 fraction)
-{
-	B3_ASSERT(fraction >= 0.0f);
-	B3_ASSERT(fraction <= 1.0f);
-	float32 w1 = 1.0f - fraction;
-	float32 w2 = fraction;
-	return w1 * a + w2 * b;
-}
-
-// Perform a spherical interpolation between two quaternions.
-inline b3Quat b3Slerp(const b3Quat& a, const b3Quat& b, float32 fraction)
-{
-	B3_ASSERT(fraction >= 0.0f);
-	B3_ASSERT(fraction <= 1.0f);
-	float32 w1 = 1.0f - fraction;
-	float32 w2 = fraction;
-
-	float32 cosine = b3Dot(a, b);
-	b3Quat b2 = b;
-	if (cosine <= FLT_EPSILON * FLT_EPSILON)
-	{
-		b2 = -b;
-		cosine = -cosine;
-	}
-
-	if (cosine > 1.0f - FLT_EPSILON)
-	{
-		return w1 * a + w2 * b2;
-	}
-
-	float32 angle = acos(cosine);
-	float32 sine = sin(angle);
-	b3Quat q1 = sin(w1 * angle) * a;
-	b3Quat q2 = sin(w2 * angle) * b2;
-	float32 invSin = 1.0f / sine;
-	return invSin * (q1 + q2);
-}
-
 #endif

include/bounce/common/settings.h

@@ -43,10 +43,6 @@ typedef float float32;
 
 // Collision
 
-// Maximum number of vertices, edges, and faces a 
-// polyhedron can have. Don't increase this value.
-#define B3_MAX_HULL_FEATURES (256)
-
 // How much an AABB in the broad-phase should be extended by 
 // to disallow unecessary proxy updates.
 // A larger value increases performance when there are 
@@ -60,15 +56,11 @@ typedef float float32;
 #define B3_AABB_MULTIPLIER (2.0f)
 
 // Collision and constraint tolerance.
-#define B3_LINEAR_SLOP (0.01f)
-
-// Collision and constraint tolerance.
+#define B3_LINEAR_SLOP (0.005f)
 #define B3_ANGULAR_SLOP (2.0f / 180.0f * B3_PI)
 
 // The radius of the hull shape skin.
-#define B3_HULL_RADIUS (2.0f * B3_LINEAR_SLOP)
-
-// Twice the radius of the hull shape skin.
+#define B3_HULL_RADIUS (0.0f * B3_LINEAR_SLOP)
 #define B3_HULL_RADIUS_SUM (2.0f * B3_HULL_RADIUS)
 
 // Dynamics
@@ -95,26 +87,22 @@ typedef float float32;
 #define B3_MAX_ROTATION (0.5f * B3_PI)
 #define B3_MAX_ROTATION_SQUARED (B3_MAX_ROTATION * B3_MAX_ROTATION)
 
-// The maximum linear position correction used when solving constraints. This helps to
+// The maximum position correction used when solving constraints. This helps to
 // prevent overshoot.
 #define B3_MAX_LINEAR_CORRECTION (0.2f)
-
-// The maximum angular position correction used when solving constraints. This helps to
-// prevent overshoot.
 #define B3_MAX_ANGULAR_CORRECTION (8.0f / 180.0f * B3_PI)
 
 // This controls how faster overlaps should be resolved per step.
 // This is less than and would be close to 1, so that the all overlap is resolved per step.
 // However values very close to 1 may lead to overshoot.
-#define B3_BAUMGARTE (0.2f)
+#define B3_BAUMGARTE (0.1f)
 
 // If the relative velocity of a contact point is below 
 // the threshold then restitution is not applied.
 #define B3_VELOCITY_THRESHOLD (1.0f)
 
 // Sleep
-
-#define B3_TIME_TO_SLEEP (0.2f )
+#define B3_TIME_TO_SLEEP (0.2f)
 #define B3_SLEEP_LINEAR_TOL (0.05f)
 #define B3_SLEEP_ANGULAR_TOL (2.0f / 180.0f * B3_PI)
 
@@ -128,6 +116,8 @@ typedef float float32;
 #define B3_MiB(n) (1024 * B3_KiB(n))
 #define B3_GiB(n) (1024 * B3_MiB(n))
 
+#define B3_FORCE_INLINE __forceinline
+
 #define B3_PROFILE(name) b3ProfileScope scope(name)
 
 // You should implement this function to use your own memory allocator.

include/bounce/common/template/array.h

@@ -50,12 +50,12 @@ public:
 		return m_elements + i;
 	}
 
-	const T* Elements() const
+	const T* Begin() const
 	{
 		return m_elements;
 	}
 
-	T* Elements()
+	T* Begin()
 	{
 		return m_elements;
 	}