(maybe) ironed out solvers but need to figure out the weak resultant force problem......

bin/data/entities/player.json

@@ -77,18 +77,18 @@
 			"gravity": [ 0, -9.81, 0 ],
 			"inertia": [ 0, 0, 0 ],
 
-		//	"type": "sphere",
-		//	"radius": 2,
+			"type": "sphere",
+			"radius": 2,
 
-			"type": "capsule",
-			"radius": 1,
-			"height": 2,
+		//	"type": "capsule",
+		//	"radius": 1,
+		//	"height": 2,
 			
 		//	"type": "bounding box",
 		//	"min": [ -1.0,  -2.0, -1.0 ],
 		//	"max": [  1.0,  0.0,  1.0 ],
 		
-			"mass": 100,
+			"mass": 1,
 			"friction": 0.95,
 			"restitution": 0.0,
 			

bin/data/scenes/sourceengine/test_grid.json

@@ -7,9 +7,9 @@
 	"metadata": {
 		"graph": {
 			"tags": {
-				" worldspawn": {
-				//	"physics": { "type": "bounding box", "static": true },
-					"physics": { "type": "plane", "static": true, "direction": [ 0, 1, 0 ], "offset": 0 },
+				"worldspawn": {
+					"physics": { "type": "bounding box", "static": true },
+				//	"physics": { "type": "plane", "static": true, "direction": [ 0, 1, 0 ], "offset": 0 },
 					"grid": { "size": [8,1,8], "epsilon": 0.001, "cleanup": true, "print": true, "clip": true },
 					"optimize meshlets": { "simplify": 0.125, "print": false },
 					"unwrap mesh": true

engine/inc/uf/utils/math/matrix.h

@@ -38,6 +38,11 @@ namespace pod {
 		bool operator==( const Matrix<T,R,C>& matrix ) const; 			// 	Equality check between two matrices (equals)
 		bool operator!=( const Matrix<T,R,C>& matrix ) const; 			// 	Equality check between two matrices (not equals)
 	};
+
+	template<typename T = pod::Math::num_t> using Matrix2t = Matrix<T,2>;
+	typedef Matrix2t<> Matrix2;
+	typedef Matrix2t<float> Matrix2f;
+
 	template<typename T = pod::Math::num_t> using Matrix3t = Matrix<T,3>;
 	typedef Matrix3t<> Matrix3;
 	typedef Matrix3t<float> Matrix3f;
@@ -62,14 +67,20 @@ namespace uf {
 		template<typename T=pod::Matrix4> bool /*UF_API*/ equals( const T& left, const T& right ); 				// 	Equality check between two matrices (equals)
 		template<typename T=pod::Matrix4> bool /*UF_API*/ equals( const T& left, const T& right, float eps ); 	// 	Equality check between two matrices (equals)
 	// 	Basic arithmetic
-	//	template<typename T=pod::Matrix4> pod::Matrix<typename T::type_t, C, C> /*UF_API*/ multiply( const T& left, const T& right );						// 	Multiplies two matrices of same type and size together
-		template<typename T, typename U> pod::Matrix<typename T::type_t, T::columns, T::columns> multiply( const T& left, const U& right );						// 	Multiplies two matrices of same type and size together
-		template<typename T> pod::Matrix<T,4,4> multiply( const pod::Matrix<T,4,4>& left, const pod::Matrix<T,4,4>& right );						// 	Multiplies two matrices of same type and size together
-		template<typename T=pod::Matrix4> T /*UF_API*/ transpose( const T& matrix );										// 	Flip sign of all components
-		template<typename T=pod::Matrix4> T /*UF_API*/ inverse( const T& matrix );										// 	Flip sign of all components
+	//	template<typename T=pod::Matrix4> pod::Matrix<typename T::type_t, C, C> /*UF_API*/ multiply( const T& left, const T& right );			// 	Multiplies two matrices of same type and size together
+		template<typename T, typename U> pod::Matrix<typename T::type_t, T::columns, T::columns> multiply( const T& left, const U& right );		// 	Multiplies two matrices of same type and size together
+		template<typename T> pod::Matrix4t<T> multiply( const pod::Matrix4t<T>& left, const pod::Matrix4t<T>& right );							// 	Multiplies two matrices of same type and size together
+		template<typename T=pod::Matrix4> T /*UF_API*/ transpose( const T& matrix );
+		
+		template<typename T=pod::Math::num_t> pod::Matrix2t<T> inverse(const pod::Matrix2t<T>& mat );
+		template<typename T=pod::Math::num_t> pod::Matrix3t<T> inverse(const pod::Matrix3t<T>& mat );
+		template<typename T=pod::Math::num_t> pod::Matrix4t<T> inverse(const pod::Matrix4t<T>& mat );
+
+		template<typename T=pod::Math::num_t> pod::Vector2t<T> multiply(const pod::Matrix2t<T>& mat, const pod::Vector2t<T>& v );
+		template<typename T=pod::Math::num_t> pod::Vector3t<T> multiply(const pod::Matrix3t<T>& mat, const pod::Vector3t<T>& v );
 		template<typename T=pod::Math::num_t> pod::Vector3t<T> multiply( const pod::Matrix4t<T>& mat, const pod::Vector3t<T>& vector, T w = 1, bool = false );
 		template<typename T=pod::Math::num_t> pod::Vector4t<T> multiply( const pod::Matrix4t<T>& mat, const pod::Vector4t<T>& vector, bool = false );
-		
+
 		template<typename T=pod::Matrix4> T /*UF_API*/ multiplyAll( const T& matrix, typename T::type_t scalar );
 		template<typename T=pod::Matrix4> T /*UF_API*/ add( const T& lhs, const T& rhs );
 	// 	Writes to first value

engine/inc/uf/utils/math/matrix/pod.inl

@@ -317,125 +317,141 @@ template<typename T> T uf::matrix::transpose( const T& matrix ) {
 
 	return transpose;
 }
-// 	Flip sign of all components
-template<typename T> T uf::matrix::inverse( const T& matrix ) {
-	if ( T::rows != 4 || T::columns != 4 ) return matrix;
+//
+template<typename T> pod::Matrix2t<T> uf::matrix::inverse( const pod::Matrix2t<T>& m ) {
+    T det = m[0] * m[3] - m[1] * m[2];
+    if ( std::fabs(det) < 1e-12f ) return m;
 
-	const typename T::type_t* m = &matrix[0];
-	ALIGN16 typename T::type_t inv[16];
-	typename T::type_t det;
-	uint_fast8_t i;
+    T invDet = 1 / det;
+    
+    return pod::Matrix2t<T>{
+		 m[3] * invDet, -m[1] * invDet,
+		-m[2] * invDet, m[0] * invDet,
+    };
+}
 
-	inv[0] = m[5]  * m[10] * m[15] - 
-			 m[5]  * m[11] * m[14] - 
-			 m[9]  * m[6]  * m[15] + 
-			 m[9]  * m[7]  * m[14] +
-			 m[13] * m[6]  * m[11] - 
-			 m[13] * m[7]  * m[10];
-	inv[4] = -m[4]  * m[10] * m[15] + 
-			  m[4]  * m[11] * m[14] + 
-			  m[8]  * m[6]  * m[15] - 
-			  m[8]  * m[7]  * m[14] - 
-			  m[12] * m[6]  * m[11] + 
-			  m[12] * m[7]  * m[10];
-	inv[8] = m[4]  * m[9] * m[15] - 
-			 m[4]  * m[11] * m[13] - 
-			 m[8]  * m[5] * m[15] + 
-			 m[8]  * m[7] * m[13] + 
-			 m[12] * m[5] * m[11] - 
-			 m[12] * m[7] * m[9];
-	inv[12] = -m[4]  * m[9] * m[14] + 
-			   m[4]  * m[10] * m[13] +
-			   m[8]  * m[5] * m[14] - 
-			   m[8]  * m[6] * m[13] - 
-			   m[12] * m[5] * m[10] + 
-			   m[12] * m[6] * m[9];
-	inv[1] = -m[1]  * m[10] * m[15] + 
-			  m[1]  * m[11] * m[14] + 
-			  m[9]  * m[2] * m[15] - 
-			  m[9]  * m[3] * m[14] - 
-			  m[13] * m[2] * m[11] + 
-			  m[13] * m[3] * m[10];
-	inv[5] = m[0]  * m[10] * m[15] - 
-			 m[0]  * m[11] * m[14] - 
-			 m[8]  * m[2] * m[15] + 
-			 m[8]  * m[3] * m[14] + 
-			 m[12] * m[2] * m[11] - 
-			 m[12] * m[3] * m[10];
-	inv[9] = -m[0]  * m[9] * m[15] + 
-			  m[0]  * m[11] * m[13] + 
-			  m[8]  * m[1] * m[15] - 
-			  m[8]  * m[3] * m[13] - 
-			  m[12] * m[1] * m[11] + 
-			  m[12] * m[3] * m[9];
-	inv[13] = m[0]  * m[9] * m[14] - 
-			  m[0]  * m[10] * m[13] - 
-			  m[8]  * m[1] * m[14] + 
-			  m[8]  * m[2] * m[13] + 
-			  m[12] * m[1] * m[10] - 
-			  m[12] * m[2] * m[9];
-	inv[2] = m[1]  * m[6] * m[15] - 
-			 m[1]  * m[7] * m[14] - 
-			 m[5]  * m[2] * m[15] + 
-			 m[5]  * m[3] * m[14] + 
-			 m[13] * m[2] * m[7] - 
-			 m[13] * m[3] * m[6];
-	inv[6] = -m[0]  * m[6] * m[15] + 
-			  m[0]  * m[7] * m[14] + 
-			  m[4]  * m[2] * m[15] - 
-			  m[4]  * m[3] * m[14] - 
-			  m[12] * m[2] * m[7] + 
-			  m[12] * m[3] * m[6];
-	inv[10] = m[0]  * m[5] * m[15] - 
-			  m[0]  * m[7] * m[13] - 
-			  m[4]  * m[1] * m[15] + 
-			  m[4]  * m[3] * m[13] + 
-			  m[12] * m[1] * m[7] - 
-			  m[12] * m[3] * m[5];
-	inv[14] = -m[0]  * m[5] * m[14] + 
-			   m[0]  * m[6] * m[13] + 
-			   m[4]  * m[1] * m[14] - 
-			   m[4]  * m[2] * m[13] - 
-			   m[12] * m[1] * m[6] + 
-			   m[12] * m[2] * m[5];
-	inv[3] = -m[1] * m[6] * m[11] + 
-			  m[1] * m[7] * m[10] + 
-			  m[5] * m[2] * m[11] - 
-			  m[5] * m[3] * m[10] - 
-			  m[9] * m[2] * m[7] + 
-			  m[9] * m[3] * m[6];
-	inv[7] = m[0] * m[6] * m[11] - 
-			 m[0] * m[7] * m[10] - 
-			 m[4] * m[2] * m[11] + 
-			 m[4] * m[3] * m[10] + 
-			 m[8] * m[2] * m[7] - 
-			 m[8] * m[3] * m[6];
-	inv[11] = -m[0] * m[5] * m[11] + 
-			   m[0] * m[7] * m[9] + 
-			   m[4] * m[1] * m[11] - 
-			   m[4] * m[3] * m[9] - 
-			   m[8] * m[1] * m[7] + 
-			   m[8] * m[3] * m[5];
-	inv[15] = m[0] * m[5] * m[10] - 
-			  m[0] * m[6] * m[9] - 
-			  m[4] * m[1] * m[10] + 
-			  m[4] * m[2] * m[9] + 
-			  m[8] * m[1] * m[6] - 
-			  m[8] * m[2] * m[5];
+template<typename T> pod::Matrix3t<T> uf::matrix::inverse( const pod::Matrix3t<T>& m ) {
+// matrix elements
+    const T* a = &m[0];
+    T det = a[0]*(a[4]*a[8] - a[5]*a[7])
+          - a[1]*(a[3]*a[8] - a[5]*a[6])
+          + a[2]*(a[3]*a[7] - a[4]*a[6]);
 
-	det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
-	if (det == 0) return matrix;
-	det = 1.0 / det;
-	ALIGN16 T inverted;
-	#pragma unroll // GCC unroll 16
-	for ( i = 0; i < 16; ++i )
-		inverted[i] = inv[i] * det;
+    if (std::fabs(det) < 1e-12f) return m; // singular
 
-	return inverted;
+    T invDet = static_cast<T>(1) / det;
+
+    return pod::Matrix3t<T>{
+		(a[4]*a[8] - a[5]*a[7]) * invDet,
+		(a[2]*a[7] - a[1]*a[8]) * invDet,
+		(a[1]*a[5] - a[2]*a[4]) * invDet,
+
+		(a[5]*a[6] - a[3]*a[8]) * invDet,
+		(a[0]*a[8] - a[2]*a[6]) * invDet,
+		(a[2]*a[3] - a[0]*a[5]) * invDet,
+
+		(a[3]*a[7] - a[4]*a[6]) * invDet,
+		(a[1]*a[6] - a[0]*a[7]) * invDet,
+		(a[0]*a[4] - a[1]*a[3]) * invDet,
+    };
+}
+
+template<typename T> pod::Matrix4t<T> uf::matrix::inverse( const pod::Matrix4t<T>& m ) {
+	const T* a = &m[0];
+    ALIGN16 pod::Matrix4t<T> inv;
+
+    inv[0]  =   a[5] * (a[10]*a[15] - a[11]*a[14]) -
+                a[9] * (a[6]*a[15]  - a[7]*a[14]) +
+                a[13]* (a[6]*a[11]  - a[7]*a[10]);
+
+    inv[4]  = - a[4] * (a[10]*a[15] - a[11]*a[14]) +
+                a[8] * (a[6]*a[15]  - a[7]*a[14]) -
+                a[12]* (a[6]*a[11]  - a[7]*a[10]);
+
+    inv[8]  =   a[4] * (a[9]*a[15]  - a[11]*a[13]) -
+                a[8] * (a[5]*a[15]  - a[7]*a[13]) +
+                a[12]* (a[5]*a[11]  - a[7]*a[9]);
+
+    inv[12] = - a[4] * (a[9]*a[14]  - a[10]*a[13]) +
+                a[8] * (a[5]*a[14]  - a[6]*a[13]) -
+                a[12]* (a[5]*a[10]  - a[6]*a[9]);
+
+    inv[1]  = - a[1] * (a[10]*a[15] - a[11]*a[14]) +
+                a[9] * (a[2]*a[15]  - a[3]*a[14]) -
+                a[13]* (a[2]*a[11]  - a[3]*a[10]);
+
+    inv[5]  =   a[0] * (a[10]*a[15] - a[11]*a[14]) -
+                a[8] * (a[2]*a[15]  - a[3]*a[14]) +
+                a[12]* (a[2]*a[11]  - a[3]*a[10]);
+
+    inv[9]  = - a[0] * (a[9]*a[15]  - a[11]*a[13]) +
+                a[8] * (a[1]*a[15]  - a[3]*a[13]) -
+                a[12]* (a[1]*a[11]  - a[3]*a[9]);
+
+    inv[13] =   a[0] * (a[9]*a[14]  - a[10]*a[13]) -
+                a[8] * (a[1]*a[14]  - a[2]*a[13]) +
+                a[12]* (a[1]*a[10]  - a[2]*a[9]);
+
+    inv[2]  =   a[1] * (a[6]*a[15]  - a[7]*a[14]) -
+                a[5] * (a[2]*a[15]  - a[3]*a[14]) +
+                a[13]* (a[2]*a[7]   - a[3]*a[6]);
+
+    inv[6]  = - a[0] * (a[6]*a[15]  - a[7]*a[14]) +
+                a[4] * (a[2]*a[15]  - a[3]*a[14]) -
+                a[12]* (a[2]*a[7]   - a[3]*a[6]);
+
+    inv[10] =   a[0] * (a[5]*a[15]  - a[7]*a[13]) -
+                a[4] * (a[1]*a[15]  - a[3]*a[13]) +
+                a[12]* (a[1]*a[7]   - a[3]*a[5]);
+
+    inv[14] = - a[0] * (a[5]*a[14]  - a[6]*a[13]) +
+                a[4] * (a[1]*a[14]  - a[2]*a[13]) -
+                a[12]* (a[1]*a[6]   - a[2]*a[5]);
+
+    inv[3]  = - a[1] * (a[6]*a[11]  - a[7]*a[10]) +
+                a[5] * (a[2]*a[11]  - a[3]*a[10]) -
+                a[9] * (a[2]*a[7]   - a[3]*a[6]);
+
+    inv[7]  =   a[0] * (a[6]*a[11]  - a[7]*a[10]) -
+                a[4] * (a[2]*a[11]  - a[3]*a[10]) +
+                a[8] * (a[2]*a[7]   - a[3]*a[6]);
+
+    inv[11] = - a[0] * (a[5]*a[11]  - a[7]*a[9]) +
+                a[4] * (a[1]*a[11]  - a[3]*a[9]) -
+                a[8] * (a[1]*a[7]   - a[3]*a[5]);
+
+    inv[15] =   a[0] * (a[5]*a[10]  - a[6]*a[9]) -
+                a[4] * (a[1]*a[10]  - a[2]*a[9]) +
+                a[8] * (a[1]*a[6]   - a[2]*a[5]);
+
+    // determinant
+    T det = a[0]*inv[0] + a[1] * inv[4] + a[2] * inv[8] + a[3] * inv[12];
+    if ( std::fabs(det) < 1e-12f ) return m; // singular
+
+    T invDet = 1 / det;
+    for (int i = 0; i < 16; ++i ) inv[i] *= invDet;
+
+    return inv;
 }
 template<typename T> pod::Vector3t<T> uf::matrix::multiply( const pod::Matrix4t<T>& mat, const pod::Vector3t<T>& vector, T w, bool div ) {
 	return uf::matrix::multiply( mat, pod::Vector4t<T>{ vector[0], vector[1], vector[2], w }, div );
 }
+template<typename T>
+pod::Vector2t<T> uf::matrix::multiply(const pod::Matrix2t<T>& mat, const pod::Vector2t<T>& v ) {
+	return pod::Vector2t<T>{
+		v[0]* mat[0] + v[1]* mat[2],
+		v[0]* mat[1] + v[1]* mat[3],
+	};
+}
+
+template<typename T>
+pod::Vector3t<T> uf::matrix::multiply(const pod::Matrix3t<T>& mat, const pod::Vector3t<T>& v ) {
+	return pod::Vector3t<T>{
+		v[0]* mat[0] + v[1]* mat[3] + v[2] * mat[6],
+		v[0]* mat[1] + v[1]* mat[4] + v[2] * mat[7],
+		v[0]* mat[2] + v[1]* mat[5] + v[2] * mat[8],
+	};
+}
 template<typename T> pod::Vector4t<T> uf::matrix::multiply( const pod::Matrix4t<T>& mat, const pod::Vector4t<T>& vector, bool div ) {
 #if UF_ENV_DREAMCAST
 	MATH_Load_XMTRX( (ALL_FLOATS_STRUCT*) &mat[0] );
@@ -455,8 +471,6 @@ template<typename T> pod::Vector4t<T> uf::matrix::multiply( const pod::Matrix4t<
 #endif
 }
 // 	Writes to first value
-// 	Multiplies two matrices of same type and size together
-// 	Flip sign of all components
 template<typename T> T& uf::matrix::invert( T& matrix ) {
 	return matrix = uf::matrix::inverse((const T&) matrix);
 }
@@ -542,11 +556,11 @@ template<typename T>
 pod::Matrix4t<T> /*UF_API*/ uf::matrix::orthographic( T l, T r, T b, T t, T f, T n ) {
 	ALIGN16 pod::Matrix4t<T> m = uf::matrix::identity();
 	m[0*4+0] = 2 / (r - l);
-    m[1*4+1] = 2 / (t - b);
-    m[2*4+2] = - 2 / (f - n);
-    m[3*4+0] = - (r + l) / (r - l);
-    m[3*4+1] = - (t + b) / (t - b);
-    m[3*4+2] = - (f + n) / (f - n);
+	m[1*4+1] = 2 / (t - b);
+	m[2*4+2] = - 2 / (f - n);
+	m[3*4+0] = - (r + l) / (r - l);
+	m[3*4+1] = - (t + b) / (t - b);
+	m[3*4+2] = - (f + n) / (f - n);
 	return m;
 /*
 	uf::stl::vector<T> m = {
@@ -575,15 +589,15 @@ pod::Matrix4t<T> /*UF_API*/ uf::matrix::perspective( T fov, T raidou, T znear, T
 		return pod::Matrix4t<T>({
 			f / raidou, 	0, 	 	0, 		0,
 			0, 			 	f, 	 	0, 		0,
-			0,       		0,    	0, 		1,
-			0,       		0,   znear, 	0
+			0,	   		0,		0, 		1,
+			0,	   		0,   znear, 	0
 		});
 	#elif UF_USE_VULKAN
 		return pod::Matrix4t<T>({
 			f / raidou, 	0, 	 	0, 		0,
 			0, 				-f, 	0, 		0,
-			0,       		0,    	0, 		1,
-			0,       		0,   znear, 	0
+			0,	   		0,		0, 		1,
+			0,	   		0,   znear, 	0
 		});
 	#endif
 	} else {

engine/inc/uf/utils/math/physics/impl.h

@@ -22,22 +22,21 @@ namespace pod {
 		SPHERE,
 		PLANE,
 		CAPSULE,
+		TRIANGLE,
 		MESH,
 	};
 
 	struct SupportPoint {
-		pod::Vector3f p;   // Minkowski difference point
-		pod::Vector3f pA;  // original support point in A
-		pod::Vector3f pB;  // original support point in B
+		pod::Vector3f p;
+		pod::Vector3f pA;
+		pod::Vector3f pB;
 	};
 
 	struct Simplex {
-		//uf::stl::vector<pod::Vector3f> pts;
 		uf::stl::vector<pod::SupportPoint> pts;
 	};
 
 	struct Face {
-	//	pod::Vector3f a,b,c;
 		pod::SupportPoint a, b, c;
 		pod::Vector3f normal;
 		float distance;
@@ -56,7 +55,7 @@ namespace pod {
 		uf::stl::vector<pod::BVH::Node> nodes;
 	};
 
-	struct MeshCollider {
+	struct MeshBVH {
 		pod::BVH* bvh;
 		const uf::Mesh* mesh;
 	};
@@ -71,7 +70,8 @@ namespace pod {
 			pod::AABB aabb;
 			pod::Plane plane;
 			pod::Capsule capsule;
-			pod::MeshCollider mesh;
+			pod::TriangleWithNormal triangle;
+			pod::MeshBVH mesh;
 		} u;
 	};
 

engine/inc/uf/utils/math/shapes.h

@@ -32,8 +32,8 @@ namespace pod {
 		pod::Vector3f points[3];
 	};
 
-	struct TriangleWithNormal : Triangle { // to-do: find a better name
-	    pod::Vector3f normals[3];
+	struct TriangleWithNormal : Triangle {
+		pod::Vector3f normals[3];
 	};
 
 	template<typename T>

engine/inc/uf/utils/tests/tests.h

@@ -26,6 +26,34 @@
 		UF_MSG_DEBUG("[{}] {}", test.passes ? "PASS" : "FAIL", test.message);\
 	}
 
+	#define EXPECT_GT(a,b) { \
+		auto& test = uf::unitTests::tests[uf::unitTests::current];\
+		test.passes = (((a) > (b)));\
+		test.message = ::fmt::format("EXPECT_GT({}, {}), ({} > {})", #a, #b, a, b);\
+		UF_MSG_DEBUG("[{}] {}", test.passes ? "PASS" : "FAIL", test.message);\
+	}
+
+	#define EXPECT_LT(a,b) { \
+		auto& test = uf::unitTests::tests[uf::unitTests::current];\
+		test.passes = (((a) < (b)));\
+		test.message = ::fmt::format("EXPECT_LT({}, {}), ({} < {})", #a, #b, a, b);\
+		UF_MSG_DEBUG("[{}] {}", test.passes ? "PASS" : "FAIL", test.message);\
+	}
+
+	#define EXPECT_GE(a,b) { \
+		auto& test = uf::unitTests::tests[uf::unitTests::current];\
+		test.passes = (((a) >= (b)));\
+		test.message = ::fmt::format("EXPECT_GE({}, {}), ({} >= {})", #a, #b, a, b);\
+		UF_MSG_DEBUG("[{}] {}", test.passes ? "PASS" : "FAIL", test.message);\
+	}
+
+	#define EXPECT_LE(a,b) { \
+		auto& test = uf::unitTests::tests[uf::unitTests::current];\
+		test.passes = (((a) <= (b)));\
+		test.message = ::fmt::format("EXPECT_LE({}, {}), ({} <= {})", #a, #b, a, b);\
+		UF_MSG_DEBUG("[{}] {}", test.passes ? "PASS" : "FAIL", test.message);\
+	}
+
 	#define EXPECT_NEAR(a,b,eps) { \
 		auto& test = uf::unitTests::tests[uf::unitTests::current];\
 		test.passes = (std::fabs((a)-(b)) <= (eps));\

engine/src/engine/ext/player/behavior.cpp

@@ -297,7 +297,7 @@ void ext::PlayerBehavior::tick( uf::Object& self ) {
 		if ( query.hit ) {
 			UF_MSG_DEBUG("{}: {} | {}", query.contact.penetration, uf::string::toString(*query.body->object), uf::vector::toString(physics.velocity));
 			stats.floored = true;
-			// if ( physics.velocity.y < 0.0f ) physics.velocity.y = 0.0f;
+			if ( physics.velocity.y < 0.0f ) physics.velocity.y = 0.0f;
 		}
 	#endif
 	}

engine/src/utils/math/physics/epa.inl

@@ -77,7 +77,7 @@ namespace {
 		}
 	}
 
-	pod::Contact epa( const pod::RigidBody& A, const pod::RigidBody& B, const pod::Simplex& simplex, int maxIterations = 64, float eps = EPS(1.0e-4f) ) {
+	pod::Contact epa( const pod::RigidBody& a, const pod::RigidBody& b, const pod::Simplex& simplex, int maxIterations = 64, float eps = EPS(1.0e-4f) ) {
 		UF_ASSERT( ::isValidSimplex(simplex) );
 
 		auto faces = ::initialPolytope(simplex);
@@ -98,7 +98,7 @@ namespace {
 			auto& f = faces[idx];
 
 			// new support
-			auto sp = ::supportMinkowskiDetailed( A, B, f.normal );
+			auto sp = ::supportMinkowskiDetailed( a, b, f.normal );
 			float d = uf::vector::dot( sp.p, f.normal );
 
 			// convergence check
@@ -120,11 +120,6 @@ namespace {
 					penetration = -penetration;
 				}
 
-				// discard invalid contact
-				if ( !std::isfinite(contact.x) || !std::isfinite(contact.y) || !std::isfinite(contact.z) || penetration <= 0.0f ) {
-					return {};
-				}
-
 				return { contact, normal, penetration };
 			}
 

engine/src/utils/math/physics/gjk.inl

@@ -15,14 +15,21 @@ namespace {
 				auto up = uf::quaternion::rotate( body.transform->orientation, pod::Vector3f{0,1,0} );
 				auto p1 = body.transform->position + up * body.collider.u.capsule.halfHeight;
 				auto p2 = body.transform->position - up * body.collider.u.capsule.halfHeight;
-				// get closest end
-				auto end = ( uf::vector::dot( dir, p1 ) > uf::vector::dot( dir, p2 ) ) ? p1 : p2;
-
+				auto end = ( uf::vector::dot( dir, p1 ) > uf::vector::dot( dir, p2 ) ) ? p1 : p2; // get closest end
 				return end + uf::vector::normalize( dir ) * body.collider.u.capsule.radius;
 			}
-			// to-do: mesh
-			default: {
+			case pod::ShapeType::TRIANGLE: {
+				const auto& tri = body.collider.u.triangle;
+			    float d0 = uf::vector::dot( tri.points[0], dir );
+			    float d1 = uf::vector::dot( tri.points[1], dir );
+			    float d2 = uf::vector::dot( tri.points[2], dir );
 
+			    if ( d0 > d1 && d0 > d2 ) return tri.points[0];
+			    if ( d1 > d2 ) return tri.points[1];
+			    return tri.points[2];
+			} break;
+
+			default: {
 			} break;
 		}
 		return {};

engine/src/utils/math/physics/helpers.inl

@@ -30,10 +30,16 @@ namespace {
 	bool meshCapsule( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) );
 	bool meshMesh( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) );
 
+	bool triangleTriangle( const pod::TriangleWithNormal& a, const pod::TriangleWithNormal& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) );
+	bool triangleAabb( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps = EPS(1.0e-6f) );
+	bool triangleSphere( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps = EPS(1.0e-6f) );
+	bool trianglePlane( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps = EPS(1.0e-6f) );
+	bool triangleCapsule( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps = EPS(1.0e-6f) );
+
 	bool aabbOverlap( const pod::AABB& a, const pod::AABB& b, float eps = EPS(1.0e-6f) );
 	pod::AABB computeTriangleAABB( const pod::Triangle& tri );
 
-	void reduceContacts( pod::Manifold& manifold );
+	void solveContacts( uf::stl::vector<pod::Manifold>& manifolds, float dt );
 
 	void traverseNodePair( const pod::BVH& bvh, int leftID, int rightID, pod::BVH::pair_t& pairs );
 	void traverseNodePair( const pod::BVH& a, int nodeA, const pod::BVH& b, int nodeB, pod::BVH::pair_t& out );

engine/src/utils/math/physics/impl.cpp

@@ -8,12 +8,12 @@ namespace {
 	bool warmupSolver = false;
 	bool blockContactSolver = false;
 	bool useGjk = false;
-	bool fixedStep = true;
+	bool fixedStep = false;
 	int substeps = 0;
 
-	int solverIterations = 20;
-	float baumgarteCorrectionPercent = 0.4f;
-	float baumgarteCorrectionSlop = 0.01f;
+	int solverIterations = 10;
+	float baumgarteCorrectionPercent = 0.2f;
+	float baumgarteCorrectionSlop = 0.001f;
 	uf::stl::unordered_map<size_t, pod::Manifold> manifoldsCache;
 }
 
@@ -26,12 +26,14 @@ namespace {
 #include "sphere.inl"
 #include "plane.inl"
 #include "capsule.inl"
+#include "triangle.inl"
 #include "mesh.inl"
 #include "ray.inl"
 #include "bvh.inl"
 #include "gjk.inl"
 #include "epa.inl"
 #include "integration.inl"
+#include "solvers.inl"
 
 // unused
 float uf::physics::impl::timescale = 1.0f / 60.0f;
@@ -120,6 +122,10 @@ void uf::physics::impl::step( pod::World& world, float dt ) {
 			}
 			// retrieve accumulated impulses
 			if ( ::warmupSolver )  ::retrieveContacts( manifold, ::manifoldsCache[::makePairKey( a, b )] );
+			// merge similar contacts from a mesh to ensure continuity
+			if ( a.collider.type == pod::ShapeType::MESH || b.collider.type == pod::ShapeType::MESH ) {
+				::mergeContacts( manifold );
+			}
 			// keep four most important contacts
 			::reduceContacts( manifold );
 		#if 0
@@ -128,6 +134,8 @@ void uf::physics::impl::step( pod::World& world, float dt ) {
 				UF_MSG_DEBUG("contact={}, normal={}, depth={}", uf::vector::toString( contact.point ), uf::vector::toString( contact.normal ), contact.penetration );
 			}
 		#endif
+			// no points remained, skip
+			if ( manifold.points.empty() ) continue;
 
 			// store manifold
 			manifolds.emplace_back(manifold);

engine/src/utils/math/physics/integration.inl

@@ -104,31 +104,62 @@ namespace {
 		return false;
 	}
 
-	bool similarContact( const pod::Contact& a, const pod::Contact& b, float eps = 1.0e-3f, float distSq = 0.95f ) {
-		return uf::vector::distanceSquared(a.point, b.point) < eps && uf::vector::dot(a.normal, b.normal) > distSq;
+	bool similarContact( const pod::Contact& a, const pod::Contact& b, float distSq = 1.0e-2f, float norm = 0.9f ) {
+		return uf::vector::distanceSquared(a.point, b.point) < distSq && uf::vector::dot(a.normal, b.normal) > norm;
 	}
 
 	void reduceContacts( pod::Manifold& manifold ) {
 		if ( manifold.points.size() <= 4 ) return;
 
-		uf::stl::vector<pod::Contact> reduced;
+		uf::stl::vector<pod::Contact> result;
 		for ( auto& c : manifold.points ) {
+			// prune invalid contacts
+			if ( !uf::vector::isValid( c.point ) ) continue;
+
 			bool merged = false;
-			for ( auto& r : reduced ) {
+			for ( auto& r : result ) {
 				if ( !::similarContact( c, r ) ) continue;
 				// merge, pick deeper penetration
-				if (c.penetration > r.penetration) r = c;
+				if ( c.penetration > r.penetration ) r = c;
 				merged = true;
 				break;
 			}
-			if ( !merged ) reduced.emplace_back(c);
+			if ( !merged ) result.emplace_back(c);
 		}
+		
+		UF_MSG_DEBUG("Reduced {} => {} contacts", manifold.points.size(), result.size());
 
 		// keep only deepest + farthest up to 4
-		std::sort(reduced.begin(), reduced.end(), [](auto& a, auto& b){ return a.penetration > b.penetration; });
-		if (reduced.size() > 4) reduced.resize(4);
+		std::sort(result.begin(), result.end(), [](auto& a, auto& b){ return a.penetration > b.penetration; });
+		if ( result.size() > 4 ) result.resize(4);
 
-		manifold.points = reduced;
+		manifold.points = result;
+	}
+
+	void mergeContacts( pod::Manifold& manifold ) {
+		uf::stl::vector<pod::Contact> result;
+		
+		for ( auto& c : manifold.points ) {
+			bool merged = false;
+			for ( auto& r : result ) {
+				if ( !::similarContact( c, r ) ) continue;
+				// merge: average position + normal, keep max penetration
+				r.point  = ( r.point + c.point ) * 0.5f;
+				r.normal = uf::vector::normalize( r.normal + c.normal );
+				r.penetration = std::max( r.penetration, c.penetration );
+				merged = true;
+				break;
+			}
+			if ( !merged ) result.emplace_back( c );
+		}
+		
+		UF_MSG_DEBUG("Merged {} => {} contacts", manifold.points.size(), result.size());
+
+		for ( auto& c : result ) {
+			UF_MSG_DEBUG("contact={}, normal={}, depth={}", uf::vector::toString( c.point ), uf::vector::toString( c.normal ), c.penetration );
+		}
+
+		manifold.points = result;
 	}
 
 	void retrieveContacts( pod::Manifold& current, const pod::Manifold& previous, float decay = 0.35f ) {
@@ -163,6 +194,8 @@ namespace {
 
 	// baumgarte position correction
 	void positionCorrection( pod::RigidBody& a, pod::RigidBody& b, const pod::Contact& contact ) {
+		if ( ::baumgarteCorrectionPercent <= 0 ) return;
+
 		float correctionMagnitude = std::max(contact.penetration - ::baumgarteCorrectionSlop, 0.0f) / (a.inverseMass + b.inverseMass) * ::baumgarteCorrectionPercent;
 		pod::Vector3f correction = contact.normal * correctionMagnitude;
 
@@ -170,108 +203,6 @@ namespace {
 		if ( !b.isStatic ) b.transform->position += correction * b.inverseMass;
 	}
 
-	void resolveContact( pod::RigidBody& a, pod::RigidBody& b, pod::Contact& contact, float dt ) {
-		// relative positions from centers to contact point
-		pod::Vector3f rA = contact.point - a.transform->position;
-		pod::Vector3f rB = contact.point - b.transform->position;
-
-		// relative velocity at contact
-		pod::Vector3f vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);
-		pod::Vector3f vB = b.velocity + uf::vector::cross(b.angularVelocity, rB);
-		pod::Vector3f rv = vB - vA;
-
-		// normal contact velocity
-		float velAlongNormal = uf::vector::dot(rv, contact.normal);
-		float velTolerance = 0; // -1.0e3f;
-		if ( velAlongNormal > velTolerance ) return; // if separating, no impulse
-
-		// compute restitution (bounce)
-		float e = std::min(a.material.restitution, b.material.restitution);
-
-		// nullify restitution if velocity is small enough
-		if ( fabs(velAlongNormal) < 1.0f) e = 0.0f;
-
-		// effective inverse mass along normal
-		float invMassN = ::computeEffectiveMass(a, b, rA, rB, contact.normal);
-
-		// normal impulse scalar
-		float jn = -(1.0f + e) * velAlongNormal;
-		jn /= invMassN;
-		if ( ::warmupSolver ) {
-			float jnOld = contact.accumulatedNormalImpulse;
-			float jnNew = std::max(0.0f, jnOld + jn);
-			float jnDelta = jnNew - jnOld;
-			contact.accumulatedNormalImpulse = jnNew;
-			jn = jnDelta;
-		}
-
-		pod::Vector3f normalImpulse = contact.normal * jn;
-		::applyImpulseTo(a, b, rA, rB, normalImpulse);
-
-		// tangent direction
-		pod::Vector3f tangent = rv - contact.normal * uf::vector::dot(rv, contact.normal);
-		float tangentMag = uf::vector::magnitude(tangent);
-		if (tangentMag > EPS(1e-6f)) {
-			tangent /= tangentMag;
-
-			// effective mass along tangent
-			float invMassT = ::computeEffectiveMass(a, b, rA, rB, tangent);
-
-			// tangential relative velocity
-			float vt = uf::vector::dot(rv, tangent);
-
-			// required tangential impulse to cancel tangent velocity
-			float jt = -vt / invMassT;
-
-			// friction coefficients
-			float mu_s = std::sqrt(a.material.staticFriction * b.material.staticFriction);
-			float mu_d = std::sqrt(a.material.dynamicFriction * b.material.dynamicFriction);
-			
-			if ( std::fabs(jt) > jn * mu_s) jt = -jn * mu_d; // dynamic friction: resist sliding proportionally
-
-			if ( ::warmupSolver ) {
-				float maxFriction = mu_s * contact.accumulatedNormalImpulse;
-				float jtOld = contact.accumulatedTangentImpulse;
-				float jtNew = std::max(-maxFriction, std::min(jtOld + jt, maxFriction));
-				float jtDelta = jtNew - jtOld;
-				contact.accumulatedTangentImpulse = jtNew;
-				contact.tangent = tangent;
-				jt = jtDelta;
-			}
-
-			pod::Vector3f frictionImpulse = tangent * jt;
-			::applyImpulseTo(a, b, rA, rB, frictionImpulse);
-		}
-
-		::positionCorrection(a, b, contact);
-	}
-
-	void solveContacts( uf::stl::vector<pod::Manifold>& manifolds, float dt ) {
-		if ( ::warmupSolver ) {
-			for ( auto& m : manifolds ) {
-				for ( auto& c : m.points ) {
-					::warmupContacts(*m.a, *m.b, c, dt);
-				}
-			}
-		}
-
-		for ( auto i = 0; i < ::solverIterations; ++i ) {
-			for ( auto& m : manifolds ) {
-			#if 0
-				if ( ::blockContactSolver ) {
-					//::solveManifoldBlockN( m, dt );
-					::solveManifoldBlock2x2( m, dt );
-				} else {
-					for ( auto& c : m.points ) {
-						::resolveContact(*m.a, *m.b, c, dt);
-					}
-				}
-			#endif
-				for ( auto& c : m.points ) ::resolveContact(*m.a, *m.b, c, dt);
-			}
-		}
-	}
-
 	void integrate( pod::RigidBody& body, float dt ) {
 		// only integrate dynamic bodies
 		if ( body.isStatic || body.mass == 0 ) return;

engine/src/utils/math/physics/mesh.inl

@@ -1,121 +1,18 @@
-#define REORIENT_NORMALS_ON_FETCH 0
-#define REORIENT_NORMALS_ON_CONTACT 0
+/*
+auto bounds = ::transformAabbToLocal( aabb.bounds, *mesh.transform );
+*/
+/*
+// transform capsule line segments to local space
+		p1 = uf::transform::applyInverse( *mesh.transform, p1 );
+		p2 = uf::transform::applyInverse( *mesh.transform, p2 );
 
-// mesh BVH
-namespace {
-	// to-do: clean this up
-	uint32_t getIndex( const void* indexData, size_t indexSize, size_t idx ) {
-		if ( indexSize == sizeof(uint8_t) ) {
-			auto* ptr = reinterpret_cast<const uint8_t*>(indexData);
-			return static_cast<uint32_t>(ptr[idx]);
-		} else if ( indexSize == sizeof(uint16_t) ) {
-			auto* ptr = reinterpret_cast<const uint16_t*>(indexData);
-			return static_cast<uint32_t>(ptr[idx]);
-		} else if ( indexSize == sizeof(uint32_t) ) {
-			auto* ptr = reinterpret_cast<const uint32_t*>(indexData);
-			return ptr[idx];
-		}
-		UF_EXCEPTION("Unsupported index type of size {}", indexSize);
-	}
-	pod::AABB computeTriangleAABB( const void* vertices, size_t vertexStride, const void* indexData, size_t indexSize, size_t triID ) {
-		auto triIndexID = triID * 3;
+*/
+/*
 
-		uint32_t i0 = ::getIndex( indexData, indexSize, triIndexID + 0 );
-		uint32_t i1 = ::getIndex( indexData, indexSize, triIndexID + 1 );
-		uint32_t i2 = ::getIndex( indexData, indexSize, triIndexID + 2 );
+contact = uf::transform::apply( *mesh.transform, contact );
+normal  = uf::quaternion::rotate( mesh.transform->orientation, normal );
 
-		auto& v0 = *reinterpret_cast<const pod::Vector3f*>(reinterpret_cast<const uint8_t*>(vertices) + i0 * vertexStride);
-		auto& v1 = *reinterpret_cast<const pod::Vector3f*>(reinterpret_cast<const uint8_t*>(vertices) + i1 * vertexStride);
-		auto& v2 = *reinterpret_cast<const pod::Vector3f*>(reinterpret_cast<const uint8_t*>(vertices) + i2 * vertexStride);
-
-		return {
-			{	
-				std::min({v0.x, v1.x, v2.x}),
-				std::min({v0.y, v1.y, v2.y}),
-				std::min({v0.z, v1.z, v2.z}),
-			},
-			{
-				std::max({v0.x, v1.x, v2.x}),
-				std::max({v0.y, v1.y, v2.y}),
-				std::max({v0.z, v1.z, v2.z}),
-			}
-		};
-	}
-
-	pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID ) {
-		auto views = mesh.makeViews({"position", "normal"});
-		UF_ASSERT(!views.empty());
-
-		uint32_t triIndexID = triID * 3; // remap triangle ID to index ID
-		// find which view contains this triangle index.
-		const uf::Mesh::View* found = nullptr;
-		for ( auto& v : views ) {
-			if ( v.index.first <= triIndexID && triIndexID < v.index.first + v.index.count ) {
-				found = &v;
-				break;
-			}
-		}
-		UF_ASSERT( found );
-
-		pod::TriangleWithNormal tri;
-
-		auto& positions = (*found)["position"];
-		auto& normals   = (*found)["normal"];
-		auto& indices   = (*found)["index"];
-
-		const void* indexBase = indices.data(found->index.first);
-		size_t indexSize	  = mesh.index.size;
-
-		// reset back to local indices range
-		triIndexID -= found->index.first;
-
-		uint32_t idxs[3];
-		// to-do: just make this a macro that could have a parallel hint
-		for ( auto i = 0; i < 3; ++i ) idxs[i] = getIndex(indexBase, indexSize, triIndexID + i);
-
-		{
-			auto* base = reinterpret_cast<const uint8_t*>(positions.data(found->vertex.first));
-			size_t stride = positions.stride();
-
-			for ( auto i = 0; i < 3; ++i ) tri.points[i] = *reinterpret_cast<const pod::Vector3f*>(base + idxs[i] * stride);
-		}
-
-		if ( normals.valid() && false ) {
-			auto* base = reinterpret_cast<const uint8_t*>(normals.data(found->vertex.first));
-			size_t stride = normals.stride();
-			for ( auto i = 0; i < 3; ++i ) tri.normals[i] = *reinterpret_cast<const pod::Vector3f*>(base + idxs[i] * stride);
-		} else {
-			auto normal = uf::vector::normalize(uf::vector::cross(tri.points[1]-tri.points[0], tri.points[2]-tri.points[0]));
-			for ( auto i = 0; i < 3; ++i ) tri.normals[i] = normal;
-		}
-
-		return tri;
-	}
-
-	// if body is a mesh, apply its transform to the triangles, else reorient the normal with respect to the body
-	pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID, const pod::RigidBody& body ) {
-		auto tri = ::fetchTriangle( mesh, triID );
-		if ( body.collider.type == pod::ShapeType::MESH ) {
-			if ( body.transform ) {
-				for ( auto i = 0; i < 3; ++i ) {
-					tri.points[i]  = uf::transform::apply(*body.transform, tri.points[i]);
-					tri.normals[i] = uf::quaternion::rotate(body.transform->orientation, tri.normals[i]);
-				}
-			}
-		}
-		else {
-		#if REORIENT_NORMALS_ON_FETCH
-			auto triCenter = (tri.points[0] + tri.points[1] + tri.points[2]) / 3.0f;
-			auto delta = body.transform->position - triCenter;
-			for ( auto i = 0; i < 3; ++i ) {
-				if ( uf::vector::dot(tri.normals[i], delta) < 0.0f ) tri.normals[i] = -tri.normals[i];
-			}
-		#endif
-		}
-
-		return tri;
-	}
-}
+*/
 
 // 
 namespace {
@@ -128,47 +25,22 @@ namespace {
 		const auto& meshData = *mesh.collider.u.mesh.mesh;
 		const auto& bvh  = *mesh.collider.u.mesh.bvh;
 
+		// transform to local space for BVH query
 		auto bounds = ::transformAabbToLocal( aabb.bounds, *mesh.transform );
-
 		uf::stl::vector<int> candidates;
-		::queryBVH(bvh, bounds, candidates);
+		::queryBVH( bvh, bounds, candidates );
 
 		bool hit = false;
+		// do collision per triangle
 		for ( auto triID : candidates ) {
-			auto tri = ::fetchTriangle( meshData, triID, aabb );
-			auto closestTri = ::closestPointOnTriangle( ::aabbCenter( bounds ), tri );
-			auto closestAabb = ::closestPointOnAABB( closestTri, bounds );
-
-			if ( !uf::vector::isValid( closestTri ) ) {
-				//UF_MSG_DEBUG("tri #{}={}, {}, {}", triID, uf::vector::toString( tri.points[0] ), uf::vector::toString( tri.points[1] ), uf::vector::toString( tri.points[2] ));
-				//UF_MSG_DEBUG("closestTri={}, closestAabb={}", uf::vector::toString( closestTri ), uf::vector::toString( closestAabb ));
-				continue;
-			}
-			
-			auto delta = closestAabb - closestTri; // direction between points
-			float dist2 = uf::vector::magnitude( delta );
-			float tolerance = 1.0e-3; // std::max( 1.0e-3f, uf::vector::magnitude(aabb.velocity) * manifold.dt * 0.5f );
-			if ( dist2 >= tolerance ) continue;
-			float dist = std::sqrt( dist2 );
-
-			auto contact = closestTri; // ( closestAabb + closestTri ) * 0.5f; // center of points
-			auto normal = ::normalizeDelta( delta, dist, eps ); // uf::vector::normalize( ::interpolateWithBarycentric( tri.normals, ::computeBarycentric( contact, tri ) ));
-			float penetration = tolerance - dist;
-
-		#if REORIENT_NORMALS_ON_CONTACT
-			if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal;
-		#endif
-
-			contact = uf::transform::apply( *mesh.transform, contact );
-			normal  = uf::quaternion::rotate( mesh.transform->orientation, normal );
-
-			manifold.points.emplace_back(pod::Contact{ contact, normal, penetration });
+			auto tri = ::fetchTriangle( meshData, triID, mesh ); // transform triangle to world space
+			if ( !::triangleAabb( tri, aabb, manifold, eps ) ) continue;
 			hit = true;
 		}
 		return hit;
 	}
 	bool meshSphere( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) {
-		ASSERT_COLLIDER_TYPES(MESH, SPHERE);
+		ASSERT_COLLIDER_TYPES( MESH, SPHERE );
 
 		const auto& mesh = a;
 		const auto& sphere = b;
@@ -176,42 +48,16 @@ namespace {
 		const auto& meshData = *mesh.collider.u.mesh.mesh;
 		const auto& bvh  = *mesh.collider.u.mesh.bvh;
 
-		auto bounds = ::transformAabbToLocal( ::computeAABB( sphere ), *mesh.transform );
-		
-		auto center = ::aabbCenter( bounds );
-		float r = sphere.collider.u.sphere.radius;
-		
+		// transform to local space for BVH query
+		auto bounds = ::transformAabbToLocal( sphere.bounds, *mesh.transform );		
 		uf::stl::vector<int> candidates;
-		::queryBVH(bvh, bounds, candidates);
+		::queryBVH( bvh, bounds, candidates );
 
 		bool hit = false;
+		// do collision per triangle
 		for ( auto triID : candidates ) {
-			auto tri = ::fetchTriangle( meshData, triID, sphere );
-			auto closest = ::closestPointOnTriangle( ::aabbCenter(bounds), tri );
-
-			if ( !uf::vector::isValid( closest ) ) {
-				//UF_MSG_DEBUG("tri #{}={}, {}, {}", triID, uf::vector::toString( tri.points[0] ), uf::vector::toString( tri.points[1] ), uf::vector::toString( tri.points[2] ));
-				//UF_MSG_DEBUG("closest={}", uf::vector::toString( closest ));
-				continue;
-			}
-
-			auto delta = center - closest;
-			float dist2 = uf::vector::magnitude(delta);
-			if ( dist2 > r * r ) continue;
-			float dist = std::sqrt( dist2 );
-
-			auto contact = closest;
-			auto normal = ::normalizeDelta( delta, dist, eps );
-			float penetration = r - dist;
-
-		#if REORIENT_NORMALS_ON_CONTACT
-			if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal;
-		#endif
-
-			contact = uf::transform::apply(*mesh.transform, contact);
-			normal  = uf::quaternion::rotate(mesh.transform->orientation, normal);
-
-			manifold.points.emplace_back(pod::Contact{ contact, normal, penetration });
+			auto tri = ::fetchTriangle( meshData, triID, mesh ); // transform triangle to world space
+			if ( !::triangleSphere( tri, sphere, manifold, eps ) ) continue;
 			hit = true;
 		}
 
@@ -219,11 +65,31 @@ namespace {
 	}
 	// to-do
 	bool meshPlane( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) {
-		ASSERT_COLLIDER_TYPES(MESH, PLANE);
-		return false;
+		ASSERT_COLLIDER_TYPES( MESH, PLANE );
+
+		const auto& mesh = a;
+		const auto& plane = b;
+
+		const auto& meshData = *mesh.collider.u.mesh.mesh;
+		const auto& bvh  = *mesh.collider.u.mesh.bvh;
+
+		// transform to local space for BVH query
+		auto bounds = ::transformAabbToLocal( plane.bounds, *mesh.transform );		
+		uf::stl::vector<int> candidates;
+		::queryBVH( bvh, bounds, candidates );
+
+		bool hit = false;
+		// do collision per triangle
+		for ( auto triID : candidates ) {
+			auto tri = ::fetchTriangle( meshData, triID, mesh ); // transform triangle to world space
+			if ( !::trianglePlane( tri, plane, manifold, eps ) ) continue;
+			hit = true;
+		}
+
+		return hit;
 	}
 	bool meshCapsule( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) {
-		ASSERT_COLLIDER_TYPES(MESH, CAPSULE);
+		ASSERT_COLLIDER_TYPES( MESH, CAPSULE );
 
 		const auto& mesh = a;
 		const auto& capsule = b;
@@ -231,42 +97,18 @@ namespace {
 		const auto& meshData = *mesh.collider.u.mesh.mesh;
 		const auto& bvh  = *mesh.collider.u.mesh.bvh;
 
-		// capsule line segment in world space
-		auto [ p1, p2 ] = ::getCapsuleSegment(b);
-		float r = b.collider.u.capsule.radius;
-		
-		auto bounds = ::transformAabbToLocal( ::computeSegmentAABB(p1, p2, r), *a.transform );
-
+		// transform to local space for BVH query
+		auto bounds = ::transformAabbToLocal( capsule.bounds, *mesh.transform );		
 		uf::stl::vector<int> candidates;
-		::queryBVH(bvh, bounds, candidates);
+		::queryBVH( bvh, bounds, candidates );
+
+		UF_MSG_DEBUG("candidates={}", candidates.size());
 
-		// for some reason (the segment points), it's just easier to transform the triangle to world space, than compare in local then transform the contact to world
 		bool hit = false;
+		// do collision per triangle
 		for ( auto triID : candidates ) {
-			auto tri = ::fetchTriangle( meshData, triID, mesh );
-
-			pod::Vector3f closestSeg, closestTri;
-			float dist2 = ::segmentTriangleDistanceSq( p1, p2, tri, closestSeg, closestTri );
-
-			if ( !uf::vector::isValid( closestTri ) ) {
-				//UF_MSG_DEBUG("tri #{}={}, {}, {}", triID, uf::vector::toString( tri.points[0] ), uf::vector::toString( tri.points[1] ), uf::vector::toString( tri.points[2] ));
-				//UF_MSG_DEBUG("closestTri={}", uf::vector::toString( closestTri ));
-				continue;
-			}
-
-			if ( dist2 > r * r ) continue;
-			float dist = std::sqrt( dist2 );
-			auto delta = ( closestSeg - closestTri );
-
-			auto contact = closestTri; // ( closestSeg + closestTri ) * 0.5f; // center of points
-			auto normal = ::normalizeDelta( delta, dist, eps ); // uf::vector::normalize( ::interpolateWithBarycentric( tri.normals, ::computeBarycentric( contact, tri ) ));
-			float penetration = r - dist;
-
-		#if REORIENT_NORMALS_ON_CONTACT
-			if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal;
-		#endif
-
-			manifold.points.emplace_back(pod::Contact{ contact, normal, penetration });
+			auto tri = ::fetchTriangle( meshData, triID, mesh ); // transform triangle to world space
+			if ( !::triangleCapsule( tri, capsule, manifold, eps ) ) continue;
 			hit = true;
 		}
 
@@ -276,28 +118,24 @@ namespace {
 	bool meshMesh( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) {
 		ASSERT_COLLIDER_TYPES( MESH, MESH );
 
-		auto& bodyA = a;
-		auto& bvhA = *a.collider.u.mesh.bvh;
-		auto& meshA = *a.collider.u.mesh.mesh;
+		const auto& bvhA = *a.collider.u.mesh.bvh;
+		const auto& meshA = *a.collider.u.mesh.mesh;
 		
-		auto& bodyB = b;
-		auto& meshB = *b.collider.u.mesh.mesh;
-		auto& bvhB = *b.collider.u.mesh.bvh;
+		const auto& meshB = *b.collider.u.mesh.mesh;
+		const auto& bvhB = *b.collider.u.mesh.bvh;
 
+		// compute overlaps between one BVH and another BVH
 		pod::BVH::pair_t pairs;
 		::traverseNodePair(bvhA, 0, bvhB, 0, pairs);
 
-		for (auto [idA, idB] : pairs) {
-			auto tA = ::fetchTriangle( meshA, idA, bodyA );
-			auto tB = ::fetchTriangle( meshB, idB, bodyB );
-
-			// narrowphase tri-tri overlap: SAT
-			if ( ::triangleTriangleIntersect( tA, tB ) ) {
-				pod::Vector3f contact = ( tA.points[0] + tB.points[0] )*0.5f;
-				pod::Vector3f n = uf::vector::normalize( uf::vector::cross( tA.points[1] - tA.points[0], tA.points[2] - tA.points[0] ) );
-				manifold.points.emplace_back(pod::Contact{ contact, n, 0.001f });
-			}
+		bool hit = false;
+		// do collision per triangle
+		for (auto [ idA, idB] : pairs ) {
+			auto tA = ::fetchTriangle( meshA, idA, a ); // transform triangles to world space
+			auto tB = ::fetchTriangle( meshB, idB, b );
+			if ( !::triangleTriangle( tA, tB, manifold, eps ) ) continue;
+			hit = true;
 		}
-		return !manifold.points.empty();
+		return hit;
 	}
 }

engine/src/utils/math/physics/solvers.inl

@@ -0,0 +1,278 @@
+namespace {
+	void iterativeImpulseSolver( pod::RigidBody& a, pod::RigidBody& b, pod::Contact& contact, float dt ) {
+		// relative positions from centers to contact point
+		pod::Vector3f rA = contact.point - a.transform->position;
+		pod::Vector3f rB = contact.point - b.transform->position;
+
+		// relative velocity at contact
+		pod::Vector3f vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);
+		pod::Vector3f vB = b.velocity + uf::vector::cross(b.angularVelocity, rB);
+		pod::Vector3f rv = vB - vA;
+
+		// normal contact velocity
+		float velAlongNormal = uf::vector::dot(rv, contact.normal);
+		float velTolerance = 0; // -1.0e3f;
+		if ( velAlongNormal > velTolerance ) return; // if separating, no impulse
+
+		// compute restitution (bounce)
+		float e = std::min(a.material.restitution, b.material.restitution);
+
+		// nullify restitution if velocity is small enough
+		if ( fabs(velAlongNormal) < 1.0f) e = 0.0f;
+
+		// effective inverse mass along normal
+		float invMassN = ::computeEffectiveMass(a, b, rA, rB, contact.normal);
+
+		// normal impulse scalar
+		float jn = -(1.0f + e) * velAlongNormal;
+		jn /= invMassN;
+		if ( ::warmupSolver ) {
+			float jnOld = contact.accumulatedNormalImpulse;
+			float jnNew = std::max(0.0f, jnOld + jn);
+			float jnDelta = jnNew - jnOld;
+			contact.accumulatedNormalImpulse = jnNew;
+			jn = jnDelta;
+		}
+
+		pod::Vector3f normalImpulse = contact.normal * jn;
+		::applyImpulseTo(a, b, rA, rB, normalImpulse);
+
+		// tangent direction
+		pod::Vector3f tangent = rv - contact.normal * uf::vector::dot(rv, contact.normal);
+		float tangentMag = uf::vector::magnitude(tangent);
+		if (tangentMag > EPS(1e-6f)) {
+			tangent /= tangentMag;
+
+			// effective mass along tangent
+			float invMassT = ::computeEffectiveMass(a, b, rA, rB, tangent);
+
+			// tangential relative velocity
+			float vt = uf::vector::dot(rv, tangent);
+
+			// required tangential impulse to cancel tangent velocity
+			float jt = -vt / invMassT;
+
+			// friction coefficients
+			float mu_s = std::sqrt(a.material.staticFriction * b.material.staticFriction);
+			float mu_d = std::sqrt(a.material.dynamicFriction * b.material.dynamicFriction);
+			
+			if ( std::fabs(jt) > jn * mu_s) jt = -jn * mu_d; // dynamic friction: resist sliding proportionally
+
+			if ( ::warmupSolver ) {
+				float maxFriction = mu_s * contact.accumulatedNormalImpulse;
+				float jtOld = contact.accumulatedTangentImpulse;
+				float jtNew = std::max(-maxFriction, std::min(jtOld + jt, maxFriction));
+				float jtDelta = jtNew - jtOld;
+				contact.accumulatedTangentImpulse = jtNew;
+				contact.tangent = tangent;
+				jt = jtDelta;
+			}
+
+			pod::Vector3f frictionImpulse = tangent * jt;
+			::applyImpulseTo(a, b, rA, rB, frictionImpulse);
+		}
+
+		::positionCorrection(a, b, contact);
+	}
+
+	template<size_t N, typename T = float>
+	void blockNxNSolver( pod::RigidBody& a, pod::RigidBody& b, pod::Manifold& manifold, float dt ) {
+		pod::Matrix<T,N> K = {};
+		pod::Vector<T,N> rhs = {};
+		pod::Vector<T,N> lambda = {};
+		pod::Vector<T,N> residual = {};
+		
+		pod::Vector3f relVelLinear = b.velocity - a.velocity;
+		// precompute inverse masses
+		float invMassA = (a.isStatic ? 0.0f : a.inverseMass);
+		float invMassB = (b.isStatic ? 0.0f : b.inverseMass);
+
+		for (int i = 0; i < N; i++) {
+			pod::Vector3f rA_i = manifold.points[i].point - a.transform->position;
+			pod::Vector3f rB_i = manifold.points[i].point - b.transform->position;
+
+			for (int j = 0; j < N; j++) {
+				pod::Vector3f rA_j = manifold.points[j].point - a.transform->position;
+				pod::Vector3f rB_j = manifold.points[j].point - b.transform->position;
+
+				pod::Vector3f n_i = manifold.points[i].normal;
+				pod::Vector3f n_j = manifold.points[j].normal;
+
+				float termLinear = (invMassA + invMassB) * uf::vector::dot(n_i, n_j);
+
+				// angular parts
+				pod::Vector3f raXnj = uf::vector::cross(rA_j, n_j);
+				pod::Vector3f rbXnj = uf::vector::cross(rB_j, n_j);
+
+				pod::Vector3f Ia_raXnj = a.inverseInertiaTensor * raXnj;
+				pod::Vector3f Ib_rbXnj = b.inverseInertiaTensor * rbXnj;
+
+				pod::Vector3f crossA = uf::vector::cross(Ia_raXnj, rA_i);
+				pod::Vector3f crossB = uf::vector::cross(Ib_rbXnj, rB_i);
+
+				float termAngular = uf::vector::dot(n_i, crossA + crossB);
+
+				K(i,j) = termLinear + termAngular;
+			}
+
+			K(i,i) += 1e-3f;
+		}
+
+		for ( auto i = 0; i < N; i++ ) {
+			float vRel = uf::vector::dot( relVelLinear, manifold.points[i].normal );
+
+			float penetrationBias = std::max( manifold.points[i].penetration - ::baumgarteCorrectionSlop, 0.0f) * (::baumgarteCorrectionPercent / dt );
+			float cDot = vRel + penetrationBias;
+
+			rhs[i] = (cDot < 0.0f) ? -cDot : 0.0f;
+			lambda[i] = manifold.points[i].accumulatedNormalImpulse;
+		}
+
+		residual = rhs - uf::matrix::multiply( K, lambda );
+		pod::Matrix<T,N> Kinv = uf::matrix::invert( K );
+		pod::Vector<T,N> dLambda = uf::matrix::multiply( Kinv, residual );
+
+		for ( auto i = 0; i < N; i++ ) {
+			float newLambda = std::max(lambda[i] + dLambda[i], 0.0f);
+			dLambda[i] = newLambda - lambda[i];
+			lambda[i] = newLambda;
+			manifold.points[i].accumulatedNormalImpulse = newLambda;
+		}
+
+		pod::Vector3f impulse{};
+		for ( auto i = 0; i < N; i++) {
+			impulse += manifold.points[i].normal * dLambda[i];
+		}
+
+		if ( !a.isStatic ) a.velocity -= impulse * a.inverseMass;
+		if ( !b.isStatic ) b.velocity += impulse * b.inverseMass;
+	}
+
+	void block2x2Solver( pod::RigidBody& a, pod::RigidBody& b, pod::Manifold& manifold, float dt ) {
+		return ::blockNxNSolver<2>( a, b, manifold, dt );
+	}
+	void block3x3Solver( pod::RigidBody& a, pod::RigidBody& b, pod::Manifold& manifold, float dt ) {
+		return ::blockNxNSolver<3>( a, b, manifold, dt );
+	}
+	void block4x4Solver( pod::RigidBody& a, pod::RigidBody& b, pod::Manifold& manifold, float dt ) {
+		return ::blockNxNSolver<4>( a, b, manifold, dt );
+	}
+
+	void blockPGSSolver( pod::RigidBody& a, pod::RigidBody& b, pod::Manifold& manifold, float dt ) {
+		const int count = std::min( (int) manifold.points.size(), 4 );
+
+		// precompute inv mass
+		float invMassA = ( a.isStatic ? 0.0f : a.inverseMass );
+		float invMassB = ( b.isStatic ? 0.0f : b.inverseMass );
+
+		// precompute Jacobians for each contact
+		struct ContactCache {
+			pod::Vector3f normal;
+			pod::Vector3f rA, rB;
+			float bias;
+			float effectiveMass;
+		} cache[4];
+
+		for ( auto i = 0; i < count; i++ ) {
+			auto& c = manifold.points[i];
+			cache[i].normal = c.normal;
+			cache[i].rA = c.point - a.transform->position;
+			cache[i].rB = c.point - b.transform->position;
+
+			// bias = restitution + Baumgarte correction
+			float vRel = uf::vector::dot(
+				(b.velocity + uf::vector::cross(b.angularVelocity, cache[i].rB)) -
+				(a.velocity + uf::vector::cross(a.angularVelocity, cache[i].rA)),
+				cache[i].normal
+			);
+
+			float e = std::min( a.material.restitution, b.material.restitution );
+			float penetrationBias = std::max(c.penetration - ::baumgarteCorrectionSlop, 0.0f) * (::baumgarteCorrectionPercent / dt);
+
+			cache[i].bias = (vRel < -1.0f ? -e * vRel : 0.0f) + penetrationBias;
+
+			// effective mass = 1 / (J M^-1 J^T)
+			pod::Vector3f rnA = uf::vector::cross( cache[i].rA, cache[i].normal );
+			pod::Vector3f rnB = uf::vector::cross( cache[i].rB, cache[i].normal );
+
+			pod::Vector3f Ia_rnA = rnA * a.inverseInertiaTensor; // diag mult
+			pod::Vector3f Ib_rnB = rnB * b.inverseInertiaTensor;
+
+			float Knormal = invMassA + invMassB + uf::vector::dot(uf::vector::cross(Ia_rnA, cache[i].rA) + uf::vector::cross(Ib_rnB, cache[i].rB), cache[i].normal);
+
+			cache[i].effectiveMass = (Knormal > 0.0f) ? 1.0f / Knormal : 0.0f;
+		}
+
+		// initialize lambdas (warm start)
+		pod::Vector4f lambda = {};
+		for ( auto i = 0; i < count; i++ ) {
+			lambda[i] = manifold.points[i].accumulatedNormalImpulse;
+		}
+
+		// iterative PGS loop
+		for ( auto iter = 0; iter < ::solverIterations; iter++ ) {
+			for (int i = 0; i < count; i++) {
+				auto& c = manifold.points[i];
+				auto& cc = cache[i];
+
+				// relative velocity along normal
+				pod::Vector3f dv = (b.velocity + uf::vector::cross(b.angularVelocity, cc.rB)) - (a.velocity + uf::vector::cross(a.angularVelocity, cc.rA));
+				float vn = uf::vector::dot(dv, cc.normal);
+
+				// compute delta impulse
+				float delta = cc.effectiveMass * (-(vn + cc.bias));
+
+				// accumulate and clamp
+				float newLambda = std::max( lambda[i] + delta, 0.0f );
+				delta = newLambda - lambda[i];
+				lambda[i] = newLambda;
+
+				// apply impulse
+				pod::Vector3f P = cc.normal * delta;
+				if ( !a.isStatic ) {
+					a.velocity -= P * invMassA;
+					a.angularVelocity -= uf::vector::cross(cc.rA, P) * a.inverseInertiaTensor;
+				}
+				if ( !b.isStatic ) {
+					b.velocity += P * invMassB;
+					b.angularVelocity += uf::vector::cross(cc.rB, P) * b.inverseInertiaTensor;
+				}
+			}
+		}
+
+		// store lambdas back
+		for ( auto i = 0; i < count; i++ ) {
+			manifold.points[i].accumulatedNormalImpulse = lambda[i];
+		}
+	}
+
+	void resolveManifold( pod::RigidBody& a, pod::RigidBody& b, pod::Manifold& manifold, float dt ) {
+		if ( manifold.points.size() == 2 ) return ::block2x2Solver( a, b, manifold, dt );
+		if ( manifold.points.size() == 3 ) return ::block3x3Solver( a, b, manifold, dt );
+		if ( manifold.points.size() == 4 ) return ::block4x4Solver( a, b, manifold, dt );
+
+		for ( auto& contact : manifold.points ) {
+			::iterativeImpulseSolver( a, b, contact, dt );	
+		}
+	}
+
+	void solveContacts( uf::stl::vector<pod::Manifold>& manifolds, float dt ) {
+		for ( auto& m : manifolds ) {
+			::blockPGSSolver( *m.a, *m.b, m, dt );
+		}
+	/*
+		if ( ::warmupSolver ) {
+			for ( auto& m : manifolds ) {
+				for ( auto& c : m.points ) {
+					::warmupContacts( *m.a, *m.b, c, dt );
+				}
+			}
+		}
+		for ( auto i = 0; i < ::solverIterations; ++i ) {
+			for ( auto& m : manifolds ) {
+				::resolveManifold( *m.a, *m.b, m, dt );
+			}
+		}
+	*/
+	}
+}

engine/src/utils/math/physics/tests.inl

@@ -28,7 +28,7 @@ namespace {
 }
 
 // list of unit tests to "standardly" verify the system works, but honestly this is a mess
-
+#if 0
 TEST(SphereSphere_Collision, {
 	pod::World world;
 	uf::Object objA, objB;
@@ -496,320 +496,508 @@ TEST(AabbPlane_RestingNoSink, {
 })
 
 TEST(CapsuleSphere_Collision, {
-    pod::World world;
-    uf::Object capObj, sphObj;
-    auto& cap = uf::physics::impl::create(world, capObj, pod::Capsule{0.5f,1.0f}, 1.0f);
-    auto& sph = uf::physics::impl::create(world, sphObj, pod::Sphere{0.5f}, 1.0f);
+	pod::World world;
+	uf::Object capObj, sphObj;
+	auto& cap = uf::physics::impl::create(world, capObj, pod::Capsule{0.5f,1.0f}, 1.0f);
+	auto& sph = uf::physics::impl::create(world, sphObj, pod::Sphere{0.5f}, 1.0f);
 
-    cap.transform->position = {0,0,0};
-    sph.transform->position = {0,0.5f,0}; // overlap
+	cap.transform->position = {0,0,0};
+	sph.transform->position = {0,0.5f,0}; // overlap
 
-    pod::Manifold m;
-    bool collided = capsuleSphere(cap, sph, m);
-    EXPECT_TRUE(collided);
-    EXPECT_TRUE(!m.points.empty());
+	pod::Manifold m;
+	bool collided = capsuleSphere(cap, sph, m);
+	EXPECT_TRUE(collided);
+	EXPECT_TRUE(!m.points.empty());
 })
 
 TEST(CapsuleSphere_NoCollision, {
-    pod::World world;
-    uf::Object capObj, sphObj;
-    auto& cap = uf::physics::impl::create(world, capObj, pod::Capsule{0.5f,1.0f}, 1.0f);
-    auto& sph = uf::physics::impl::create(world, sphObj, pod::Sphere{0.5f}, 1.0f);
+	pod::World world;
+	uf::Object capObj, sphObj;
+	auto& cap = uf::physics::impl::create(world, capObj, pod::Capsule{0.5f,1.0f}, 1.0f);
+	auto& sph = uf::physics::impl::create(world, sphObj, pod::Sphere{0.5f}, 1.0f);
 
-    cap.transform->position = {0,0,0};
-    sph.transform->position = {0,5,0}; // too far
+	cap.transform->position = {0,0,0};
+	sph.transform->position = {0,5,0}; // too far
 
-    pod::Manifold m;
-    bool collided = capsuleSphere(cap, sph, m);
-    EXPECT_TRUE(!collided);
+	pod::Manifold m;
+	bool collided = capsuleSphere(cap, sph, m);
+	EXPECT_TRUE(!collided);
 })
 
 
 TEST(AabbSphere_Collision, {
-    pod::World world;
-    uf::Object objA, objB;
-    auto& box   = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
-    auto& sphere= uf::physics::impl::create(world, objB, pod::Sphere{0.5f}, 1.0f);
+	pod::World world;
+	uf::Object objA, objB;
+	auto& box   = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
+	auto& sphere= uf::physics::impl::create(world, objB, pod::Sphere{0.5f}, 1.0f);
 
-    box.transform->position    = {0,0,0};
-    sphere.transform->position = {0.75f,0,0}; // Intersecting into box
+	box.transform->position	= {0,0,0};
+	sphere.transform->position = {0.75f,0,0}; // Intersecting into box
 
-    pod::Manifold m;
-    bool collided = aabbSphere(box,sphere,m);
-    EXPECT_TRUE(collided);
-    EXPECT_TRUE(!m.points.empty());
+	pod::Manifold m;
+	bool collided = aabbSphere(box,sphere,m);
+	EXPECT_TRUE(collided);
+	EXPECT_TRUE(!m.points.empty());
 })
 
 TEST(AabbSphere_NoCollision, {
-    pod::World world;
-    uf::Object objA, objB;
-    auto& box   = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
-    auto& sphere= uf::physics::impl::create(world, objB, pod::Sphere{0.5f}, 1.0f);
+	pod::World world;
+	uf::Object objA, objB;
+	auto& box   = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
+	auto& sphere= uf::physics::impl::create(world, objB, pod::Sphere{0.5f}, 1.0f);
 
-    box.transform->position    = {0,0,0};
-    sphere.transform->position = {5,0,0}; // too far away
+	box.transform->position	= {0,0,0};
+	sphere.transform->position = {5,0,0}; // too far away
 
-    pod::Manifold m;
-    bool collided = aabbSphere(box,sphere,m);
-    EXPECT_TRUE(!collided);
+	pod::Manifold m;
+	bool collided = aabbSphere(box,sphere,m);
+	EXPECT_TRUE(!collided);
 })
 
 
 TEST(AabbPlane_Collision, {
-    pod::World world;
-    uf::Object objA, objB;
-    auto& box   = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
-    auto& plane = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f);
+	pod::World world;
+	uf::Object objA, objB;
+	auto& box   = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
+	auto& plane = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f);
 
-    box.transform->position = {0,0.5f,0}; // half interpenetrating plane
+	box.transform->position = {0,0.5f,0}; // half interpenetrating plane
 
-    pod::Manifold m;
-    bool collided = aabbPlane(box,plane,m);
-    EXPECT_TRUE(collided);
-    EXPECT_TRUE(!m.points.empty());
+	pod::Manifold m;
+	bool collided = aabbPlane(box,plane,m);
+	EXPECT_TRUE(collided);
+	EXPECT_TRUE(!m.points.empty());
 })
 
 TEST(AabbPlane_NoCollision, {
-    pod::World world;
-    uf::Object objA, objB;
-    auto& box   = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
-    auto& plane = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f);
+	pod::World world;
+	uf::Object objA, objB;
+	auto& box   = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
+	auto& plane = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f);
 
-    box.transform->position = {0,5,0}; // clearly above
+	box.transform->position = {0,5,0}; // clearly above
 
-    pod::Manifold m;
-    bool collided = aabbPlane(box,plane,m);
-    EXPECT_TRUE(!collided);
+	pod::Manifold m;
+	bool collided = aabbPlane(box,plane,m);
+	EXPECT_TRUE(!collided);
 })
 
 
 TEST(AabbCapsule_Collision, {
-    pod::World world;
-    uf::Object objA, objB;
-    auto& box     = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
-    auto& cap     = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f);
+	pod::World world;
+	uf::Object objA, objB;
+	auto& box	 = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
+	auto& cap	 = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f);
 
-    box.transform->position = {0,0,0};
-    cap.transform->position = {0,0.5f,0}; // partially overlapping
+	box.transform->position = {0,0,0};
+	cap.transform->position = {0,0.5f,0}; // partially overlapping
 
-    pod::Manifold m;
-    bool collided = aabbCapsule(box,cap,m);
-    EXPECT_TRUE(collided);
-    EXPECT_TRUE(!m.points.empty());
+	pod::Manifold m;
+	bool collided = aabbCapsule(box,cap,m);
+	EXPECT_TRUE(collided);
+	EXPECT_TRUE(!m.points.empty());
 })
 
 TEST(AabbCapsule_NoCollision, {
-    pod::World world;
-    uf::Object objA, objB;
-    auto& box     = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
-    auto& cap     = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f);
+	pod::World world;
+	uf::Object objA, objB;
+	auto& box	 = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
+	auto& cap	 = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f);
 
-    box.transform->position = {0,0,0};
-    cap.transform->position = {0,5,0};
+	box.transform->position = {0,0,0};
+	cap.transform->position = {0,5,0};
 
-    pod::Manifold m;
-    bool collided = aabbCapsule(box,cap,m);
-    EXPECT_TRUE(!collided);
+	pod::Manifold m;
+	bool collided = aabbCapsule(box,cap,m);
+	EXPECT_TRUE(!collided);
 })
 
 
 TEST(SphereCapsule_Collision, {
-    pod::World world;
-    uf::Object objA, objB;
-    auto& sphere = uf::physics::impl::create(world, objA, pod::Sphere{0.5f}, 1.0f);
-    auto& cap    = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f);
+	pod::World world;
+	uf::Object objA, objB;
+	auto& sphere = uf::physics::impl::create(world, objA, pod::Sphere{0.5f}, 1.0f);
+	auto& cap	= uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f);
 
-    sphere.transform->position = {0,0.0f,0};
-    cap.transform->position    = {0,0.25f,0};
+	sphere.transform->position = {0,0.0f,0};
+	cap.transform->position	= {0,0.25f,0};
 
-    pod::Manifold m;
-    bool collided = sphereCapsule(sphere,cap,m);
-    EXPECT_TRUE(collided);
-    EXPECT_TRUE(!m.points.empty());
+	pod::Manifold m;
+	bool collided = sphereCapsule(sphere,cap,m);
+	EXPECT_TRUE(collided);
+	EXPECT_TRUE(!m.points.empty());
 })
 
 TEST(SphereCapsule_NoCollision, {
-    pod::World world;
-    uf::Object objA, objB;
-    auto& sphere = uf::physics::impl::create(world, objA, pod::Sphere{0.5f}, 1.0f);
-    auto& cap    = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f);
+	pod::World world;
+	uf::Object objA, objB;
+	auto& sphere = uf::physics::impl::create(world, objA, pod::Sphere{0.5f}, 1.0f);
+	auto& cap	= uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f);
 
-    sphere.transform->position = {0,5,0};
-    cap.transform->position    = {0,0,0};
+	sphere.transform->position = {0,5,0};
+	cap.transform->position	= {0,0,0};
 
-    pod::Manifold m;
-    bool collided = sphereCapsule(sphere,cap,m);
-    EXPECT_TRUE(!collided);
+	pod::Manifold m;
+	bool collided = sphereCapsule(sphere,cap,m);
+	EXPECT_TRUE(!collided);
 })
 
 TEST(PlanePlane_NoCollision, {
-    pod::World world;
-    uf::Object objA, objB;
-    auto& planeA= uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f);
-    auto& planeB= uf::physics::impl::create(world, objB, pod::Plane{{0,0,1},0.0f}, 0.0f);
+	pod::World world;
+	uf::Object objA, objB;
+	auto& planeA= uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f);
+	auto& planeB= uf::physics::impl::create(world, objB, pod::Plane{{0,0,1},0.0f}, 0.0f);
 
-    pod::Manifold m;
-    bool collided = planePlane(planeA,planeB,m);
-    EXPECT_TRUE(!collided); // always false in your engine
+	pod::Manifold m;
+	bool collided = planePlane(planeA,planeB,m);
+	EXPECT_TRUE(!collided); // always false in your engine
 })
 
 
 TEST(PlaneCapsule_Collision, {
-    pod::World world;
-    uf::Object objA, objB;
-    auto& plane = uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f);
-    auto& cap   = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f);
+	pod::World world;
+	uf::Object objA, objB;
+	auto& plane = uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f);
+	auto& cap   = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f);
 
-    cap.transform->position = {0,0.25f,0}; // foot intersecting
+	cap.transform->position = {0,0.25f,0}; // foot intersecting
 
-    pod::Manifold m;
-    bool collided = planeCapsule(plane,cap,m);
-    EXPECT_TRUE(collided);
-    EXPECT_TRUE(!m.points.empty());
+	pod::Manifold m;
+	bool collided = planeCapsule(plane,cap,m);
+	EXPECT_TRUE(collided);
+	EXPECT_TRUE(!m.points.empty());
 })
 
 TEST(PlaneCapsule_NoCollision, {
-    pod::World world;
-    uf::Object objA, objB;
-    auto& plane = uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f);
-    auto& cap   = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f);
+	pod::World world;
+	uf::Object objA, objB;
+	auto& plane = uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f);
+	auto& cap   = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f);
 
-    cap.transform->position = {0,5,0}; // far above
+	cap.transform->position = {0,5,0}; // far above
 
-    pod::Manifold m;
-    bool collided = planeCapsule(plane,cap,m);
-    EXPECT_TRUE(!collided);
+	pod::Manifold m;
+	bool collided = planeCapsule(plane,cap,m);
+	EXPECT_TRUE(!collided);
 })
 TEST(MeshSphere_Collision, {
-    pod::World world;
-    uf::Object objMesh, objSphere;
+	pod::World world;
+	uf::Object objMesh, objSphere;
 
-    // Create mesh body (a plane on Y=0, size=1)
-    auto mesh = ::generateMesh(1.0f);
-    auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); // static mesh
-    meshBody.transform->position = {0,0,0};
+	// Create mesh body (a plane on Y=0, size=1)
+	auto mesh = ::generateMesh(1.0f);
+	auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); // static mesh
+	meshBody.transform->position = {0,0,0};
 
-    // Sphere just above plane, radius 1, intersects
-    auto& sphereBody = uf::physics::impl::create(world, objSphere, pod::Sphere{2.0f}, 1.0f);
-    sphereBody.transform->position = {0,0.5f,0}; // half below plane (since plane is at y=0)
+	// Sphere just above plane, radius 1, intersects
+	auto& sphereBody = uf::physics::impl::create(world, objSphere, pod::Sphere{2.0f}, 1.0f);
+	sphereBody.transform->position = {0,0.5f,0}; // half below plane (since plane is at y=0)
 
-    pod::Manifold m;
-    bool collided = meshSphere(meshBody, sphereBody, m);
-    EXPECT_TRUE(collided);
-    EXPECT_TRUE(!m.points.empty());
-    if ( !m.points.empty() ) EXPECT_TRUE(m.points[0].penetration > 0.0f);
+	pod::Manifold m;
+	bool collided = meshSphere(meshBody, sphereBody, m);
+	EXPECT_TRUE(collided);
+	EXPECT_TRUE(!m.points.empty());
+	if ( !m.points.empty() ) EXPECT_TRUE(m.points[0].penetration > 0.0f);
 })
 
 TEST(MeshSphere_NoCollision, {
-    pod::World world;
-    uf::Object objMesh, objSphere;
+	pod::World world;
+	uf::Object objMesh, objSphere;
 
-    auto mesh = ::generateMesh(1.0f);
-    auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f);
-    meshBody.transform->position = {0,0,0};
+	auto mesh = ::generateMesh(1.0f);
+	auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f);
+	meshBody.transform->position = {0,0,0};
 
-    auto& sphereBody = uf::physics::impl::create(world, objSphere, pod::Sphere{0.5f}, 1.0f);
-    sphereBody.transform->position = {0,5.0f,0}; // far above plane
+	auto& sphereBody = uf::physics::impl::create(world, objSphere, pod::Sphere{0.5f}, 1.0f);
+	sphereBody.transform->position = {0,5.0f,0}; // far above plane
 
-    pod::Manifold m;
-    bool collided = meshSphere(meshBody, sphereBody, m);
-    EXPECT_FALSE(collided);
+	pod::Manifold m;
+	bool collided = meshSphere(meshBody, sphereBody, m);
+	EXPECT_FALSE(collided);
 })
 
 TEST(MeshAabb_Collision, {
-    pod::World world;
-    uf::Object objMesh, objBox;
+	pod::World world;
+	uf::Object objMesh, objBox;
 
-    auto mesh = ::generateMesh(2.0f);
-    auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f);
-    meshBody.transform->position = {0,0,0};
+	auto mesh = ::generateMesh(2.0f);
+	auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f);
+	meshBody.transform->position = {0,0,0};
 
-    pod::AABB box = { {-0.5f,-0.5f,-0.5f}, {0.5f,0.5f,0.5f} };
-    auto& boxBody = uf::physics::impl::create(world, objBox, box, 1.0f);
-    boxBody.transform->position = {0,0.25f,0}; // overlaps plane
+	pod::AABB box = { {-0.5f,-0.5f,-0.5f}, {0.5f,0.5f,0.5f} };
+	auto& boxBody = uf::physics::impl::create(world, objBox, box, 1.0f);
+	boxBody.transform->position = {0,0.25f,0}; // overlaps plane
 
-    pod::Manifold m;
-    bool collided = meshAabb(meshBody, boxBody, m);
-    EXPECT_TRUE(collided);
-    EXPECT_TRUE(!m.points.empty());
+	pod::Manifold m;
+	bool collided = meshAabb(meshBody, boxBody, m);
+	EXPECT_TRUE(collided);
+	EXPECT_TRUE(!m.points.empty());
 })
 
 TEST(MeshAabb_NoCollision, {
-    pod::World world;
-    uf::Object objMesh, objBox;
+	pod::World world;
+	uf::Object objMesh, objBox;
 
-    auto mesh = ::generateMesh(2.0f);
-    auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f);
-    meshBody.transform->position = {0,0,0};
+	auto mesh = ::generateMesh(2.0f);
+	auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f);
+	meshBody.transform->position = {0,0,0};
 
-    pod::AABB box = { {-0.5f,-0.5f,-0.5f}, {0.5f,0.5f,0.5f} };
-    auto& boxBody = uf::physics::impl::create(world, objBox, box, 1.0f);
-    boxBody.transform->position = {0,5.0f,0}; // above plane, no overlap
+	pod::AABB box = { {-0.5f,-0.5f,-0.5f}, {0.5f,0.5f,0.5f} };
+	auto& boxBody = uf::physics::impl::create(world, objBox, box, 1.0f);
+	boxBody.transform->position = {0,5.0f,0}; // above plane, no overlap
 
-    pod::Manifold m;
-    bool collided = meshAabb(meshBody, boxBody, m);
-    EXPECT_FALSE(collided);
+	pod::Manifold m;
+	bool collided = meshAabb(meshBody, boxBody, m);
+	EXPECT_FALSE(collided);
 })
 
 TEST(RayMesh_Hit, {
-    pod::World world;
-    uf::Object objMesh;
+	pod::World world;
+	uf::Object objMesh;
 
-    auto mesh = ::generateMesh(1.0f);
-    auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f);
-    meshBody.transform->position = {0,0,0};
+	auto mesh = ::generateMesh(1.0f);
+	auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f);
+	meshBody.transform->position = {0,0,0};
 
-    pod::Ray ray{ {0,1,0}, {0,-1,0} }; // from above, pointing down
-    pod::RayQuery hit = uf::physics::impl::rayCast(ray, world, 100.0f);
+	pod::Ray ray{ {0,1,0}, {0,-1,0} }; // from above, pointing down
+	pod::RayQuery hit = uf::physics::impl::rayCast(ray, world, 100.0f);
 
-    EXPECT_TRUE(hit.hit);
-    EXPECT_TRUE(hit.contact.penetration > 0.0f);
+	EXPECT_TRUE(hit.hit);
+	EXPECT_TRUE(hit.contact.penetration > 0.0f);
 })
 
 TEST(RayMesh_Miss, {
-    pod::World world;
-    uf::Object objMesh;
+	pod::World world;
+	uf::Object objMesh;
 
-    auto mesh = ::generateMesh(1.0f);
-    auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f);
-    meshBody.transform->position = {0,0,0};
+	auto mesh = ::generateMesh(1.0f);
+	auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f);
+	meshBody.transform->position = {0,0,0};
 
-    pod::Ray ray{ {0,2,0}, {1,0,0} }; // parallel, goes sideways
-    pod::RayQuery hit = uf::physics::impl::rayCast(ray, world, 100.0f);
+	pod::Ray ray{ {0,2,0}, {1,0,0} }; // parallel, goes sideways
+	pod::RayQuery hit = uf::physics::impl::rayCast(ray, world, 100.0f);
 
-    EXPECT_FALSE(hit.hit);
+	EXPECT_FALSE(hit.hit);
 })
 
 TEST(MeshMesh_Collision, {
-    pod::World world;
-    uf::Object objA, objB;
+	pod::World world;
+	uf::Object objA, objB;
 
-    auto mesh = ::generateMesh(1.0f);
+	auto mesh = ::generateMesh(1.0f);
 
-    auto& meshA = uf::physics::impl::create(world, objA, mesh, 0.0f);
-    auto& meshB = uf::physics::impl::create(world, objB, mesh, 0.0f);
+	auto& meshA = uf::physics::impl::create(world, objA, mesh, 0.0f);
+	auto& meshB = uf::physics::impl::create(world, objB, mesh, 0.0f);
 
-    meshA.transform->position = {0,0,0};
-    meshB.transform->position = {0,0,0}; // same location
+	meshA.transform->position = {0,0,0};
+	meshB.transform->position = {0,0,0}; // same location
 
-    pod::Manifold m;
-    bool collided = meshMesh(meshA, meshB, m);
-    EXPECT_TRUE(collided);
+	pod::Manifold m;
+	bool collided = meshMesh(meshA, meshB, m);
+	EXPECT_TRUE(collided);
 })
 
 TEST(MeshMesh_NoCollision, {
-    pod::World world;
-    uf::Object objA, objB;
+	pod::World world;
+	uf::Object objA, objB;
 
-    auto mesh = ::generateMesh(1.0f);
+	auto mesh = ::generateMesh(1.0f);
 
-    auto& meshA = uf::physics::impl::create(world, objA, mesh, 0.0f);
-    auto& meshB = uf::physics::impl::create(world, objB, mesh, 0.0f);
+	auto& meshA = uf::physics::impl::create(world, objA, mesh, 0.0f);
+	auto& meshB = uf::physics::impl::create(world, objB, mesh, 0.0f);
 
-    meshA.transform->position = {0,0,0};
-    meshB.transform->position = {0,10.0f,0}; // too far apart
+	meshA.transform->position = {0,0,0};
+	meshB.transform->position = {0,10.0f,0}; // too far apart
+
+	pod::Manifold m;
+	bool collided = meshMesh(meshA, meshB, m);
+	EXPECT_FALSE(collided);
+})
+#endif
+
+#define EPS 1.0e-4
+#if 0
+TEST(TriangleTriangle_Collision_SimpleOverlap, {
+	// Two identical triangles overlapping on XY plane
+	pod::TriangleWithNormal triA {
+		{ { {0,0,0}, {1,0,0}, {0,1,0} } },
+		{ {0,0,1}, {0,0,1}, {0,0,1} },
+	};
+	pod::TriangleWithNormal triB {
+		{ { {0.25f,0.25f,0}, {1.25f,0.25f,0},  {0.25f,1.25f,0} } },
+		{ {0,0,1}, {0,0,1},  {0,0,1} },
+	};
+
+	pod::Manifold m;
+	bool collided = triangleTriangle( triA, triB, m, EPS );
+
+	EXPECT_TRUE( collided );
+	if ( !m.points.empty() ) {
+		EXPECT_NEAR(m.points[0].point.z, 0, EPS); // contact should be on z=0 plane
+		EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS);
+		EXPECT_GE(m.points[0].penetration, 0.0f);
+	}
+})
+#endif
+
+TEST(TriangleAabb_Collision_CenterInside, {
+	pod::TriangleWithNormal tri {
+		{ { {0,0,0}, {1,0,0}, {0,1,0} } },
+		{ {0,0,1}, {0,0,1}, {0,0,1} },
+	};
+
+	// Make cube overlapping
+	pod::RigidBody box;
+	pod::Transform<> transform;
+	box.transform = &transform;
+	box.collider.type = pod::ShapeType::AABB;
+	box.collider.u.aabb.min = {0.25f, 0.25f, -0.1f};
+	box.collider.u.aabb.max = {0.75f, 0.75f, +0.1f};
+	box.transform->position = ::aabbCenter( box.collider.u.aabb );
+	box.bounds = ::computeAABB( box );
+
+	pod::Manifold m;
+	bool collided = triangleAabb(tri, box, m, EPS);
+
+	EXPECT_TRUE(collided);
+	if ( !m.points.empty() ) {
+		EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS);
+		EXPECT_GE(m.points[0].penetration, 0.0f);
+
+		UF_MSG_DEBUG("contact={}, normal={}, depth={}", uf::vector::toString( m.points[0].point ), uf::vector::toString( m.points[0].normal ), m.points[0].penetration );
+	}
+})
+
+TEST(TriangleSphere_Collision_SphereTouchingTriangle, {
+	pod::TriangleWithNormal tri {
+		{ { {0,-1,0}, {1,1,0}, {-1,1,0} } },
+		{ {0,0,1}, {0,0,1}, {0,0,1} },
+	};
+
+	pod::RigidBody sphere;
+	pod::Transform<> transform;
+	sphere.transform = &transform;
+	sphere.collider.type = pod::ShapeType::SPHERE;
+	sphere.collider.u.sphere.radius = 1.0f;
+	sphere.transform->position = {0,0,0.5f}; // Place sphere just above triangle so it penetrates slightly
+	sphere.bounds = ::computeAABB( sphere );
+
+	pod::Manifold m;
+	bool collided = triangleSphere(tri, sphere, m, EPS);
+
+	EXPECT_TRUE(collided);
+	if ( !m.points.empty() ) {
+		EXPECT_NEAR(m.points[0].point.z, 0.0f, 0.5f);
+		EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS);
+
+		UF_MSG_DEBUG("contact={}, normal={}, depth={}", uf::vector::toString( m.points[0].point ), uf::vector::toString( m.points[0].normal ), m.points[0].penetration );
+		EXPECT_GE(m.points[0].penetration, 0.0f);
+	}
+})
+
+TEST(TriangleCapsule_Collision_CapsuleIntersectingEdge, {
+	pod::TriangleWithNormal tri {
+		{ { {0,0,0}, {2,0,0},  {1,1,0} } },
+		{ {0,0,1}, {0,0,1},  {0,0,1} },
+	};
+
+	pod::RigidBody capsule;
+	pod::Transform<> transform;
+	capsule.transform = &transform;
+
+	capsule.collider.type = pod::ShapeType::CAPSULE;
+	capsule.collider.u.capsule.radius = 0.5f;
+	auto [ p1, p2 ] = std::pair{ pod::Vector3f{1,0.5f,-1}, pod::Vector3f{1,0.5f,1} };
+	capsule.bounds = computeSegmentAABB(p1, p2, capsule.collider.u.capsule.radius);
+
+	pod::Manifold m;
+	bool collided = triangleCapsule(tri, capsule, m, EPS);
+
+	EXPECT_TRUE(collided);
+	if ( !m.points.empty() ) {
+		EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS);
+		EXPECT_GE(m.points[0].penetration, 0.0f);
+
+		UF_MSG_DEBUG("contact={}, normal={}, depth={}", uf::vector::toString( m.points[0].point ), uf::vector::toString( m.points[0].normal ), m.points[0].penetration );
+	}
+})
+
+TEST(TriangleSphere_Collision_SphereTangentFace, {
+    // Triangle is in Z=0 plane
+    pod::TriangleWithNormal tri {
+        { { {0,-1,0}, {1,1,0}, {-1,1,0} } },
+        { {0,0,1}, {0,0,1}, {0,0,1} },
+    };
+
+    pod::RigidBody sphere;
+    pod::Transform<> transform;
+    sphere.transform = &transform;
+    sphere.collider.type = pod::ShapeType::SPHERE;
+    sphere.collider.u.sphere.radius = 1.0f;
+    sphere.transform->position = {0, 0, 1.0f}; // center exactly 1 unit above plane
+    sphere.bounds = ::computeAABB(sphere);
 
     pod::Manifold m;
-    bool collided = meshMesh(meshA, meshB, m);
-    EXPECT_FALSE(collided);
+    bool collided = triangleSphere(tri, sphere, m, EPS);
+
+    // Should either detect a grazing collision or at least not error
+    EXPECT_TRUE(collided);
+    if (!m.points.empty()) {
+        EXPECT_NEAR(m.points[0].penetration, 0.0f, 1e-4f);
+        EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS);
+    }
+})
+
+TEST(TriangleSphere_Collision_SphereTangentEdge, {
+    // Triangle tilted in XY plane, edge from (0,0,0) to (1,0,0)
+    pod::TriangleWithNormal tri {
+        { { {0,0,0}, {1,0,0}, {0,1,0} } },
+        { {0,0,1}, {0,0,1}, {0,0,1} },
+    };
+
+    pod::RigidBody sphere;
+    pod::Transform<> transform;
+    sphere.transform = &transform;
+    sphere.collider.type = pod::ShapeType::SPHERE;
+    sphere.collider.u.sphere.radius = 0.5f;
+
+    // Place sphere center exactly 0.5 units away from edge line
+    sphere.transform->position = {0.5f, -0.5f, 0.0f};
+    sphere.bounds = ::computeAABB(sphere);
+
+    pod::Manifold m;
+    bool collided = triangleSphere(tri, sphere, m, EPS);
+
+    EXPECT_TRUE(collided); // Tangential along edge
+    if (!m.points.empty()) {
+        EXPECT_NEAR(m.points[0].penetration, 0.0f, 1e-4f);
+    }
+})
+
+TEST(TriangleCapsule_Collision_TangentVertex, {
+    pod::TriangleWithNormal tri {
+        { { {0,0,0}, {1,0,0}, {0,1,0} } },
+        { {0,0,1}, {0,0,1}, {0,0,1} },
+    };
+
+    pod::RigidBody capsule;
+    pod::Transform<> transform;
+    capsule.transform = &transform;
+
+    capsule.collider.type = pod::ShapeType::CAPSULE;
+    capsule.collider.u.capsule.radius = 0.25f;
+
+    // Align segment so it hovers exactly through the vertex (0,0,0)
+    pod::Vector3f p1{0.0f, -0.25f, 0.0f};
+    pod::Vector3f p2{0.0f, -0.25f, 1.0f};
+
+    capsule.bounds = computeSegmentAABB(p1, p2, capsule.collider.u.capsule.radius);
+
+    pod::Manifold m;
+    bool collided = triangleCapsule(tri, capsule, m, EPS);
+
+    EXPECT_TRUE(collided);
+    if (!m.points.empty()) {
+        EXPECT_NEAR(m.points[0].penetration, 0.0f, 1e-4f);
+    }
 })

engine/src/utils/math/physics/triangle.inl

@@ -0,0 +1,233 @@
+#define REORIENT_NORMALS_ON_FETCH 0
+#define REORIENT_NORMALS_ON_CONTACT 1
+
+// mesh BVH
+namespace {
+	// to-do: clean this up
+	uint32_t getIndex( const void* indexData, size_t indexSize, size_t idx ) {
+		if ( indexSize == sizeof(uint8_t) ) {
+			auto* ptr = reinterpret_cast<const uint8_t*>(indexData);
+			return static_cast<uint32_t>(ptr[idx]);
+		} else if ( indexSize == sizeof(uint16_t) ) {
+			auto* ptr = reinterpret_cast<const uint16_t*>(indexData);
+			return static_cast<uint32_t>(ptr[idx]);
+		} else if ( indexSize == sizeof(uint32_t) ) {
+			auto* ptr = reinterpret_cast<const uint32_t*>(indexData);
+			return ptr[idx];
+		}
+		UF_EXCEPTION("Unsupported index type of size {}", indexSize);
+	}
+	pod::AABB computeTriangleAABB( const void* vertices, size_t vertexStride, const void* indexData, size_t indexSize, size_t triID ) {
+		auto triIndexID = triID * 3;
+
+		uint32_t i0 = ::getIndex( indexData, indexSize, triIndexID + 0 );
+		uint32_t i1 = ::getIndex( indexData, indexSize, triIndexID + 1 );
+		uint32_t i2 = ::getIndex( indexData, indexSize, triIndexID + 2 );
+
+		auto& v0 = *reinterpret_cast<const pod::Vector3f*>(reinterpret_cast<const uint8_t*>(vertices) + i0 * vertexStride);
+		auto& v1 = *reinterpret_cast<const pod::Vector3f*>(reinterpret_cast<const uint8_t*>(vertices) + i1 * vertexStride);
+		auto& v2 = *reinterpret_cast<const pod::Vector3f*>(reinterpret_cast<const uint8_t*>(vertices) + i2 * vertexStride);
+
+		return {
+			{	
+				std::min({v0.x, v1.x, v2.x}),
+				std::min({v0.y, v1.y, v2.y}),
+				std::min({v0.z, v1.z, v2.z}),
+			},
+			{
+				std::max({v0.x, v1.x, v2.x}),
+				std::max({v0.y, v1.y, v2.y}),
+				std::max({v0.z, v1.z, v2.z}),
+			}
+		};
+	}
+
+	pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID ) {
+		auto views = mesh.makeViews({"position", "normal"});
+		UF_ASSERT(!views.empty());
+
+		uint32_t triIndexID = triID * 3; // remap triangle ID to index ID
+		// find which view contains this triangle index.
+		const uf::Mesh::View* found = nullptr;
+		for ( auto& v : views ) {
+			if ( v.index.first <= triIndexID && triIndexID < v.index.first + v.index.count ) {
+				found = &v;
+				break;
+			}
+		}
+		UF_ASSERT( found );
+
+		pod::TriangleWithNormal tri;
+
+		auto& positions = (*found)["position"];
+		auto& normals   = (*found)["normal"];
+		auto& indices   = (*found)["index"];
+
+		const void* indexBase = indices.data(found->index.first);
+		size_t indexSize	  = mesh.index.size;
+
+		// reset back to local indices range
+		triIndexID -= found->index.first;
+
+		uint32_t idxs[3];
+		// to-do: just make this a macro that could have a parallel hint
+		for ( auto i = 0; i < 3; ++i ) idxs[i] = getIndex(indexBase, indexSize, triIndexID + i);
+
+		{
+			auto* base = reinterpret_cast<const uint8_t*>(positions.data(found->vertex.first));
+			size_t stride = positions.stride();
+
+			for ( auto i = 0; i < 3; ++i ) tri.points[i] = *reinterpret_cast<const pod::Vector3f*>(base + idxs[i] * stride);
+		}
+
+		if ( normals.valid() ) {
+			auto* base = reinterpret_cast<const uint8_t*>(normals.data(found->vertex.first));
+			size_t stride = normals.stride();
+			for ( auto i = 0; i < 3; ++i ) tri.normals[i] = *reinterpret_cast<const pod::Vector3f*>(base + idxs[i] * stride);
+		} else {
+			auto normal = uf::vector::normalize(uf::vector::cross(tri.points[1]-tri.points[0], tri.points[2]-tri.points[0]));
+			for ( auto i = 0; i < 3; ++i ) tri.normals[i] = normal;
+		}
+
+		return tri;
+	}
+
+	// if body is a mesh, apply its transform to the triangles, else reorient the normal with respect to the body
+	pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID, const pod::RigidBody& body ) {
+		auto tri = ::fetchTriangle( mesh, triID );
+		if ( body.collider.type == pod::ShapeType::MESH ) {
+			if ( body.transform ) {
+				for ( auto i = 0; i < 3; ++i ) {
+					tri.points[i]  = uf::transform::apply(*body.transform, tri.points[i]);
+					tri.normals[i] = uf::quaternion::rotate(body.transform->orientation, tri.normals[i]);
+				}
+			}
+		}
+		else {
+		#if REORIENT_NORMALS_ON_FETCH
+			auto triCenter = (tri.points[0] + tri.points[1] + tri.points[2]) / 3.0f;
+			auto delta = body.transform->position - triCenter;
+			for ( auto i = 0; i < 3; ++i ) {
+				if ( uf::vector::dot(tri.normals[i], delta) < 0.0f ) tri.normals[i] = -tri.normals[i];
+			}
+		#endif
+		}
+
+		return tri;
+	}
+
+	pod::Vector3f computeTriangleNormal( const pod::TriangleWithNormal& tri ) {
+		return uf::vector::normalize(( tri.normals[0] + tri.normals[1] + tri.normals[2] ) / 3.0f);
+	}
+}
+
+/*
+
+#if REORIENT_NORMALS_ON_CONTACT
+	if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal;
+#endif
+
+
+// uf::vector::normalize( ::interpolateWithBarycentric( ::computeBarycentric( contact, tri ), tri.normals ) );
+
+*/
+
+// triangle colliders
+namespace {
+	bool triangleTriangle( const pod::TriangleWithNormal& a, const pod::TriangleWithNormal& b, pod::Manifold& manifold, float eps ) {
+		if ( !::triangleTriangleIntersect( a, b ) ) return false;
+
+		// to-do: properly derive the contact information
+		auto contact = ( a.points[0] + b.points[0] ) * 0.5f; // center point
+		auto normal = uf::vector::normalize( uf::vector::cross( a.points[1] - a.points[0], a.points[2] - a.points[0] ) );
+		float penetration = 0.001f;
+		manifold.points.emplace_back(pod::Contact{ contact, normal, penetration });
+		return true;
+	}
+
+	bool triangleAabb( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps ) {
+		const auto& aabb = body;
+
+		auto closest = ::closestPointOnTriangle( ::aabbCenter( aabb.bounds ), tri );
+		auto closestAabb = ::closestPointOnAABB( closest, aabb.bounds );
+
+		if ( !uf::vector::isValid( closest ) ) return false;
+		
+		// to-do: derive proper delta
+		auto delta = closestAabb - closest;
+		float dist2 = uf::vector::dot( delta, delta );
+		float tolerance = 1.0e-3;
+		if ( dist2 >= tolerance ) return false;
+		float dist = std::sqrt( dist2 );
+
+		// to-do: properly derive the contact information
+		auto contact = closest;
+		auto normal = ( dist > eps ) ? ( delta / dist ) : ::computeTriangleNormal( tri );
+		float penetration = tolerance - dist;
+
+	#if REORIENT_NORMALS_ON_CONTACT
+		if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal;
+	#endif
+
+		manifold.points.emplace_back(pod::Contact{ contact, normal, penetration });
+		return true;
+	}
+	bool triangleSphere( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps ) {
+		const auto& sphere = body;
+
+		float r = sphere.collider.u.sphere.radius;
+		auto center = ::aabbCenter( sphere.bounds );
+		auto closest = ::closestPointOnTriangle( center, tri );
+
+		if ( !uf::vector::isValid( closest ) ) return false;
+
+		// to-do: derive proper delta
+		auto delta = center - closest;
+		float dist2 = uf::vector::dot(delta, delta);
+		if ( dist2 > r * r ) return false;
+		float dist = std::sqrt(dist2);
+
+		auto contact = closest;
+		auto normal = ( dist > eps ) ? ( delta / dist ) : ::computeTriangleNormal( tri );
+		float penetration = r - dist;
+
+	#if REORIENT_NORMALS_ON_CONTACT
+		if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal;
+	#endif
+
+		manifold.points.emplace_back(pod::Contact{ contact, normal, penetration });
+		return true;
+	}
+	// to-do
+	bool trianglePlane( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps ) {
+		return false;
+	}
+	bool triangleCapsule( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps ) {
+		const auto& capsule = body;
+
+		float r = capsule.collider.u.capsule.radius;
+		auto [ p1, p2 ] = ::getCapsuleSegment( capsule );
+		auto bounds = ::computeSegmentAABB( p1, p2, r );
+
+		// to-do: derive proper delta
+		pod::Vector3f closestSeg, closest;
+		float dist2 = ::segmentTriangleDistanceSq( p1, p2, tri, closestSeg, closest );
+
+		if ( !uf::vector::isValid( closest ) ) return false;
+		if ( dist2 > r * r ) return false;
+		float dist = std::sqrt( dist2 );
+		auto delta = ( closestSeg - closest );
+
+		// to-do: properly derive the contact information
+		auto contact = closest;
+		auto normal = ( dist > eps ) ? ( delta / dist ) : ::computeTriangleNormal( tri );
+		float penetration = r - dist;
+
+	#if REORIENT_NORMALS_ON_CONTACT
+		if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal;
+	#endif
+
+		manifold.points.emplace_back(pod::Contact{ contact, normal, penetration });
+		return true;
+	}
+}