lots of physics cleanup and fixes (split-impulse performs better than a NGS position correction somehow?)

bin/data/config.json

@@ -337,10 +337,11 @@
 		},
 		"physics": {
 			"warmup solver": true,
-			"block solver": true,
+			"block solver": false,
-			"resolve block solver": true,
+			"resolve block true": true,
 			"correction percent": 0.2,
-			"correction slop": 0.005,
+			"correction slop": 0.01,
+			"max correction": 0.01,
 			"gjk": false,
 			"debug draw": {
 				"dynamic": true

bin/data/entities/physics_prop.json

@@ -6,9 +6,9 @@
 	"metadata": {
 		"holdable": true,
 		"physics": {
-			"mass": 1,
+			"mass": 100,
 		//	"inertia": false,
-			"type": "bounding box"
+			"type": "obb"
 		//	"type": "mesh"
 		//	"type": "hull"
 		}

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

@@ -38,7 +38,8 @@ namespace impl {
 	pod::Vector3f closestPointOnTriangle( const pod::Vector3f& p, const pod::Triangle& tri );
 	pod::Vector3f orientNormalToAB( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Vector3f n );
 	float segmentTriangleDistanceSq( const pod::Vector3f& p0, const pod::Vector3f& p1, const pod::Triangle& tri, pod::Vector3f& outSeg, pod::Vector3f& outTri );
-	bool testSeparatingAxis( const pod::Triangle& triangle, const pod::AABB& box, const pod::Vector3f& axis, const pod::Vector3f axes[3], float& outMinOverlap, pod::Vector3f& outBestAxis );
+	bool testSeparatingAxis( const pod::Triangle& triangle, const pod::OBB& box, const pod::Vector3f& axis, const pod::Vector3f axes[3], float& outMinOverlap, pod::Vector3f& outBestAxis );
+	bool testSeparatingAxis( const pod::OBB& boxA, const pod::OBB& boxB, const pod::Vector3f axesA[3], const pod::Vector3f axesB[3], const pod::Vector3f& axis, float& outMinOverlap, pod::Vector3f& outBestAxis );
 	void clipPolygon( pod::Vector3f* poly, int& polyCount, const pod::Plane& plane );
 	void clipPolygon( pod::Vector3f* poly, int& polyCount, const pod::AABB& plane );
 	pod::Vector3f triangleCenter( const pod::Triangle& tri );
@@ -63,6 +64,14 @@ namespace impl {
 	/*FORCE_INLINE*/ pod::AABB mergeAabb( const pod::AABB& a, const pod::AABB& b );
 	/*FORCE_INLINE*/ pod::Vector3f aabbCenter( const pod::AABB& aabb );
 	/*FORCE_INLINE*/ pod::Vector3f aabbExtent( const pod::AABB& aabb );
+	/*FORCE_INLINE*/ pod::Vector3f obbMin( const pod::OBB& obb );
+	/*FORCE_INLINE*/ pod::Vector3f obbMax( const pod::OBB& obb );
+	/*FORCE_INLINE*/ pod::OBB aabbToObb( const pod::AABB& aabb );
+	/*FORCE_INLINE*/ pod::AABB obbToAabb( const pod::OBB& obb );
+	/*FORCE_INLINE*/ void boxAxes( pod::Vector3f axes[3] );
+	/*FORCE_INLINE*/ void boxAxes( pod::Vector3f axes[3], const pod::Transform<>& transform );
+	/*FORCE_INLINE*/ pod::Vector3f extentFromAxes( const pod::OBB& box, const pod::Vector3f axes[3] );
+	/*FORCE_INLINE*/ float projectExtents( const pod::OBB& box, const pod::Vector3f& normal, const pod::Vector3f axes[3] );
 	pod::AABB transformAabbToWorld( const pod::AABB& localBox, const pod::Transform<>& transform );
 	std::pair<pod::Vector3f, pod::Vector3f> getCapsuleSegment( const pod::PhysicsBody& body );
 	pod::AABB computeAABB( const pod::PhysicsBody& body );

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

@@ -289,6 +289,7 @@ namespace pod {
 		uint32_t solverIterations = 10;
 		float baumgarteCorrectionPercent = 0.2f;
 		float baumgarteCorrectionSlop = 0.005f;
+		float maxLinearCorrection = 0.2f;
 		
 		uf::stl::unordered_map<size_t, pod::Manifold> manifoldsCache;
 		uint32_t manifoldCacheLifetime = 0; // 0 = derive from current settings
@@ -371,6 +372,7 @@ namespace uf {
 		
 		pod::PhysicsBody& UF_API create( uf::Object&, float mass = 0.0f, const pod::Vector3f& = {} );
 		pod::PhysicsBody& UF_API create( uf::Object& object, const pod::AABB& aabb, float mass = 0.0f, const pod::Vector3f& = {} );
+		pod::PhysicsBody& UF_API create( uf::Object& object, const pod::OBB& obb, float mass = 0.0f, const pod::Vector3f& = {} );
 		pod::PhysicsBody& UF_API create( uf::Object& object, const pod::Sphere& sphere, float mass = 0.0f, const pod::Vector3f& = {} );
 		pod::PhysicsBody& UF_API create( uf::Object& object, const pod::Plane& plane, float mass = 0.0f, const pod::Vector3f& = {} );
 		pod::PhysicsBody& UF_API create( uf::Object& object, const pod::Capsule& capsule, float mass = 0.0f, const pod::Vector3f& = {} );
@@ -378,6 +380,7 @@ namespace uf {
 
 		pod::PhysicsBody& UF_API create( pod::World&, uf::Object&, float mass = 0.0f, const pod::Vector3f& = {} );
 		pod::PhysicsBody& UF_API create( pod::World&, uf::Object& object, const pod::AABB& aabb, float mass = 0.0f, const pod::Vector3f& = {} );
+		pod::PhysicsBody& UF_API create( pod::World&, uf::Object& object, const pod::OBB& obb, float mass = 0.0f, const pod::Vector3f& = {} );
 		pod::PhysicsBody& UF_API create( pod::World&, uf::Object& object, const pod::Sphere& sphere, float mass = 0.0f, const pod::Vector3f& = {} );
 		pod::PhysicsBody& UF_API create( pod::World&, uf::Object& object, const pod::Plane& plane, float mass = 0.0f, const pod::Vector3f& = {} );
 		pod::PhysicsBody& UF_API create( pod::World&, uf::Object& object, const pod::Capsule& capsule, float mass = 0.0f, const pod::Vector3f& = {} );

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

@@ -8,5 +8,5 @@
 namespace impl {
 	void resolveManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
 	void solveContacts( uf::stl::vector<pod::Manifold>& manifolds, float dt );
-	void solvePositions( uf::stl::vector<pod::Manifold>& manifolds, float dt, uint32_t iterations = 2 );
+	void solvePositions( uf::stl::vector<pod::Manifold>& manifolds, float dt, uint32_t iterations = 4 );
 }

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

@@ -12,9 +12,14 @@ namespace pod {
 	struct AABB {
 		alignas(16) pod::Vector3f min;
 		alignas(16) pod::Vector3f max;
+	//	operator OBB() const { return OBB{ (min + max) * 0.5f, (min - max) * 0.5f }; }
 	};
 
-	typedef AABB OBB;
+	struct OBB {
+		alignas(16) pod::Vector3f center;
+		alignas(16) pod::Vector3f extent;
+	//	operator AABB() const { return AABB{ center - extent, center + extent }; }
+	};
 
 	struct Sphere {
 		float radius;

engine/src/engine/ext/ext.cpp

@@ -212,6 +212,7 @@ void UF_API uf::load( ext::json::Value& json ) {
 		uf::physics::settings.solverIterations = configEnginePhysicsJson["solver iterations"].as(uf::physics::settings.solverIterations);
 		uf::physics::settings.baumgarteCorrectionPercent = configEnginePhysicsJson["correction percent"].as(uf::physics::settings.baumgarteCorrectionPercent);
 		uf::physics::settings.baumgarteCorrectionSlop = configEnginePhysicsJson["correction slop"].as(uf::physics::settings.baumgarteCorrectionSlop);
+		uf::physics::settings.maxLinearCorrection = configEnginePhysicsJson["max correction"].as(uf::physics::settings.maxLinearCorrection);
 		
 		auto& configEnginePhysicsDebugDrawJson = configEnginePhysicsJson["debug draw"];
 		if ( ext::json::isObject( configEnginePhysicsDebugDrawJson ) ) {

engine/src/engine/graph/graph.cpp

@@ -1321,10 +1321,10 @@ void uf::graph::process( pod::Graph& graph, int32_t index, uf::Object& parent )
 					auto max = bounds.max; // uf::matrix::multiply<float>( model, bounds.max, 1.0f );
 
 					pod::Vector3f center = (max + min) * 0.5f;
-					pod::Vector3f corner = uf::vector::abs(max - min) * 0.5f;
+					pod::Vector3f extent = uf::vector::abs(max - min) * 0.5f;
 
 					if ( ext::json::isNull( metadataJson["physics"]["center"] ) ) metadataJson["physics"]["center"] = uf::vector::encode( center );
-					if ( ext::json::isNull( metadataJson["physics"]["corner"] ) ) metadataJson["physics"]["corner"] = uf::vector::encode( corner );
+					if ( ext::json::isNull( metadataJson["physics"]["extent"] ) ) metadataJson["physics"]["extent"] = uf::vector::encode( extent );
 					if ( ext::json::isNull( metadataJson["physics"]["min"] ) ) metadataJson["physics"]["min"] = uf::vector::encode( min );
 					if ( ext::json::isNull( metadataJson["physics"]["max"] ) ) metadataJson["physics"]["max"] = uf::vector::encode( max );
 				}

engine/src/engine/object/behavior.cpp

@@ -137,13 +137,22 @@ void uf::ObjectBehavior::initialize( uf::Object& self ) {
 		bool recenter = metadataJsonPhysics["recenter"].as<bool>();
 		pod::Vector3f offset = uf::vector::decode( metadataJsonPhysics["offset"], pod::Vector3f{} );
 
-		if ( offset == pod::Vector3f{} ) recenter = true;
+	//	if ( offset == pod::Vector3f{} ) recenter = true;
 
-		if ( type == "bounding box" || type == "aabb" ) {
+		if ( type == "bounding box" || type == "box" || type == "obb" ) {
+			pod::Vector3f center = uf::vector::decode( metadataJsonPhysics["center"], pod::Vector3f{0.0f, 0.0f, 0.0f} );
+			pod::Vector3f extent = uf::vector::decode( metadataJsonPhysics["extent"], pod::Vector3f{0.5f, 0.5f, 0.5f} );
+			
+			if ( recenter ) {
+				offset = center;
+				center = {};
+			}
+
+			uf::physics::create( self, pod::OBB{ .center = center, .extent = extent }, mass, offset );
+		} else if ( type == "aabb" ) {
 			pod::Vector3f min = uf::vector::decode( metadataJsonPhysics["min"], pod::Vector3f{-0.5f, -0.5f, -0.5f} );
 			pod::Vector3f max = uf::vector::decode( metadataJsonPhysics["max"], pod::Vector3f{0.5f, 0.5f, 0.5f} );
 
-			// recenter
 			if ( recenter ) {
 				pod::Vector3f center = (max + min) * 0.5f;
 				pod::Vector3f extents = (max - min) * 0.5f;

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

@@ -382,7 +382,7 @@ float impl::segmentTriangleDistanceSq( const pod::Vector3f& p0, const pod::Vecto
 }
 
 // Separating Axis Theorem test
-bool impl::testSeparatingAxis( const pod::Triangle& triangle, const pod::AABB& box, const pod::Vector3f& axis, const pod::Vector3f axes[3], float& outMinOverlap, pod::Vector3f& outBestAxis ) {
+bool impl::testSeparatingAxis( const pod::Triangle& triangle, const pod::OBB& box, const pod::Vector3f& axis, const pod::Vector3f axes[3], float& outMinOverlap, pod::Vector3f& outBestAxis ) {
 	float mag = uf::vector::magnitude(axis);
 	if ( mag < EPS2 ) return true;
 	pod::Vector3f n = axis / mag;
@@ -395,12 +395,8 @@ bool impl::testSeparatingAxis( const pod::Triangle& triangle, const pod::AABB& b
 	float maxT = std::max( { p0, p1, p2 } );
 
 	// project box
-	pod::Vector3f cB = impl::aabbCenter( box );
+	float pB = uf::vector::dot( box.center, n );
-	pod::Vector3f eB = impl::aabbExtent( box );
+	float rB = impl::projectExtents( box, n, axes );
-	float pB = uf::vector::dot(cB, n);
-	float rB = eB.x * std::fabs(uf::vector::dot(axes[0], n)) +
-			   eB.y * std::fabs(uf::vector::dot(axes[1], n)) +
-			   eB.z * std::fabs(uf::vector::dot(axes[2], n));
 
 	float minB = pB - rB;
 	float maxB = pB + rB;
@@ -416,6 +412,28 @@ bool impl::testSeparatingAxis( const pod::Triangle& triangle, const pod::AABB& b
 	}
 	return true;
 }
+bool impl::testSeparatingAxis( const pod::OBB& boxA, const pod::OBB& boxB, const pod::Vector3f axesA[3], const pod::Vector3f axesB[3], const pod::Vector3f& axis, float& outMinOverlap, pod::Vector3f& outBestAxis ) {
+	float mag = uf::vector::magnitude(axis);
+	if ( mag < EPS2 ) return true;
+	pod::Vector3f n = axis / mag;
+
+	float pA = uf::vector::dot( boxA.center, n );
+	float rA = impl::projectExtents( boxA, n, axesA );
+
+	float pB = uf::vector::dot( boxB.center, n );
+	float rB = impl::projectExtents( boxB, n, axesB );
+
+	float dist = std::fabs( pB - pA );
+	float overlap = ( rA + rB ) - dist;
+
+	if ( overlap < 0.0f ) return false;
+
+	if ( overlap < outMinOverlap ) {
+		outMinOverlap = overlap;
+		outBestAxis = n;
+	}
+	return true;
+}
 
 // Sutherland-Hodgman polygon clipping
 void impl::clipPolygon( pod::Vector3f* poly, int& polyCount, const pod::Plane& plane ) {
@@ -649,29 +667,63 @@ pod::Vector3f impl::aabbCenter( const pod::AABB& aabb ) {
 pod::Vector3f impl::aabbExtent( const pod::AABB& aabb ) {
 	return ( aabb.max - aabb.min ) * 0.5f;
 }
+// returns the min bound of an OBB
+pod::Vector3f impl::obbMin( const pod::OBB& obb ) {
+	return obb.center - obb.extent;
+}
+// returns the max bound of an OBB
+pod::Vector3f impl::obbMax( const pod::OBB& obb ) {
+	return obb.center + obb.extent;
+}
+// converts a min-max AABB to center-extents OBB
+pod::OBB impl::aabbToObb( const pod::AABB& aabb ) {
+	return pod::OBB{
+		.center = impl::aabbCenter( aabb ),
+		.extent = impl::aabbExtent( aabb ),
+	};
+}
+// converts a center-extents OBB to min-max AABB
+pod::AABB impl::obbToAabb( const pod::OBB& obb ) {
+	return pod::AABB{
+		.min = impl::obbMin( obb ),
+		.max = impl::obbMax( obb ),
+	};
+}
+// returns AABB axes
+void impl::boxAxes( pod::Vector3f axes[3] ) {
+	axes[0] = {1,0,0};
+	axes[1] = {0,1,0};
+	axes[2] = {0,0,1};
+}
+// returns OBB axes
+void impl::boxAxes( pod::Vector3f axes[3], const pod::Transform<>& transform ) {
+	axes[0] = uf::quaternion::rotate(transform.orientation, pod::Vector3f{1,0,0});
+	axes[1] = uf::quaternion::rotate(transform.orientation, pod::Vector3f{0,1,0});
+	axes[2] = uf::quaternion::rotate(transform.orientation, pod::Vector3f{0,0,1});
+}
+// computes a box's extents from given axes
+pod::Vector3f impl::extentFromAxes( const pod::OBB& box, const pod::Vector3f axes[3] ) {
+	return ( uf::vector::abs(axes[0]) * box.extent.x ) + ( uf::vector::abs(axes[1]) * box.extent.y ) + ( uf::vector::abs(axes[2]) * box.extent.z );
+}
+//
+float impl::projectExtents( const pod::OBB& box, const pod::Vector3f& normal, const pod::Vector3f axes[3] ) {
+    return uf::vector::dot(box.extent, uf::vector::abs( pod::Vector3f{
+        uf::vector::dot(axes[0], normal),
+        uf::vector::dot(axes[1], normal),
+        uf::vector::dot(axes[2], normal)
+    } ) );
+//	return box.extent.x * std::fabs(uf::vector::dot(axes[0], normal)) + box.extent.y * std::fabs(uf::vector::dot(axes[1], normal)) + box.extent.z * std::fabs(uf::vector::dot(axes[2], normal));
+}
 // transforms an AABB into world-space
 pod::AABB impl::transformAabbToWorld( const pod::AABB& aabb, const pod::Transform<>& transform ) {
-	// to-do: flatten, since transform might not be flattened (even though getTransform does that)
+	auto box = impl::aabbToObb( aabb );
-	const auto& q = transform.orientation;
+	pod::Vector3f axes[3];
-	const auto& p = transform.position;
+	impl::boxAxes( axes, transform );
 
-	pod::Vector3f cB  = impl::aabbCenter( aabb );
+	pod::Vector3f center = uf::quaternion::rotate(transform.orientation, box.center) + transform.position;
-	pod::Vector3f eB = impl::aabbExtent( aabb );
+	pod::Vector3f extent = impl::extentFromAxes( box, axes );
-	pod::Vector3f axes[] = {
-		uf::quaternion::rotate(q, pod::Vector3f{1,0,0}),
-		uf::quaternion::rotate(q, pod::Vector3f{0,1,0}),
-		uf::quaternion::rotate(q, pod::Vector3f{0,0,1}),
-	};
 
-	pod::Vector3f cW = uf::quaternion::rotate(q, cB) + p;
+	return { center - extent, center + extent };
-	// to-do: a cleaner way of doing this with uf::vector::abs
-	pod::Vector3f cE = {
-		fabs(axes[0].x) * eB.x + fabs(axes[1].x) * eB.y + fabs(axes[2].x) * eB.z,
-		fabs(axes[0].y) * eB.x + fabs(axes[1].y) * eB.y + fabs(axes[2].y) * eB.z,
-		fabs(axes[0].z) * eB.x + fabs(axes[1].z) * eB.y + fabs(axes[2].z) * eB.z
-	};
-
-	return { cW - cE, cW + cE };
 }
 // returns the line segment of a capsule
 std::pair<pod::Vector3f, pod::Vector3f> impl::getCapsuleSegment( const pod::PhysicsBody& body ) {
@@ -688,15 +740,11 @@ std::pair<pod::Vector3f, pod::Vector3f> impl::getCapsuleSegment( const pod::Phys
 pod::AABB impl::computeAABB( const pod::PhysicsBody& body ) {
 	const auto transform = impl::getTransform( body );
 	switch ( body.collider.type ) {
-		case pod::ShapeType::AABB:
+		case pod::ShapeType::AABB: {
-		case pod::ShapeType::OBB: {
 			return impl::transformAabbToWorld( body.collider.aabb, transform );
-		/*
+		} break;
-			return {
+		case pod::ShapeType::OBB: {
-				transform.position + body.collider.aabb.min,
+			return impl::transformAabbToWorld( impl::obbToAabb( body.collider.obb ), transform );
-				transform.position + body.collider.aabb.max,
-			};
-		*/
 		} break;
 		case pod::ShapeType::SPHERE: {
 			return {

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

@@ -190,7 +190,7 @@ void uf::physics::step( pod::World& world, float dt ) {
 		// pass manifolds to solver
 		impl::solveContacts( manifolds, dt );
 		// do position correction
-		//impl::solvePositions( manifolds, dt );
+		impl::solvePositions( manifolds, dt );
 		// cache manifold positions
 		if ( uf::physics::settings.warmupSolver ) {
 			impl::updateManifoldCache( manifolds, uf::physics::settings.manifoldsCache );
@@ -265,14 +265,20 @@ void uf::physics::updateInertia( pod::PhysicsBody& body ) {
 	}
 
 	switch ( body.collider.type ) {
-		case pod::ShapeType::AABB: 
+		case pod::ShapeType::AABB: {
-		case pod::ShapeType::OBB: {
 			pod::Vector3f dims = (body.collider.aabb.max - body.collider.aabb.min);
 			pod::Vector3f dimsSq = dims * dims;
 			body.inertiaTensor = pod::Vector3f{ dimsSq.y + dimsSq.z, dimsSq.x + dimsSq.z, dimsSq.x + dimsSq.y } * (body.mass / 12.0f);
 			body.inertiaTensor = uf::vector::max( body.inertiaTensor, { EPS, EPS, EPS } );
 			body.inverseInertiaTensor = 1.0f / body.inertiaTensor;
 		} break;
+		case pod::ShapeType::OBB: {
+			pod::Vector3f dims = body.collider.obb.extent * 2.0f;
+			pod::Vector3f dimsSq = dims * dims;
+			body.inertiaTensor = pod::Vector3f{ dimsSq.y + dimsSq.z, dimsSq.x + dimsSq.z, dimsSq.x + dimsSq.y } * (body.mass / 12.0f);
+			body.inertiaTensor = uf::vector::max( body.inertiaTensor, { EPS, EPS, EPS } );
+			body.inverseInertiaTensor = 1.0f / body.inertiaTensor;
+		} break;
 		case pod::ShapeType::SPHERE: {
 			float I = 0.4f * body.mass * body.collider.sphere.radius * body.collider.sphere.radius;
 			float invI = 1.0f / I;
@@ -464,14 +470,22 @@ pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, fl
 }
 pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, const pod::AABB& aabb, float mass, const pod::Vector3f& offset ) {
 	auto& body = uf::physics::create( world, object, mass, offset );
-	//body.collider.type = pod::ShapeType::AABB;
+	body.collider.type = pod::ShapeType::AABB;
-	body.collider.type = pod::ShapeType::OBB;
 	body.collider.aabb = aabb;
 	body.bounds = impl::computeAABB( body );
 
 	uf::physics::updateInertia( body );
 	return body;
 }
+pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, const pod::OBB& obb, float mass, const pod::Vector3f& offset ) {
+	auto& body = uf::physics::create( world, object, mass, offset );
+	body.collider.type = pod::ShapeType::OBB;
+	body.collider.obb = obb;
+	body.bounds = impl::computeAABB( body );
+
+	uf::physics::updateInertia( body );
+	return body;
+}
 pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, const pod::Sphere& sphere, float mass, const pod::Vector3f& offset ) {
 	auto& body = uf::physics::create( world, object, mass, offset );
 	body.collider.type = pod::ShapeType::SPHERE;
@@ -541,6 +555,11 @@ pod::PhysicsBody& uf::physics::create( uf::Object& object, const pod::AABB& aabb
 	auto& world = scene.getComponent<pod::World>();
 	return create( world, object, aabb, mass, offset );
 }
+pod::PhysicsBody& uf::physics::create( uf::Object& object, const pod::OBB& obb, float mass, const pod::Vector3f& offset ) {
+	auto& scene = uf::scene::getCurrentScene();
+	auto& world = scene.getComponent<pod::World>();
+	return create( world, object, obb, mass, offset );
+}
 pod::PhysicsBody& uf::physics::create( uf::Object& object, const pod::Sphere& sphere, float mass, const pod::Vector3f& offset ) {
 	auto& scene = uf::scene::getCurrentScene();
 	auto& world = scene.getComponent<pod::World>();

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

@@ -23,10 +23,8 @@ float impl::computeEffectiveMass( pod::PhysicsBody& a, pod::PhysicsBody& b, cons
 		angularTermB = uf::vector::dot(uf::vector::cross(I_crossB, rB), n);
 	}
 
-	float result = inverseMass + angularTermA + angularTermB;
 	// to-do: assert / handle result == 0 to avoid division by zero (this probably only would happen with two static bodies colliding, which never should happen)
-	if ( result < EPS ) result = 1.0f;
+	return inverseMass + angularTermA + angularTermB;
-	return result;
 }
 
 void impl::applyImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& impulse ) {
@@ -41,7 +39,6 @@ void impl::applyImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::
 		b.angularVelocity += uf::matrix::multiply( invIb, uf::vector::cross(rB, impulse) );
 	}
 }
-
 void impl::applyPseudoImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& impulse ) {
 	if ( !a.isStatic ) {
 		a.pseudoVelocity -= impulse * a.inverseMass;
@@ -324,14 +321,12 @@ void impl::retrieveContacts( pod::Manifold& current, const pod::Manifold& previo
 
 		validContact.accumulatedNormalImpulse *= decay;
 		validContact.accumulatedTangentImpulse *= decay;
-		validContact.accumulatedPseudoImpulse = 0.0f;
 
 		bool isDuplicate = false;
-		for ( auto& c : current.points ) {
+		for ( auto& c : merged ) {
 			if ( impl::similarContact( validContact, c ) ) {
 				c.accumulatedNormalImpulse = validContact.accumulatedNormalImpulse;
 				c.accumulatedTangentImpulse = validContact.accumulatedTangentImpulse;
-				c.accumulatedPseudoImpulse = validContact.accumulatedPseudoImpulse;
 				c.lifetime = validContact.lifetime;
 				isDuplicate = true;
 				break;
@@ -474,15 +469,18 @@ void impl::integrate( pod::PhysicsBody& body, float dt ) {
 		uf::transform::rotate( *body.transform/*.reference*/, dq );
 	}
 
-	// split impulse updates
+	// pseudo-impulse position correction
 	{
 		body.transform->position += body.pseudoVelocity * dt;
 
 		float pseudoAngularSpeed2 = uf::vector::magnitude( body.pseudoAngularVelocity );
 		if ( pseudoAngularSpeed2 > EPS ) {
 			float pseudoAngularSpeed = std::sqrt( pseudoAngularSpeed2 );
-			pod::Quaternion<> dq = uf::quaternion::axisAngle( body.pseudoAngularVelocity / pseudoAngularSpeed, pseudoAngularSpeed * dt );
+			pod::Vector3f axis = body.pseudoAngularVelocity / pseudoAngularSpeed;
-			uf::transform::rotate( *body.transform/*.reference*/, dq );
+
+			float clampedSpeed = std::min(pseudoAngularSpeed, (2.0f * M_PI / 180.0f) / dt);
+			pod::Quaternion<> dq = uf::quaternion::axisAngle( axis, clampedSpeed * dt );
+			uf::transform::rotate( *body.transform, dq );
 		}
 		
 		// reset

engine/src/utils/math/physics/narrowphase/epa.cpp

@@ -50,13 +50,46 @@ void impl::getSupportFace( const pod::PhysicsBody& body, const pod::Vector3f& di
 				outPoly[i] = hasTransform ? uf::transform::apply( transform, body.collider.triangle.points[i] ) : body.collider.triangle.points[i];
 			});
 		} break;
-		case pod::ShapeType::AABB:
+		case pod::ShapeType::AABB: {
+			outCount = 4;
+			pod::Vector3f n = localDir;
+			pod::Vector3f absN = uf::vector::abs( n );
+			pod::Vector3f min = body.collider.aabb.min;
+			pod::Vector3f max = body.collider.aabb.max;
+
+			// dominant axis
+			if ( absN.x > absN.y && absN.x > absN.z ) {
+				float x = (n.x > 0) ? max.x : min.x;
+				outPoly[0] = {x, min.y, max.z};
+				outPoly[1] = {x, min.y, min.z};
+				outPoly[2] = {x, max.y, min.z};
+				outPoly[3] = {x, max.y, max.z};
+				if ( n.x < 0 ) std::swap(outPoly[1], outPoly[3]);
+			} else if ( absN.y > absN.z ) {
+				float y = (n.y > 0) ? max.y : min.y;
+				outPoly[0] = {max.x, y, max.z};
+				outPoly[1] = {max.x, y, min.z};
+				outPoly[2] = {min.x, y, min.z};
+				outPoly[3] = {min.x, y, max.z};
+				if ( n.y < 0 ) std::swap(outPoly[1], outPoly[3]);
+			} else {
+				float z = (n.z > 0) ? max.z : min.z;
+				outPoly[0] = {min.x, max.y, z};
+				outPoly[1] = {min.x, min.y, z};
+				outPoly[2] = {max.x, min.y, z};
+				outPoly[3] = {max.x, max.y, z};
+				if (n.z < 0) std::swap(outPoly[1], outPoly[3]);
+			}
+			FOR_EACH(4, {
+				outPoly[i] = uf::transform::apply(transform, outPoly[i]);
+			});
+		} break;
 		case pod::ShapeType::OBB: {
 			outCount = 4;
 			pod::Vector3f n = localDir;
-			pod::Vector3f absN = { std::fabs(n.x), std::fabs(n.y), std::fabs(n.z) };
+			pod::Vector3f absN = uf::vector::abs( n );
-			pod::Vector3f min = body.collider.aabb.min;
+			pod::Vector3f min = body.collider.obb.center - body.collider.obb.extent;
-			pod::Vector3f max = body.collider.aabb.max;
+			pod::Vector3f max = body.collider.obb.center + body.collider.obb.extent;
 
 			// dominant axis
 			if ( absN.x > absN.y && absN.x > absN.z ) {

engine/src/utils/math/physics/narrowphase/gjk.cpp

@@ -12,11 +12,12 @@ pod::Vector3f impl::support( const pod::PhysicsBody& body, const pod::Vector3f&
 			};
 		} break;
 		case pod::ShapeType::OBB: {
+			auto box = impl::obbToAabb( body.collider.obb );
 			pod::Vector3f localDir = uf::quaternion::rotate( uf::quaternion::inverse(transform.orientation), dir );
 			pod::Vector3f localPt = {
-				( localDir.x >= 0.0f ) ? body.collider.obb.max.x : body.collider.obb.min.x,
+				( localDir.x >= 0.0f ) ? box.max.x : box.min.x,
-				( localDir.y >= 0.0f ) ? body.collider.obb.max.y : body.collider.obb.min.y,
+				( localDir.y >= 0.0f ) ? box.max.y : box.min.y,
-				( localDir.z >= 0.0f ) ? body.collider.obb.max.z : body.collider.obb.min.z
+				( localDir.z >= 0.0f ) ? box.max.z : box.min.z
 			};
 			return uf::transform::apply( transform, localPt );
 		} break;

engine/src/utils/math/physics/narrowphase/obb.cpp

@@ -1,154 +1,182 @@
 #include <uf/utils/math/physics/common.h>
 #include <uf/utils/math/physics/narrowphase.h>
 
+namespace impl {
+	void getIncidentFace( const pod::OBB& obb, const pod::Vector3f* axes, const pod::Vector3f& normal, pod::Vector3f* outPoly ) {
+		pod::Vector3f n = -normal;
+		pod::Vector3f absN = uf::vector::abs(pod::Vector3f{
+			uf::vector::dot(n, axes[0]),
+			uf::vector::dot(n, axes[1]),
+			uf::vector::dot(n, axes[2])
+		});
+
+		int maxAxis = 0;
+		if ( absN.y > absN.x ) maxAxis = 1;
+		if ( absN.z > absN.x && absN.z > absN.y ) maxAxis = 2;
+
+		pod::Vector3f axis = axes[maxAxis];
+		if ( uf::vector::dot(n, axis) < 0.0f ) axis = -axis;
+
+		pod::Vector3f center = obb.center + axis * ((maxAxis == 0) ? obb.extent.x : (maxAxis == 1) ? obb.extent.y : obb.extent.z);
+
+		int a1 = (maxAxis + 1) % 3;
+		int a2 = (maxAxis + 2) % 3;
+
+		float ext1 = (a1 == 0) ? obb.extent.x : (a1 == 1) ? obb.extent.y : obb.extent.z;
+		float ext2 = (a2 == 0) ? obb.extent.x : (a2 == 1) ? obb.extent.y : obb.extent.z;
+
+		outPoly[0] = center + axis[a1] * ext1 + axis[a2] * ext2;
+		outPoly[1] = center - axis[a1] * ext1 + axis[a2] * ext2;
+		outPoly[2] = center - axis[a1] * ext1 - axis[a2] * ext2;
+		outPoly[3] = center + axis[a1] * ext1 - axis[a2] * ext2;
+	}
+
+	bool boxBox( const pod::OBB& boxA, const pod::OBB& boxB, const pod::Vector3f* axesA, const pod::Vector3f* axesB, pod::Manifold& manifold ) {
+		float minOverlap = FLT_MAX;
+		pod::Vector3f bestAxis;
+
+		for ( int i = 0; i < 3; ++i ) {
+			if ( !impl::testSeparatingAxis(boxA, boxB, axesA, axesB, axesA[i], minOverlap, bestAxis) ) return false;
+			if ( !impl::testSeparatingAxis(boxA, boxB, axesA, axesB, axesB[i], minOverlap, bestAxis) ) return false;
+		}
+
+		for ( int i = 0; i < 3; ++i ) {
+			for ( int j = 0; j < 3; j++ ) {
+				pod::Vector3f axis = uf::vector::cross(axesA[i], axesB[j]);
+				if ( !impl::testSeparatingAxis(boxA, boxB, axesA, axesB, axis, minOverlap, bestAxis) ) return false;
+			}
+		}
+
+		if ( uf::vector::dot(bestAxis, boxB.center - boxA.center) < 0.0f ) bestAxis = -bestAxis;
+		
+	#if 1
+		pod::Vector3f contactPoint = boxA.center + bestAxis * impl::projectExtents( boxA, bestAxis, axesA );
+		manifold.points.emplace_back( pod::Contact{ contactPoint, bestAxis, minOverlap } );
+	#else
+		auto refBox = boxA;
+		auto incBox = boxB;
+		auto* refAxes = axesA;
+		auto* incAxes = axesB;
+		bool isARef = false;
+
+		float maxDot = -1.0f;
+		for ( int i = 0; i < 3; i++ ) {
+			float dotA = std::fabs(uf::vector::dot(axesA[i], bestAxis));
+			float dotB = std::fabs(uf::vector::dot(axesB[i], bestAxis));
+			if ( dotA > maxDot ) { maxDot = dotA; isARef = true; }
+			if ( dotB > maxDot ) { maxDot = dotB; isARef = false; }
+		}
+
+		if ( !isARef ) {
+			refBox = boxB;
+			incBox = boxA;
+			refAxes = axesB;
+			incAxes = axesA;
+		}
+
+		int polyCount = 4;
+		pod::Vector3f poly[8];
+		pod::Vector3f incNormal = isARef ? bestAxis : -bestAxis;
+		impl::getIncidentFace( incBox, incAxes, incNormal, poly );
+
+		int refAxisIdx = 0;
+		float maxRefDot = -1.0f;
+		for ( int i = 0; i < 3; i++ ) {
+			float d = std::fabs(uf::vector::dot(refAxes[i], bestAxis));
+			if ( d > maxRefDot ) { maxRefDot = d; refAxisIdx = i; }
+		}
+
+		pod::Vector3f refFaceNormal = refAxes[refAxisIdx];
+		if ( uf::vector::dot(refFaceNormal, isARef ? bestAxis : -bestAxis ) < 0.0f) {
+			refFaceNormal = -refFaceNormal;
+		}
+
+		int a1 = ( refAxisIdx + 1 ) % 3;
+		int a2 = ( refAxisIdx + 2 ) % 3;
+		float ext1 = ( a1 == 0 ) ? refBox.extent.x : ( a1 == 1 ) ? refBox.extent.y : refBox.extent.z;
+		float ext2 = ( a2 == 0 ) ? refBox.extent.x : ( a2 == 1 ) ? refBox.extent.y : refBox.extent.z;
+
+		impl::clipPolygon( poly, polyCount, pod::Plane{  refAxes[a1], ext1 + uf::vector::dot(refAxes[a1], refBox.center) });
+		impl::clipPolygon( poly, polyCount, pod::Plane{ -refAxes[a1], ext1 - uf::vector::dot(refAxes[a1], refBox.center) });
+		impl::clipPolygon( poly, polyCount, pod::Plane{  refAxes[a2], ext2 + uf::vector::dot(refAxes[a2], refBox.center) });
+		impl::clipPolygon( poly, polyCount, pod::Plane{ -refAxes[a2], ext2 - uf::vector::dot(refAxes[a2], refBox.center) });
+
+		if ( polyCount == 0 ) return false;
+
+		float refExt = (refAxisIdx == 0) ? refBox.extent.x : (refAxisIdx == 1) ? refBox.extent.y : refBox.extent.z;
+		pod::Vector3f refFaceCenter = refBox.center + refFaceNormal * refExt;
+		float referenceOffset = uf::vector::dot(refFaceNormal, refFaceCenter);
+
+		for ( auto i = 0; i < polyCount; i++ ) {
+			float depth = referenceOffset - uf::vector::dot(refFaceNormal, poly[i]);
+			if ( depth >= 0.0f ) {
+				manifold.points.emplace_back( pod::Contact{ poly[i], bestAxis, depth } );
+			}
+		}
+	#endif
+
+		return manifold.points.size() > 0;
+	}
+}
+
 bool impl::obbObb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold ) {
 	ASSERT_COLLIDER_TYPES( OBB, OBB );
 
 	auto tA = impl::getTransform( a );
 	auto tB = impl::getTransform( b );
 
-	pod::Vector3f cA = uf::transform::apply( tA, (a.collider.obb.max + a.collider.obb.min) * 0.5f );
+	auto boxA = a.collider.obb;
-	pod::Vector3f cB = uf::transform::apply( tB, (b.collider.obb.max + b.collider.obb.min) * 0.5f );
+	auto boxB = b.collider.obb;
-	pod::Vector3f eA = (a.collider.obb.max - a.collider.obb.min) * 0.5f;
-	pod::Vector3f eB = (b.collider.obb.max - b.collider.obb.min) * 0.5f;
 
-	pod::Vector3f axesA[3] = {
+	boxA.center = uf::quaternion::rotate(tA.orientation, boxA.center) + tA.position;
-		uf::quaternion::rotate(tA.orientation, pod::Vector3f{1,0,0}),
+	boxB.center = uf::quaternion::rotate(tB.orientation, boxB.center) + tB.position;
-		uf::quaternion::rotate(tA.orientation, pod::Vector3f{0,1,0}),
-		uf::quaternion::rotate(tA.orientation, pod::Vector3f{0,0,1})
-	};
-	pod::Vector3f axesB[3] = {
-		uf::quaternion::rotate(tB.orientation, pod::Vector3f{1,0,0}),
-		uf::quaternion::rotate(tB.orientation, pod::Vector3f{0,1,0}),
-		uf::quaternion::rotate(tB.orientation, pod::Vector3f{0,0,1})
-	};
 
-	float minOverlap = FLT_MAX;
+	pod::Vector3f axesA[3];
-	pod::Vector3f bestAxis;
+	pod::Vector3f axesB[3];
+	impl::boxAxes( axesA, tA );
+	impl::boxAxes( axesB, tB );
 
-	auto testAxis = [&](const pod::Vector3f& axis) -> bool {
+	return impl::boxBox( boxA, boxB, axesA, axesB, manifold );
-		float mag = uf::vector::magnitude(axis);
-		if (mag < EPS) return true;
-		pod::Vector3f n = axis / mag;
-
-		float pA = uf::vector::dot(cA, n);
-		float rA = eA.x * std::fabs(uf::vector::dot(axesA[0], n)) +
-				   eA.y * std::fabs(uf::vector::dot(axesA[1], n)) +
-				   eA.z * std::fabs(uf::vector::dot(axesA[2], n));
-
-		float pB = uf::vector::dot(cB, n);
-		float rB = eB.x * std::fabs(uf::vector::dot(axesB[0], n)) +
-				   eB.y * std::fabs(uf::vector::dot(axesB[1], n)) +
-				   eB.z * std::fabs(uf::vector::dot(axesB[2], n));
-
-		float dist = std::fabs(pB - pA);
-		float overlap = (rA + rB) - dist;
-
-		if ( overlap < 0) return false;
-
-		if ( overlap < minOverlap ) {
-			minOverlap = overlap;
-			bestAxis = n;
-		}
-		return true;
-	};
-
-	for ( auto i = 0; i < 3; ++i ) if ( !testAxis(axesA[i]) ) return false;
-	for ( auto i = 0; i < 3; ++i ) if ( !testAxis(axesB[i]) ) return false;
-	for ( auto i = 0; i < 3; ++i ) {
-		for ( auto  j = 0; j < 3; j++ ) if ( !testAxis(uf::vector::cross(axesA[i], axesB[j])) ) return false;
-	};
-
-	if ( uf::vector::dot(bestAxis, cB - cA) < 0.0f ) bestAxis = -bestAxis;
-
-	// to-do: generate contact face
-	pod::Vector3f contactPoint = cA + bestAxis * (eA.x * std::fabs(uf::vector::dot(axesA[0], bestAxis)) +
-												  eA.y * std::fabs(uf::vector::dot(axesA[1], bestAxis)) +
-												  eA.z * std::fabs(uf::vector::dot(axesA[2], bestAxis)));
-
-	manifold.points.emplace_back( pod::Contact{ contactPoint, bestAxis, minOverlap } );
-	return true;
 }
 
 
 bool impl::obbAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold ) {
-	ASSERT_COLLIDER_TYPES( OBB, AABB );
+	ASSERT_COLLIDER_TYPES( OBB, OBB );
 
 	auto tA = impl::getTransform( a );
+	auto tB = impl::getTransform( b );
 
-	pod::Vector3f cA = uf::transform::apply( tA, (a.collider.obb.max + a.collider.obb.min) * 0.5f );
+	auto boxA = a.collider.obb;
-	pod::Vector3f eA = (a.collider.obb.max - a.collider.obb.min) * 0.5f;
+	auto boxB = impl::aabbToObb( b.bounds );
-	pod::Vector3f axesA[3] = {
+	boxA.center = uf::quaternion::rotate(tA.orientation, boxA.center) + tA.position;
-		uf::quaternion::rotate(tA.orientation, pod::Vector3f{1,0,0}),
-		uf::quaternion::rotate(tA.orientation, pod::Vector3f{0,1,0}),
-		uf::quaternion::rotate(tA.orientation, pod::Vector3f{0,0,1})
-	};
 
-	pod::Vector3f cB = impl::aabbCenter( b.bounds );
+	pod::Vector3f axesA[3];
-	pod::Vector3f eB = impl::aabbExtent( b.bounds );
+	pod::Vector3f axesB[3];
-	pod::Vector3f axesB[3] = { {1,0,0}, {0,1,0}, {0,0,1} };
+	impl::boxAxes( axesA, tA );
+	impl::boxAxes( axesB );
 
-	float minOverlap = FLT_MAX;
+	return impl::boxBox( boxA, boxB, axesA, axesB, manifold );
-	pod::Vector3f bestAxis;
-
-	auto testAxis = [&](const pod::Vector3f& axis) -> bool {
-		float mag = uf::vector::magnitude(axis);
-		if ( mag < EPS ) return true;
-		pod::Vector3f n = axis / mag;
-
-		float pA = uf::vector::dot(cA, n);
-		float rA = eA.x * std::fabs(uf::vector::dot(axesA[0], n)) +
-				   eA.y * std::fabs(uf::vector::dot(axesA[1], n)) +
-				   eA.z * std::fabs(uf::vector::dot(axesA[2], n));
-
-		float pB = uf::vector::dot(cB, n);
-		float rB = eB.x * std::fabs(n.x) + eB.y * std::fabs(n.y) + eB.z * std::fabs(n.z);
-
-		float dist = std::fabs(pB - pA);
-		float overlap = (rA + rB) - dist;
-		if ( overlap < 0 ) return false;
-		if ( overlap < minOverlap ) {
-			minOverlap = overlap;
-			bestAxis = n;
-		}
-		return true;
-	};
-
-	for ( auto i = 0; i < 3; ++i ) if ( !testAxis(axesA[i]) ) return false;
-	for ( auto i = 0; i < 3; ++i ) if ( !testAxis(axesB[i]) ) return false;
-	for ( auto i = 0; i < 3; ++i ) {
-		for ( auto  j = 0; j < 3; j++ ) if ( !testAxis(uf::vector::cross(axesA[i], axesB[j])) ) return false;
-	};
-
-	if ( uf::vector::dot(bestAxis, cB - cA) < 0.0f ) bestAxis = -bestAxis;
-
-	pod::Vector3f contactPoint = cA + bestAxis * (eA.x * std::fabs(uf::vector::dot(axesA[0], bestAxis)) +
-												  eA.y * std::fabs(uf::vector::dot(axesA[1], bestAxis)) +
-												  eA.z * std::fabs(uf::vector::dot(axesA[2], bestAxis)));
-
-	manifold.points.emplace_back( pod::Contact{ contactPoint, bestAxis, minOverlap } );
-	return true;
 }
 
 bool impl::obbSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold ) {
 	ASSERT_COLLIDER_TYPES( OBB, SPHERE );
 
 	auto tA = impl::getTransform( a );
-	auto localCenter = ( a.collider.obb.max + a.collider.obb.min ) * 0.5f;
+	auto box = a.collider.obb;
-	auto extents = ( a.collider.obb.max - a.collider.obb.min ) * 0.5f;
+	box.center = uf::quaternion::rotate(tA.orientation, box.center) + tA.position;
 
 	auto sphereCenter = impl::getPosition( b );
 	float radius = b.collider.sphere.radius;
 
-	auto localP = uf::transform::applyInverse( tA, sphereCenter ) - localCenter;
+	auto localP = uf::transform::applyInverse( tA, sphereCenter ) - box.center;
-	auto closestLocal = uf::vector::clamp( localP, -extents, extents );
+	auto closestLocal = uf::vector::clamp( localP, -box.extent, box.extent );
 
 	auto deltaLocal = localP - closestLocal;
 	float distSq = uf::vector::dot( deltaLocal, deltaLocal );
 
 	if ( distSq > radius * radius ) return false;
 
-	auto closestWorld = uf::transform::apply( tA, closestLocal + localCenter );
+	auto closestWorld = uf::transform::apply( tA, closestLocal + box.center );
 	float dist = std::sqrt( distSq );
 
 	pod::Vector3f normal;
@@ -160,8 +188,8 @@ bool impl::obbSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod:
 		float sign = 1.0f;
 
 		FOR_EACH(3, {
-			float distToMax = extents[i] - localP[i];
+			float distToMax = box.extent[i] - localP[i];
-			float distToMin = localP[i] - (-extents[i]);
+			float distToMin = localP[i] - (-box.extent[i]);
 			if (distToMax < minDist) { minDist = distToMax; axis = i; sign = 1.0f; }
 			if (distToMin < minDist) { minDist = distToMin; axis = i; sign = -1.0f; }
 		});
@@ -182,28 +210,26 @@ bool impl::obbPlane( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::
 	ASSERT_COLLIDER_TYPES( OBB, PLANE );
 
 	auto tA = impl::getTransform( a );
-	pod::Vector3f cA = uf::transform::apply( tA, (a.collider.obb.max + a.collider.obb.min) * 0.5f );
+	auto box = a.collider.obb;
-	pod::Vector3f eA = (a.collider.obb.max - a.collider.obb.min) * 0.5f;
+	box.center = uf::quaternion::rotate(tA.orientation, box.center) + tA.position;
-	pod::Vector3f axesA[3] = {
+
-		uf::quaternion::rotate(tA.orientation, pod::Vector3f{1,0,0}),
+	pod::Vector3f axesA[3];
-		uf::quaternion::rotate(tA.orientation, pod::Vector3f{0,1,0}),
+	impl::boxAxes( axesA, tA );
-		uf::quaternion::rotate(tA.orientation, pod::Vector3f{0,0,1})
-	};
 
 	pod::Vector3f normal = b.collider.plane.normal;
 	float offset = b.collider.plane.offset;
 
-	float rA = eA.x * std::fabs(uf::vector::dot(axesA[0], normal)) +
+	float rA = box.extent.x * std::fabs(uf::vector::dot(axesA[0], normal)) +
-			   eA.y * std::fabs(uf::vector::dot(axesA[1], normal)) +
+			   box.extent.y * std::fabs(uf::vector::dot(axesA[1], normal)) +
-			   eA.z * std::fabs(uf::vector::dot(axesA[2], normal));
+			   box.extent.z * std::fabs(uf::vector::dot(axesA[2], normal));
 
-	float dist = uf::vector::dot(cA, normal) - offset;
+	float dist = uf::vector::dot(box.center, normal) - offset;
 	if ( dist > rA ) return false; // in front of plane
 
-	pod::Vector3f deepestPoint = cA
+	pod::Vector3f deepestPoint = box.center
-		- axesA[0] * eA.x * (uf::vector::dot(axesA[0], normal) > 0 ? 1.0f : -1.0f)
+		- axesA[0] * box.extent.x * (uf::vector::dot(axesA[0], normal) > 0 ? 1.0f : -1.0f)
-		- axesA[1] * eA.y * (uf::vector::dot(axesA[1], normal) > 0 ? 1.0f : -1.0f)
+		- axesA[1] * box.extent.y * (uf::vector::dot(axesA[1], normal) > 0 ? 1.0f : -1.0f)
-		- axesA[2] * eA.z * (uf::vector::dot(axesA[2], normal) > 0 ? 1.0f : -1.0f);
+		- axesA[2] * box.extent.z * (uf::vector::dot(axesA[2], normal) > 0 ? 1.0f : -1.0f);
 
 	float penetration = rA - dist;
 	manifold.points.emplace_back( pod::Contact{ deepestPoint, normal, penetration } );
@@ -214,13 +240,11 @@ bool impl::obbCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod
 	ASSERT_COLLIDER_TYPES( OBB, CAPSULE );
 
 	auto tA = impl::getTransform( a );
-	pod::Vector3f cA = uf::transform::apply( tA, (a.collider.obb.max + a.collider.obb.min) * 0.5f );
+	auto box = a.collider.obb;
-	pod::Vector3f eA = (a.collider.obb.max - a.collider.obb.min) * 0.5f;
+	box.center = uf::quaternion::rotate(tA.orientation, box.center) + tA.position;
-	pod::Vector3f axesA[3] = {
+
-		uf::quaternion::rotate(tA.orientation, pod::Vector3f{1,0,0}),
+	pod::Vector3f axesA[3];
-		uf::quaternion::rotate(tA.orientation, pod::Vector3f{0,1,0}),
+	impl::boxAxes( axesA, tA );
-		uf::quaternion::rotate(tA.orientation, pod::Vector3f{0,0,1})
-	};
 
 	auto [p1, p2] = impl::getCapsuleSegment( b );
 	pod::Vector3f cB = (p1 + p2) * 0.5f;
@@ -236,10 +260,10 @@ bool impl::obbCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod
 		if (mag < EPS) return true;
 		pod::Vector3f n = axis / mag;
 
-		float pA = uf::vector::dot(cA, n);
+		float pA = uf::vector::dot(box.center, n);
-		float rA = eA.x * std::fabs(uf::vector::dot(axesA[0], n)) +
+		float rA = box.extent.x * std::fabs(uf::vector::dot(axesA[0], n)) +
-				   eA.y * std::fabs(uf::vector::dot(axesA[1], n)) +
+				   box.extent.y * std::fabs(uf::vector::dot(axesA[1], n)) +
-				   eA.z * std::fabs(uf::vector::dot(axesA[2], n));
+				   box.extent.z * std::fabs(uf::vector::dot(axesA[2], n));
 
 		float pB = uf::vector::dot(cB, n);
 		float rB = halfHeight * std::fabs(uf::vector::dot(capAxis, n)) + radius;
@@ -247,9 +271,9 @@ bool impl::obbCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod
 		float dist = std::fabs(pB - pA);
 		float overlap = (rA + rB) - dist;
 
-		if (overlap < 0) return false;
+		if ( overlap < 0 ) return false;
 
-		if (overlap < minOverlap) {
+		if ( overlap < minOverlap ) {
 			minOverlap = overlap;
 			bestAxis = n;
 		}
@@ -260,7 +284,7 @@ bool impl::obbCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod
 	for ( auto i = 0; i < 3; ++i ) if ( !testAxis(axesA[i]) ) return false;
 	for ( auto i = 0; i < 3; ++i ) if ( !testAxis(uf::vector::cross(axesA[i], capAxis)) ) return false;
 
-	if ( uf::vector::dot(bestAxis, cB - cA) < 0.0f ) bestAxis = -bestAxis;
+	if ( uf::vector::dot(bestAxis, cB - box.center) < 0.0f ) bestAxis = -bestAxis;
 
 	pod::Vector3f contactPoint = cB - bestAxis * radius;
 
@@ -277,7 +301,7 @@ bool impl::obbHull( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::M
 }
 
 void impl::drawObb( const pod::PhysicsBody& body ) {
-	auto& aabb = body.collider.aabb;
+	auto aabb = impl::obbToAabb( body.collider.obb );
 	pod::Vector3f corners[8] = {
 		{aabb.min.x, aabb.min.y, aabb.min.z}, {aabb.max.x, aabb.min.y, aabb.min.z},
 		{aabb.max.x, aabb.max.y, aabb.min.z}, {aabb.min.x, aabb.max.y, aabb.min.z},

engine/src/utils/math/physics/narrowphase/ray.cpp

@@ -65,7 +65,7 @@ bool impl::rayAabb( const pod::Ray& ray, const pod::PhysicsBody& body, pod::RayQ
 
 		auto local = rayHit.contact.point - (body.bounds.min + body.bounds.max) * 0.5f;
 		auto extents = (body.bounds.max - body.bounds.min) * 0.5f;
-		auto absLocal = pod::Vector3f{fabs(local.x), fabs(local.y), fabs(local.z)};
+		auto absLocal = uf::vector::abs( local );
 
 		if ( absLocal.x > absLocal.y && absLocal.x > absLocal.z ) {
 			rayHit.contact.normal = { (local.x > 0 ? 1.0f : -1.0f), 0, 0 };
@@ -84,10 +84,10 @@ bool impl::rayObb( const pod::Ray& ray, const pod::PhysicsBody& body, pod::RayQu
 	localRay.origin = uf::transform::applyInverse( tA, ray.origin );
 	localRay.direction = uf::quaternion::rotate( uf::quaternion::inverse(tA.orientation), ray.direction );
 
-	pod::AABB localBox = { body.collider.obb.min, body.collider.obb.max };
+	auto box = impl::obbToAabb( body.collider.obb );
 
 	float tMin, tMax;
-	if ( !impl::rayAabbIntersect( localRay, localBox, tMin, tMax ) ) return false;
+	if ( !impl::rayAabbIntersect( localRay, box, tMin, tMax ) ) return false;
 
 	if ( tMin < rayHit.contact.penetration ) {
 		rayHit.hit = true;
@@ -97,10 +97,8 @@ bool impl::rayObb( const pod::Ray& ray, const pod::PhysicsBody& body, pod::RayQu
 		rayHit.contact.point = ray.origin + ray.direction * tMin;
 
 		auto localPoint = localRay.origin + localRay.direction * tMin;
-		auto center = (localBox.max + localBox.min) * 0.5f;
+		auto localDelta = localPoint - body.collider.obb.center;
-		auto extents = (localBox.max - localBox.min) * 0.5f;
+		auto absDelta = uf::vector::abs( localDelta / body.collider.obb.extent );
-		auto localDelta = localPoint - center;
-		auto absDelta = pod::Vector3f{ std::fabs(localDelta.x) / extents.x, std::fabs(localDelta.y) / extents.y, std::fabs(localDelta.z) / extents.z };
 
 		pod::Vector3f localNormal = {0,0,0};
 		if ( absDelta.x > absDelta.y && absDelta.x > absDelta.z ) localNormal.x = localDelta.x > 0 ? 1.0f : -1.0f;

engine/src/utils/math/physics/narrowphase/triangle.cpp

@@ -19,6 +19,61 @@ namespace impl {
 
 		return triangleGeneric( tri, body, manifold );
 	}
+
+	bool triangleBox( const pod::TriangleWithNormal& tri, const pod::OBB& box, const pod::Vector3f* axes, pod::Manifold& manifold ) {
+		float minOverlap = FLT_MAX;
+		pod::Vector3f bestAxis;
+
+		if ( !impl::testSeparatingAxis( tri, box, tri.normal, axes, minOverlap, bestAxis ) ) return false;
+		if ( !impl::testSeparatingAxis( tri, box, axes[0], axes, minOverlap, bestAxis ) ) return false;
+		if ( !impl::testSeparatingAxis( tri, box, axes[1], axes, minOverlap, bestAxis ) ) return false;
+		if ( !impl::testSeparatingAxis( tri, box, axes[2], axes, minOverlap, bestAxis ) ) return false;
+
+		pod::Vector3f edges[3] = {
+			tri.points[1] - tri.points[0],
+			tri.points[2] - tri.points[1],
+			tri.points[0] - tri.points[2]
+		};
+		for ( int i = 0; i < 3; i++ ) {
+			for ( int j = 0; j < 3; j++ ) {
+				auto axis = uf::vector::cross(edges[i], axes[j]);
+				if ( !impl::testSeparatingAxis( tri, box, axis, axes, minOverlap, bestAxis ) ) return false;
+			}
+		}
+
+		auto cT = impl::triangleCenter(tri);
+		if ( uf::vector::dot( bestAxis, box.center - cT ) < 0.0f ) bestAxis = -bestAxis;
+
+		int polyCount = 3;
+		pod::Vector3f poly[8];
+		poly[0] = tri.points[0];
+		poly[1] = tri.points[1];
+		poly[2] = tri.points[2];
+
+		impl::clipPolygon( poly, polyCount, pod::Plane{  axes[0], box.extent.x + uf::vector::dot(axes[0], box.center) });
+		impl::clipPolygon( poly, polyCount, pod::Plane{ -axes[0], box.extent.x - uf::vector::dot(axes[0], box.center) });
+		impl::clipPolygon( poly, polyCount, pod::Plane{  axes[1], box.extent.y + uf::vector::dot(axes[1], box.center) });
+		impl::clipPolygon( poly, polyCount, pod::Plane{ -axes[1], box.extent.y - uf::vector::dot(axes[1], box.center) });
+		impl::clipPolygon( poly, polyCount, pod::Plane{  axes[2], box.extent.z + uf::vector::dot(axes[2], box.center) });
+		impl::clipPolygon( poly, polyCount, pod::Plane{ -axes[2], box.extent.z - uf::vector::dot(axes[2], box.center) });
+
+		if ( polyCount == 0 ) return false;
+
+		pod::Vector3f boxSupport = box.center;
+
+		boxSupport -= axes[0] * std::copysign(box.extent.x, uf::vector::dot(bestAxis, axes[0]));
+		boxSupport -= axes[1] * std::copysign(box.extent.y, uf::vector::dot(bestAxis, axes[1]));
+		boxSupport -= axes[2] * std::copysign(box.extent.z, uf::vector::dot(bestAxis, axes[2]));
+
+		float referenceOffset = uf::vector::dot(bestAxis, boxSupport);
+
+		for ( auto i = 0; i < polyCount; i++ ) {
+			float penetration = uf::vector::dot(bestAxis, poly[i]) - referenceOffset;
+			manifold.points.emplace_back( pod::Contact{ poly[i], bestAxis, penetration } );
+		}
+
+		return true;
+	}
 }
 
 bool impl::triangleTriangle( const pod::TriangleWithNormal& a, const pod::TriangleWithNormal& b, pod::Manifold& manifold ) {
@@ -96,8 +151,8 @@ bool impl::triangleTriangle( const pod::TriangleWithNormal& a, const pod::Triang
 		auto p1 = refTri.points[(i+1)%3];
 		auto edge = p1 - p0;
 
-		//auto edgeNormal = uf::vector::normalize( uf::vector::cross( refNormal, edge ) );
+		auto edgeNormal = uf::vector::normalize( uf::vector::cross( refNormal, edge ) );
-		auto edgeNormal = uf::vector::normalize( uf::vector::cross( edge, refNormal ) );
+		//auto edgeNormal = uf::vector::normalize( uf::vector::cross( edge, refNormal ) );
 		impl::clipPolygon( poly, polyCount, pod::Plane{ edgeNormal, uf::vector::dot(edgeNormal, p0) } );
 		if ( polyCount == 0 ) return false;
 	}
@@ -110,8 +165,7 @@ bool impl::triangleTriangle( const pod::TriangleWithNormal& a, const pod::Triang
 		float pointProj = uf::vector::dot(bestAxis, poly[i]);
 		float penetration = isAReference ? (pointProj - refOffset) : (refOffset - pointProj);
 	#else
-		float dist = uf::vector::dot(poly[i], refNormal) - uf::vector::dot(refNormal, refTri.points[0]);
+		float penetration = uf::vector::dot(refNormal, refTri.points[0]) - uf::vector::dot(poly[i], refNormal);
-		float penetration = -dist;
 	#endif
 
 		manifold.points.emplace_back(pod::Contact{ poly[i], bestAxis, penetration });
@@ -123,131 +177,23 @@ bool impl::triangleTriangle( const pod::TriangleWithNormal& a, const pod::Triang
 bool impl::triangleAabb( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold ) {
 	if ( uf::physics::settings.useGjk ) return impl::triangleGeneric( tri, body, manifold );
 
-	const auto& box = body.bounds;
+	auto box = impl::aabbToObb( body.bounds );
-	pod::Vector3f axes[3] = { {1,0,0}, {0,1,0}, {0,0,1} };
+	pod::Vector3f axes[3];
+	impl::boxAxes( axes );
 
-	float minOverlap = FLT_MAX;
+	return impl::triangleBox( tri, box, axes, manifold );
-	pod::Vector3f bestAxis;
-	if ( !impl::testSeparatingAxis( tri, box, tri.normal, axes, minOverlap, bestAxis ) ) return false;
-	if ( !impl::testSeparatingAxis( tri, box, axes[0], axes, minOverlap, bestAxis ) ) return false;
-	if ( !impl::testSeparatingAxis( tri, box, axes[1], axes, minOverlap, bestAxis ) ) return false;
-	if ( !impl::testSeparatingAxis( tri, box, axes[2], axes, minOverlap, bestAxis ) ) return false;
-
-	// test edges (3 triangle edges crossed with 3 AABB axes)
-	pod::Vector3f edges[3] = {
-		tri.points[1] - tri.points[0],
-		tri.points[2] - tri.points[1],
-		tri.points[0] - tri.points[2]
-	};
-	for ( int i = 0; i < 3; i++ ) {
-		for ( int j = 0; j < 3; j++ ) {
-			auto axis = uf::vector::cross(edges[i], axes[j]);
-			if ( !impl::testSeparatingAxis( tri, box, axis, axes, minOverlap, bestAxis ) ) return false;
-		}
-	}
-
-	auto cB = impl::aabbCenter(box);
-	auto eB = impl::aabbExtent(box);
-	auto cT = impl::triangleCenter(tri);
-	if ( uf::vector::dot( bestAxis, cB - cT ) < 0.0f ) bestAxis = -bestAxis;
-
-	int polyCount = 3;
-	pod::Vector3f poly[8];
-	poly[0] = tri.points[0];
-	poly[1] = tri.points[1];
-	poly[2] = tri.points[2];
-
-	impl::clipPolygon( poly, polyCount, pod::Plane{ { 1,  0,  0}, eB.x + cB.x} );
-	impl::clipPolygon( poly, polyCount, pod::Plane{ {-1,  0,  0}, eB.x - cB.x} );
-	impl::clipPolygon( poly, polyCount, pod::Plane{ { 0,  1,  0}, eB.y + cB.y} );
-	impl::clipPolygon( poly, polyCount, pod::Plane{ { 0, -1,  0}, eB.y - cB.y} );
-	impl::clipPolygon( poly, polyCount, pod::Plane{ { 0,  0,  1}, eB.z + cB.z} );
-	impl::clipPolygon( poly, polyCount, pod::Plane{ { 0,  0, -1}, eB.z - cB.z} );
-
-	if ( polyCount == 0 ) return false;
-
-	pod::Vector3f boxSupport = cB;
-	boxSupport.x -= std::copysign(eB.x, bestAxis.x);
-	boxSupport.y -= std::copysign(eB.y, bestAxis.y);
-	boxSupport.z -= std::copysign(eB.z, bestAxis.z);
-
-	float referenceOffset = uf::vector::dot(bestAxis, boxSupport);
-
-	for ( auto i = 0; i < polyCount; i++ ) {
-		float pointProjection = uf::vector::dot(bestAxis, poly[i]);
-		float penetration = pointProjection - referenceOffset;
-
-		manifold.points.emplace_back( pod::Contact{ poly[i], bestAxis, penetration } );
-	}
-
-	return true;
 }
 bool impl::triangleObb( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold ) {
 	if ( uf::physics::settings.useGjk ) return impl::triangleGeneric( tri, body, manifold );
 
-	const auto& box = body.bounds;
+	auto transform = impl::getTransform( body );
-	auto tB = impl::getTransform( body );
+	auto box = body.collider.obb;
-	pod::Vector3f axes[3] = {
+	box.center = uf::quaternion::rotate(transform.orientation, box.center) + transform.position;
-		uf::quaternion::rotate(tB.orientation, pod::Vector3f{1,0,0}),
-		uf::quaternion::rotate(tB.orientation, pod::Vector3f{0,1,0}),
-		uf::quaternion::rotate(tB.orientation, pod::Vector3f{0,0,1})
-	};
 
-	float minOverlap = FLT_MAX;
+	pod::Vector3f axes[3];
-	pod::Vector3f bestAxis;
+	impl::boxAxes( axes, transform );
 
-	if ( !impl::testSeparatingAxis( tri, box, tri.normal, axes, minOverlap, bestAxis ) ) return false;
+	return impl::triangleBox( tri, box, axes, manifold );
-	if ( !impl::testSeparatingAxis( tri, box, axes[0], axes, minOverlap, bestAxis ) ) return false;
-	if ( !impl::testSeparatingAxis( tri, box, axes[1], axes, minOverlap, bestAxis ) ) return false;
-	if ( !impl::testSeparatingAxis( tri, box, axes[2], axes, minOverlap, bestAxis ) ) return false;
-
-	pod::Vector3f edges[3] = {
-		tri.points[1] - tri.points[0],
-		tri.points[2] - tri.points[1],
-		tri.points[0] - tri.points[2]
-	};
-	for ( auto i = 0; i < 3; i++ ) {
-		for ( auto j = 0; j < 3; j++ ) {
-			pod::Vector3f axis = uf::vector::cross( edges[i], axes[j] );
-			if ( !impl::testSeparatingAxis( tri, box, axis, axes, minOverlap, bestAxis ) ) return false;
-		}
-	}
-
-	auto cB = impl::aabbCenter(box);
-	auto eB = impl::aabbExtent(box);
-	auto cT = impl::triangleCenter(tri);
-	if ( uf::vector::dot( bestAxis, cB - cT ) < 0.0f ) bestAxis = -bestAxis;
-
-	int polyCount = 3;
-	pod::Vector3f poly[8];
-	poly[0] = tri.points[0];
-	poly[1] = tri.points[1];
-	poly[2] = tri.points[2];
-
-	impl::clipPolygon( poly, polyCount, pod::Plane{  axes[0], eB.x + uf::vector::dot(axes[0], cB) });
-	impl::clipPolygon( poly, polyCount, pod::Plane{ -axes[0], eB.x - uf::vector::dot(axes[0], cB) });
-	impl::clipPolygon( poly, polyCount, pod::Plane{  axes[1], eB.y + uf::vector::dot(axes[1], cB) });
-	impl::clipPolygon( poly, polyCount, pod::Plane{ -axes[1], eB.y - uf::vector::dot(axes[1], cB) });
-	impl::clipPolygon( poly, polyCount, pod::Plane{  axes[2], eB.z + uf::vector::dot(axes[2], cB) });
-	impl::clipPolygon( poly, polyCount, pod::Plane{ -axes[2], eB.z - uf::vector::dot(axes[2], cB) });
-
-	if ( polyCount == 0 ) return false;
-
-	pod::Vector3f boxSupport = cB;
-	boxSupport.x -= std::copysign(eB.x, bestAxis.x);
-	boxSupport.y -= std::copysign(eB.y, bestAxis.y);
-	boxSupport.z -= std::copysign(eB.z, bestAxis.z);
-
-	float referenceOffset = uf::vector::dot(bestAxis, boxSupport);
-
-	for ( auto i = 0; i < polyCount; i++ ) {
-		float pointProjection = uf::vector::dot(bestAxis, poly[i]);
-		float penetration = pointProjection - referenceOffset;
-
-		manifold.points.emplace_back( pod::Contact{ poly[i], bestAxis, penetration } );
-	}
-
-	return true;
 }
 bool impl::triangleSphere( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold ) {
 	if ( uf::physics::settings.useGjk ) return impl::triangleGeneric( tri, body, manifold );

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

@@ -13,21 +13,71 @@ void impl::solveContacts( uf::stl::vector<pod::Manifold>& manifolds, float dt )
 	if ( uf::physics::settings.warmupSolver ) for ( auto& manifold : manifolds ) impl::warmupManifold( *manifold.a, *manifold.b, manifold, dt );
 	for ( auto i = 0; i < uf::physics::settings.solverIterations; ++i ) for ( auto& manifold : manifolds ) impl::resolveManifold( *manifold.a, *manifold.b, manifold, dt );
 }
-// unused
 void impl::solvePositions( uf::stl::vector<pod::Manifold>& manifolds, float dt, uint32_t iterations ) {
+	if ( true || uf::physics::settings.baumgarteCorrectionPercent <= 0 ) return;
 	for ( auto i = 0; i < iterations; ++i ) {
-		for ( auto& m : manifolds ) {
+		float minSeparation = 0.0f;
-			pod::Contact s = {};
+
-			float weight = 0;
+		for ( auto& manifold : manifolds ) {
-			for ( auto& c : m.points ) {
+			auto& a = *manifold.a;
-				float w = std::max( c.penetration, 0.0f );
+			auto& b = *manifold.b;
-				s.normal += c.normal * w;
+			auto tA = impl::getTransform( a );
-				s.penetration = std::max(s.penetration, c.penetration);
+			auto tB = impl::getTransform( b );
-				weight += w;
+
+			if ( a.isStatic && b.isStatic ) continue;
+
+			for ( auto& c : manifold.points ) {
+				pod::Vector3f rA = uf::quaternion::rotate( tA.orientation, c.localA );
+				pod::Vector3f rB = uf::quaternion::rotate( tB.orientation, c.localB );
+				pod::Vector3f worldA = tA.position + rA;
+				pod::Vector3f worldB = tB.position + rB;
+
+				float penetration = -uf::vector::dot( worldB - worldA, c.normal );
+				minSeparation = std::min( minSeparation, -penetration );
+
+				float C = std::clamp( penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f, uf::physics::settings.maxLinearCorrection );
+				if ( C <= 0.0f ) continue;
+
+				float invMassN = impl::computeEffectiveMass( a, b, rA, rB, c.normal );
+
+				float lambda = (C / invMassN) * uf::physics::settings.baumgarteCorrectionPercent;
+				pod::Vector3f P = c.normal * lambda;
+
+				// apply impulses directly
+				if ( !a.isStatic ) {
+					pod::Vector3f translation = P * a.inverseMass;
+					a.transform->position -= translation;
+					tA.position -= translation;
+
+					pod::Matrix3f invIa = impl::computeWorldInverseInertia(a);
+					pod::Vector3f deltaAngleA = uf::matrix::multiply(invIa, uf::vector::cross(rA, -P));
+
+					float angleA2 = uf::vector::magnitude( deltaAngleA );
+					if ( angleA2 > EPS2 ) {
+						float angleA = std::sqrt( angleA2);
+						pod::Quaternion<> dq = uf::quaternion::axisAngle(deltaAngleA / angleA, angleA);
+						uf::transform::rotate( *a.transform, dq );
+						tA.orientation = uf::quaternion::multiply(dq, tA.orientation);
+					}	
+				}
+
+				if ( !b.isStatic ) {
+					pod::Vector3f translation = P * b.inverseMass;
+					b.transform->position += translation;
+					tB.position += translation;
+
+					pod::Matrix3f invIb = impl::computeWorldInverseInertia(b);
+					pod::Vector3f deltaAngleB = uf::matrix::multiply(invIb, uf::vector::cross(rB, P));
+
+					float angleB2 = uf::vector::magnitude( deltaAngleB );
+					if ( angleB2 > EPS2 ) {
+						float angleB = std::sqrt( angleB2);
+						pod::Quaternion<> dq = uf::quaternion::axisAngle(deltaAngleB / angleB, angleB);
+						uf::transform::rotate( *b.transform, dq );
+						tB.orientation = uf::quaternion::multiply(dq, tB.orientation);
+					}
+				}
 			}
-			s.normal = weight > 0.0f ? uf::vector::normalize(s.normal) : pod::Vector3f{0,1,0};
-			
-			impl::positionCorrection( *m.a, *m.b, s );
 		}
 	}
 }

engine/src/utils/math/physics/solvers/block.cpp

@@ -45,11 +45,8 @@ namespace impl {
 			K(i,i) += 1e-3f;
 		}
 
-		pod::Vector<T,N> rhsVel = {};
+		pod::Vector<T,N> rhs = {};
-		pod::Vector<T,N> K_lambdaVel = {};
+		pod::Vector<T,N> K_lambda = {};
-
-		pod::Vector<T,N> rhsPos = {};
-		pod::Vector<T,N> K_lambdaPos = {};
 
 		for ( auto i = 0; i < N; i++ ) {
 			auto& contact = manifold.points[i];
@@ -61,32 +58,19 @@ namespace impl {
 			float e = std::min(a.material.restitution, b.material.restitution);
 			float restitutionBias = (vRel < -1.0f) ? -e * vRel : 0.0f;
 
-			rhsVel[i] = -vRel + restitutionBias;
+			rhs[i] = -vRel + restitutionBias;
-
+			K_lambda[i] = contact.accumulatedNormalImpulse;
-			pod::Vector3f pseudoVa = a.pseudoVelocity + uf::vector::cross( a.pseudoAngularVelocity, contact.point - pA );
-			pod::Vector3f pseudoVb = b.pseudoVelocity + uf::vector::cross( b.pseudoAngularVelocity, contact.point - pB );
-			float pseudoVRel = uf::vector::dot((pseudoVb - pseudoVa), contact.normal);
-
-			float penetrationBias = std::max(contact.penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f) * (uf::physics::settings.baumgarteCorrectionPercent / dt);
-
-			rhsPos[i] = -pseudoVRel + penetrationBias;
-
-			K_lambdaVel[i] = contact.accumulatedNormalImpulse;
-			K_lambdaPos[i] = contact.accumulatedPseudoImpulse;
 		}
 
 		pod::Matrix<T,N> Kinv = uf::matrix::inverse( K );
 
-		pod::Vector<T,N> residualVel = rhsVel - uf::matrix::multiply( K, K_lambdaVel );
+		pod::Vector<T,N> residual = rhs - uf::matrix::multiply( K, K_lambda );
-		pod::Vector<T,N> dLambdaVel = uf::matrix::multiply( Kinv, residualVel );
+		pod::Vector<T,N> dLambda = uf::matrix::multiply( Kinv, residual );
-
-		pod::Vector<T,N> residualPos = rhsPos - uf::matrix::multiply( K, K_lambdaPos );
-		pod::Vector<T,N> dLambdaPos = uf::matrix::multiply( Kinv, residualPos );
 
 		// check if contacts are all valid
 		int invalidContactIndex = -1;
 		for ( auto i = 0; i < N; i++ ) {
-			if ( K_lambdaVel[i] + dLambdaVel[i] < 0.0f || K_lambdaPos[i] + dLambdaPos[i] < 0.0f ) {
+			if ( K_lambda[i] + dLambda[i] < 0.0f ) {
 				invalidContactIndex = i;
 				break;
 			}
@@ -96,10 +80,17 @@ namespace impl {
 			bool success = false;
 			// reduce the manifold
 			if ( uf::physics::settings.resolveBlockContact && N > 1 ) {
+			#if 1
+				pod::Manifold reducedManifold = manifold;
+				reducedManifold.points.erase( reducedManifold.points.begin() + invalidContactIndex );
+				// re-solve
+				success = impl::blockSolver( a, b, reducedManifold, dt );
+				// copy back to original manifold
+				if ( success ) manifold = reducedManifold;
+			#else
 				pod::Manifold reducedManifold = manifold;
 				reducedManifold.points.erase( reducedManifold.points.begin() + invalidContactIndex );
 				manifold.points[invalidContactIndex].accumulatedNormalImpulse = 0.0f;
-				manifold.points[invalidContactIndex].accumulatedPseudoImpulse = 0.0f;
 				manifold.points[invalidContactIndex].accumulatedTangentImpulse = 0.0f;
 
 				// re-solve
@@ -110,6 +101,7 @@ namespace impl {
 						manifold.points[i] = reducedManifold.points[r++];
 					}
 				}
+			#endif
 			}
 			return success;
 		}
@@ -119,19 +111,31 @@ namespace impl {
 			pod::Vector3f rA = manifold.points[i].point - pA;
 			pod::Vector3f rB = manifold.points[i].point - pB;
 
-			// real impulse
+			// normal impulse
 			{
-				float newLambdaVel = contact.accumulatedNormalImpulse + dLambdaVel[i];
+				float newLambda = contact.accumulatedNormalImpulse + dLambda[i];
-				dLambdaVel[i] = newLambdaVel - contact.accumulatedNormalImpulse;
+				dLambda[i] = newLambda - contact.accumulatedNormalImpulse;
-				contact.accumulatedNormalImpulse = newLambdaVel;
+				contact.accumulatedNormalImpulse = newLambda;
-				impl::applyImpulseTo( a, b, rA, rB, manifold.points[i].normal * dLambdaVel[i] );
+				impl::applyImpulseTo( a, b, rA, rB, manifold.points[i].normal * dLambda[i] );
 			}
 			// pseudo impulse
 			{
-				float newLambdaPos = contact.accumulatedPseudoImpulse + dLambdaPos[i];
+				float penetrationBias = std::max(contact.penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f) * (uf::physics::settings.baumgarteCorrectionPercent / dt);
-				dLambdaPos[i] = newLambdaPos - contact.accumulatedPseudoImpulse;
+				penetrationBias = std::min(penetrationBias, uf::physics::settings.maxLinearCorrection / dt);
-				contact.accumulatedPseudoImpulse = newLambdaPos;
+
-				impl::applyPseudoImpulseTo( a, b, rA, rB, manifold.points[i].normal * dLambdaPos[i] );
+				pod::Vector3f pseudoVa = a.pseudoVelocity + uf::vector::cross(a.pseudoAngularVelocity, rA);
+				pod::Vector3f pseudoVb = b.pseudoVelocity + uf::vector::cross(b.pseudoAngularVelocity, rB);
+				float pseudoVelAlongNormal = uf::vector::dot(pseudoVb - pseudoVa, contact.normal);
+
+				float invMassN = impl::computeEffectiveMass(a, b, rA, rB, contact.normal);
+				float jPseudo = (penetrationBias - pseudoVelAlongNormal) / invMassN;
+
+				float jPseudoOld = contact.accumulatedPseudoImpulse;
+				float jPseudoNew = std::max(0.0f, jPseudoOld + jPseudo);
+				contact.accumulatedPseudoImpulse = jPseudoNew;
+				jPseudo = jPseudoNew - jPseudoOld;
+
+				impl::applyPseudoImpulseTo(a, b, rA, rB, contact.normal * jPseudo);
 			}
 			// tangent friction
 			{

engine/src/utils/math/physics/solvers/iterativeImpulse.cpp

@@ -8,7 +8,7 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod
 
 	float invMassN = impl::computeEffectiveMass(a, b, rA, rB, contact.normal);
 
-	// real impulse
+	// normal impulse
 	{
 		pod::Vector3f vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);
 		pod::Vector3f vB = b.velocity + uf::vector::cross(b.angularVelocity, rB);
@@ -33,7 +33,7 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod
 	// pseudo impulse
 	{
 		float penetrationBias = std::max(contact.penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f) * (uf::physics::settings.baumgarteCorrectionPercent / dt);
-		penetrationBias = std::min(penetrationBias, 2.0f / dt);
+		penetrationBias = std::min(penetrationBias, uf::physics::settings.maxLinearCorrection / dt);
 
 		pod::Vector3f pseudoVa = a.pseudoVelocity + uf::vector::cross(a.pseudoAngularVelocity, rA);
 		pod::Vector3f pseudoVb = b.pseudoVelocity + uf::vector::cross(b.pseudoAngularVelocity, rB);
@@ -48,7 +48,6 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod
 
 		impl::applyPseudoImpulseTo(a, b, rA, rB, contact.normal * jPseudo);
 	}
-
 	// tangent friction
 	{
 		pod::Vector3f vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);