even more inane cleanup, and even more constraints

bin/data/config.json

@@ -350,7 +350,7 @@
 				"max": 0.01 // 0.2
 			},
 			"debug draw": {
-				"dynamic": true
+				"dynamic": false
 			},
 			"fixed step": true,
 			"substeps": 4

bin/data/entities/ball.json

@@ -14,7 +14,7 @@
 	"metadata": {
 		"holdable": true,
 		"physics": {
-			"mass": 10,
+			"mass": 1,
 			"type": "sphere",
 			"radius": 1,
 			"min": [ -0.5, -0.5, -0.5 ],

bin/data/entities/physics_prop.json

@@ -6,7 +6,7 @@
 	"metadata": {
 		"holdable": true,
 		"physics": {
-			"mass": 100,
+			"mass": 1900,
 		//	"inertia": false,
 			"type": "obb"
 		//	"type": "mesh"

bin/data/entities/prop.json

@@ -7,7 +7,7 @@
 		"holdable": true,
 		"physics": {
 			"mass": 0,
-			"inertia": false,
+		//	"inertia": false,
 		//	"type": "bounding box"
 			"type": "mesh"
 		//	"type": "hull"

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

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "impl.h"
+#include "structs.h"
 
 #include "broadphase/bvh.h"
 #include "broadphase/island.h"

engine/inc/uf/utils/math/physics/broadphase/bvh.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../impl.h"
+#include "../structs.h"
 
 namespace impl {	
 	pod::BVH::index_t buildBVHNode( pod::BVH& bvh, const uf::stl::vector<pod::AABB>& bounds, pod::BVH::index_t start, pod::BVH::index_t end, pod::BVH::index_t capacity = 2 );

engine/inc/uf/utils/math/physics/broadphase/island.h

@@ -1,7 +1,7 @@
 #pragma once
 
-#include "../impl.h"
+#include "../structs.h"
 
 namespace impl {
-	void buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector<pod::PhysicsBody*>& bodies, uf::stl::vector<pod::Island>& islands );
+	void buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector<pod::PhysicsBody*>& bodies, const uf::stl::vector<pod::Constraint*>& constraints, uf::stl::vector<pod::Island>& islands );
 }

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

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "impl.h"
+#include "structs.h"
 #include "draw.h"
 
 // to-do: organize this mess

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

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "structs.h"
+
+#include "constraints/contact.h"
+#include "constraints/ballSocket.h"
+#include "constraints/hinge.h"
+#include "constraints/coneTwist.h"
+#include "constraints/slider.h"
+#include "constraints/distance.h"
+#include "constraints/weld.h"
+#include "constraints/spring.h"
+#include "constraints/motor.h"
+
+namespace impl {
+	void solveConstraint( pod::Constraint& constraint, float dt );
+}

engine/inc/uf/utils/math/physics/constraints/ballSocket.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "../structs.h"
+
+namespace impl {
+	void solveBallSocketConstraint( pod::Constraint& constraint, float dt );
+}
+
+namespace uf {
+	namespace physics {
+		pod::Constraint& UF_API constrain( uf::Object&, uf::Object&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::World&, uf::Object&, uf::Object&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::World&, pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f& );
+	}
+}

engine/inc/uf/utils/math/physics/constraints/coneTwist.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "../structs.h"
+
+namespace impl {
+	void solveConeTwistConstraint( pod::Constraint& constraint, float dt );
+}
+
+namespace uf {
+	namespace physics {
+		pod::Constraint& UF_API constrain( uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f&, float, float );
+		pod::Constraint& UF_API constrain( pod::World&, uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f&, float, float );
+		pod::Constraint& UF_API constrain( pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f&, float, float );
+		pod::Constraint& UF_API constrain( pod::World&, pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f&, float, float );
+	}
+}

engine/inc/uf/utils/math/physics/constraints/contact.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include "../structs.h"
+
+namespace impl {
+	void bindManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt = 0 );
+	bool generateContactsGjk( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
+	bool generateContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
+	void computeLocalContacts( pod::Manifold& manifold );
+	bool similarContact( const pod::Contact& a, const pod::Contact& b, float distSqThreshold = 1.0e-2f, float normThreshold = 0.9f );
+	void reduceContacts( pod::Manifold& manifold );
+	void mergeContacts( pod::Manifold& manifold );
+	void retrieveContacts( pod::Manifold& current, const pod::Manifold& previous, float distanceThreshold = 0.1f, float separationThreshold = 0.1f, float decay = 0.85f );
+	void prepareManifoldCache( uf::stl::unordered_map<size_t, pod::Manifold>& cache, const uf::stl::vector<pod::Island>& islands, const uf::stl::vector<pod::PhysicsBody*>& bodies );
+	void updateManifoldCache( const uf::stl::vector<pod::Manifold>& manifolds, uf::stl::unordered_map<size_t, pod::Manifold>& cache );
+	void pruneManifoldCache( uf::stl::unordered_map<size_t, pod::Manifold>& cache );
+	void warmupContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& c, float dt );
+	void warmupManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Manifold& manifold, float dt );
+	
+	void resolveManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
+	void solveContacts( uf::stl::vector<pod::Manifold>& manifolds, float dt );
+}

engine/inc/uf/utils/math/physics/constraints/distance.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "../structs.h"
+
+namespace impl {
+	void solveDistanceConstraint( pod::Constraint& constraint, float dt );
+}
+/*
+namespace uf {
+	namespace physics {
+		pod::Constraint& UF_API constrain( uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::World&, uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::World&, pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
+	}
+}
+*/

engine/inc/uf/utils/math/physics/constraints/hinge.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "../structs.h"
+
+namespace impl {
+	void solveHingeConstraint( pod::Constraint& constraint, float dt );
+}
+
+namespace uf {
+	namespace physics {
+		pod::Constraint& UF_API constrain( uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::World&, uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::World&, pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
+	}
+}

engine/inc/uf/utils/math/physics/constraints/motor.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "../structs.h"
+
+namespace impl {
+    void solve1DLinearMotor(
+	   pod::PhysicsBody& a, pod::PhysicsBody& b,
+        const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& axis,
+        float targetVelocity, float maxForce, float& accumulatedImpulse, float dt
+    );
+    void solve1DAngularMotor(
+        pod::PhysicsBody& a, pod::PhysicsBody& b,
+        const pod::Vector3f& axis,
+        float targetVelocity, float maxTorque,
+        float& accumulatedImpulse, float dt
+    );
+}

engine/inc/uf/utils/math/physics/constraints/slider.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "../structs.h"
+
+namespace impl {
+	void solveSliderConstraint( pod::Constraint& constraint, float dt );
+}
+/*
+namespace uf {
+	namespace physics {
+		pod::Constraint& UF_API constrain( uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::World&, uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::World&, pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
+	}
+}
+*/

engine/inc/uf/utils/math/physics/constraints/spring.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "../structs.h"
+
+namespace impl {
+	void solveSpringConstraint( pod::Constraint& constraint, float dt );
+}
+/*
+namespace uf {
+	namespace physics {
+		pod::Constraint& UF_API constrain( uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::World&, uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::World&, pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
+	}
+}
+*/

engine/inc/uf/utils/math/physics/constraints/weld.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "../structs.h"
+
+namespace impl {
+	void solveWeldConstraint( pod::Constraint& constraint, float dt );
+}
+/*
+namespace uf {
+	namespace physics {
+		pod::Constraint& UF_API constrain( uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::World&, uf::Object&, uf::Object&, const pod::Vector3f&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
+		pod::Constraint& UF_API constrain( pod::World&, pod::PhysicsBody&, pod::PhysicsBody&, const pod::Vector3f&, const pod::Vector3f& );
+	}
+}
+*/

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

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "impl.h"
+#include "structs.h"
 
 namespace impl {
 	struct Vertex {

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

@@ -2,331 +2,14 @@
 
 #include <uf/config.h>
 #include <uf/utils/time/time.h>
-#include <uf/utils/math/vector.h>
-#include <uf/utils/math/quaternion.h>
-#include <uf/utils/math/matrix.h>
-#include <uf/utils/math/shapes.h>
-#include <uf/utils/math/transform.h>
-
-#include <uf/utils/memory/vector.h>
-#include <uf/utils/memory/unordered_map.h>
-#include <uf/utils/memory/unordered_set.h>
-#include <uf/utils/memory/stack.h>
-
-#include <uf/engine/object/object.h>
-#include <uf/utils/mesh/mesh.h>
 #include <uf/utils/math/quant.h>
 
-#include <cfloat>
-
-#define EPS 1.0e-6f
-#define EPS2 (EPS * EPS)
-#define ASSERT_COLLIDER_TYPES( A, B ) UF_ASSERT( a.collider.type == pod::ShapeType::A && b.collider.type == pod::ShapeType::B );
-
-#define REORIENT_NORMALS_ON_FETCH 0
-#define REORIENT_NORMALS_ON_CONTACT 1
-
-#define REVERSE_COLLIDER( a, b, fun )\
-	auto start = manifold.points.size();\
-	if ( !::fun( b, a, manifold ) ) return false;\
-	for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal;\
-	return true;
-
-namespace pod {
-	enum class ShapeType {
-		AABB,
-		OBB,
-		SPHERE,
-		PLANE,
-		CAPSULE,
-		TRIANGLE,
-		MESH,
-		CONVEX_HULL,
-	};
-
-	struct SupportPoint {
-		/*alignas(16)*/ pod::Vector3f p;
-		/*alignas(16)*/ pod::Vector3f pA;
-		/*alignas(16)*/ pod::Vector3f pB;
-	};
-
-	struct Simplex {
-		uf::stl::vector<pod::SupportPoint> pts;
-	};
-
-	struct Face {
-		pod::SupportPoint a, b, c;
-		/*alignas(16)*/ pod::Vector3f normal;
-		float distance;
-	};
-
-	struct BVH {
-		typedef uint32_t index_t;
-		typedef std::pair<index_t,index_t> pair_t;
-		
-		struct PairHash {
-			size_t operator()( const pair_t& p ) const noexcept {
-				uint64_t a = (uint64_t) std::min(p.first, p.second);
-				uint64_t b = (uint64_t) std::max(p.first, p.second);
-				return (a << 32) ^ b;
-			}
-		};
-		struct PairEq {
-			bool operator()( const pair_t& a, const pair_t& b ) const noexcept {
-				return (a.first == b.first && a.second == b.second) || (a.first == b.second && a.second == b.first);
-			}
-		};
-
-	//	typedef uf::stl::unordered_set<pair_t, PairHash, PairEq> pairs_t;
-		typedef uf::stl::vector<pair_t> pairs_t;
-		
-		struct Node {
-			BVH::index_t left = 0;
-			BVH::index_t right = 0;
-			BVH::index_t start = 0;
-			BVH::index_t flags = 0;
-
-			BVH::index_t getCount() const { return flags & 0x7FFFFFFF; }
-			bool isAsleep() const { return (flags & 0x80000000u) != 0; }
-			void setCount(BVH::index_t c) { flags = (flags & 0x80000000u) | (c & 0x7FFFFFFF); }
-			void setAsleep(bool a) { flags = (flags & 0x7FFFFFFF) | (a ? 0x80000000u : 0); }
-		};
-		struct FlatNode {
-			BVH::index_t start = 0;
-			BVH::index_t skipIndex = 0;
-			BVH::index_t flags = 0;
-
-			BVH::index_t getCount() const { return flags & 0x7FFFFFFF; }
-			bool isAsleep() const { return (flags & 0x80000000u) != 0; }
-			void setCount(BVH::index_t c) { flags = (flags & 0x80000000u) | (c & 0x7FFFFFFF); }
-			void setAsleep(bool a) { flags = (flags & 0x7FFFFFFF) | (a ? 0x80000000u : 0); }
-		};
-		struct UpdatePolicy {
-			enum class Decision {
-				NONE,	// do nothing
-				REFIT,   // refit bounds
-				REBUILD  // rebuild from scratch
-			};
-			float displacementThreshold = 0.25f; // 25% of AABB size
-			float overlapThreshold = 2.0f;	   // 2x growth in root surface area
-			float dirtyRatioThreshold = 0.3f;	// 30% dirty bodies
-			uint16_t maxFramesBeforeRebuild = 600;   // force rebuild every 600 frames
-		};
-
-		bool dirty = false;
-		uf::stl::vector<pod::BVH::index_t> indices;
-		uf::stl::vector<pod::BVH::Node> nodes;
-		uf::stl::vector<pod::BVH::FlatNode> flattened;
-		
-		uf::stl::vector<pod::AABB> bounds;
-		uf::stl::vector<pod::AABB> flatBounds;
-	};
-
-	struct MeshBVH {
-		pod::BVH* bvh;
-		const uf::Mesh* mesh;
-	};
-	typedef MeshBVH ConvexHullBVH;
-
-	typedef uint32_t CollisionMask;
-	struct Collider {
-		// what it is
-		enum CategoryMask : uint32_t {
-			CATEGORY_NONE			= 0,
-			CATEGORY_STATIC			= 1 << 0,
-			CATEGORY_DYNAMIC		= 1 << 1,
-			CATEGORY_PLAYER	  		= 1 << 2,
-			CATEGORY_NPC			= 1 << 3,
-			CATEGORY_TRIGGER		= 1 << 4,
-			CATEGORY_PROJECTILE		= 1 << 5,
-			CATEGORY_CHARACTER		= CATEGORY_PLAYER | CATEGORY_NPC,
-			CATEGORY_ALL			= 0xFFFFFFFF
-		};
-		// what it collides with
-		enum CollisionMask : uint32_t {
-			MASK_NONE				= 0,
-			MASK_STATIC				= CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_NPC | CATEGORY_PROJECTILE,
-			MASK_DYNAMIC			= CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_NPC,
-			MASK_PLAYER				= CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_NPC | CATEGORY_PROJECTILE,
-			MASK_NPC				= CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_PROJECTILE,
-			MASK_TRIGGER			= CATEGORY_PLAYER | CATEGORY_NPC,
-			MASK_PROJECTILE			= CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_NPC,
-			MASK_CHARACTER			= MASK_PLAYER | MASK_NPC,
-			MASK_ALL				= 0xFFFFFFFF
-		};
-
-		pod::ShapeType type;
-		pod::CollisionMask category = Collider::CATEGORY_ALL;
-		pod::CollisionMask mask = Collider::MASK_ALL;
-
-		union {
-			pod::AABB aabb;
-			pod::OBB obb;
-			pod::Sphere sphere;
-			pod::Plane plane;
-			pod::Capsule capsule;
-			pod::TriangleWithNormal triangle;
-			pod::MeshBVH mesh;
-			pod::ConvexHullBVH convexHull;
-		};
-	};
-
-	struct PhysicsMaterial {
-		float restitution = 0.2f;
-		float staticFriction = 0.5f;
-		float dynamicFriction = 0.3f;
-	};
-
-	struct Activity {
-		bool awake = true;
-		bool grounded = false;
-		float sleepTimer = 0.0f;
-		int32_t islandID = -1;
-		static constexpr float sleepThreshold = 0.5f; // seconds
-		static constexpr float linearSleepEpsilon = 0.2f; // m/s
-		static constexpr float angularSleepEpsilon = 0.2f; // rad/s		
-	};
-
-	struct World; // forward declare
-
-	struct PhysicsBody {
-		pod::World* world = NULL;
-		uf::Object* object = NULL;
-		pod::Transform<>* transform = NULL;
-
-		bool isStatic = false;
-
-		float mass = 1.0f;
-		float inverseMass = 1.0f; // for fast division
-		int32_t viewIndex = -1; // -1 means it's not an aliased view
-
-		pod::Vector3f offset = {};
-
-		pod::Vector3f velocity = {};
-		pod::Vector3f forceAccumulator = {};
-
-		pod::Vector3f angularVelocity = {};
-		pod::Vector3f torqueAccumulator = {};
-
-		pod::Vector3f pseudoVelocity = {};
-		pod::Vector3f pseudoAngularVelocity = {};
-
-		pod::Vector3f inverseInertiaTensor = { 1, 1, 1 };
-
-		pod::Vector3f gravity = { NAN, NAN, NAN }; // an invalid gravity will fallback to world gravity
-
-		pod::AABB bounds;
-		pod::Collider collider;
-		pod::PhysicsMaterial material;
-		pod::Activity activity;
-	};
-
-	struct Contact {
-		pod::Vector3f point;
-		pod::Vector3f normal;
-		float penetration;
-
-		pod::Vector3f localA;
-		pod::Vector3f localB;
-
-		// warm-start cached values
-		int lifetime = 0;
-		pod::Vector3f tangent;
-		float accumulatedNormalImpulse = 0.0f;
-		float accumulatedTangentImpulse = 0.0f;
-		float accumulatedPseudoImpulse = 0.0f;
-	};
-
-	struct Manifold {
-		pod::PhysicsBody* a = NULL;
-		pod::PhysicsBody* b = NULL;
-		float dt = 0;
-		uf::stl::vector<pod::Contact> points;
-	};
-
-	struct RayQuery {
-		bool hit = false;
-		const pod::PhysicsBody* body = NULL;
-		const pod::PhysicsBody* invoker = NULL;
-		pod::Contact contact = { pod::Vector3f{}, pod::Vector3f{}, FLT_MAX };
-	};
-
-	struct Island {
-		bool awake = true;
-		uf::stl::vector<uint32_t> indices;
-		pod::BVH::pairs_t pairs;
-
-		uf::stl::vector<pod::Manifold> manifolds;
-	};
-
-	struct PhysicsSettings {
-		bool warmupSolver = true; // cache manifold data to warm up the solver
-		bool blockContactSolver = true; // use BlockNxN solvers (where N = number of contacts for a manifold)
-		bool resolveBlockContact = true; // attempts to resolve an invalid BlockNxN solve with an BlockN-1xN-1 solve
-		bool useGjk = false; // currently don't have a way to broadphase mesh => narrowphase tri via GJK
-		
-		pod::CollisionMask debugDraw = pod::Collider::CATEGORY_NONE; // draws wireframe of collision bodies
-		bool async = false; // dedicated thread for physics sim
-		float timestep = 1.0f / 60.0f; // timestep for fixed step ticks
-		bool fixedStep = true; // run physics simulation with a fixed delta time (with accumulation), rather than rely on actual engine deltatime
-		uint32_t substeps = 4; // number of substeps per frame tick
-		uint32_t reserveCount = 32; // amount of elements to reserve for vectors used in this system, to-do: have it tie to a memory pool allocator
-
-		uint32_t broadphaseBvhCapacity = 1; // number of bodies per leaf node
-		uint32_t meshBvhCapacity = 1; // number of triangles per leaf node
-
-		// additionally flattens a BVH for linear iteration, rather than a recursive / stack-based traversal
-		bool flattenBvhBodies = true;
-		bool flattenBvhMeshes = true;
-		
-		// use surface area heuristics for building the BVH, rather than naive splits
-		bool useBvhSahBodies = true;
-		bool useBvhSahMeshes = true;
-
-		bool useSplitBvhs = true; // creates separate BVHs for static / dynamic objects
-
-		// to-do: find possibly better values for this
-		uint32_t solverIterations = 10;
-		bool ngsPositionSolver = false;
-		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
-
-		uint32_t frameCounter = 0;
-
-		// to-do: tweak this to not be annoying
-		pod::BVH::UpdatePolicy bvhUpdatePolicy = {
-			// triggers a refit
-			.displacementThreshold = 0.25f,
-			// triggers a rebuild
-			.overlapThreshold = 2.0f,
-			.dirtyRatioThreshold = 0.3,
-			.maxFramesBeforeRebuild = 60,
-		};
-
-		float groundedThreshold = 0.7f; // threshold before marking a body as grounded
-	};
-
-	struct World {
-		uf::stl::vector<pod::PhysicsBody*> bodies;
-	
-		pod::Vector3f gravity = { 0, -9.81f, 0 };
-		pod::BVH dynamicBvh;
-		pod::BVH staticBvh;
-	};
-}
+#include "structs.h"
+#include "narrowphase.h"
+#include "constraints.h"
 
 namespace uf {
 	namespace physics {
-		typedef pod::Math::num_t num_t;
-		namespace time = uf::time; // to-do: have separate values from the physics system
-		
-		extern UF_API pod::World world;
-		extern UF_API pod::PhysicsSettings settings;
-
 		void UF_API initialize();
 		void UF_API initialize( uf::Object& );
 		void UF_API initialize( pod::World& );
@@ -370,29 +53,20 @@ namespace uf {
 
 		void UF_API applyRotation( pod::PhysicsBody& body, const pod::Quaternion<>& q );
 		void UF_API applyRotation( pod::PhysicsBody& body, const pod::Vector3f& axis, float angle );
+
+		pod::World& UF_API getWorld();
 		
 		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& = {} );
-		pod::PhysicsBody& UF_API create( uf::Object&, const uf::Mesh& mesh, float mass = 0.0f, const pod::Vector3f& = {}, bool convex = false );
-
 		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& = {} );
-		pod::PhysicsBody& UF_API create( pod::World&, uf::Object& object, const pod::TriangleWithNormal& tri, float mass = 0.0f, const pod::Vector3f& = {} );
-		pod::PhysicsBody& UF_API create( pod::World&, uf::Object&, const uf::Mesh& mesh, float mass = 0.0f, const pod::Vector3f& = {}, bool convex = false );
-
 		void UF_API destroy( uf::Object& );
 		void UF_API destroy( pod::PhysicsBody& );
 
-		pod::RayQuery UF_API rayCast( const pod::Ray&, const pod::PhysicsBody&, float = FLT_MAX );
-		pod::RayQuery UF_API rayCast( const pod::Ray&, const pod::World&, float = FLT_MAX );
-		pod::RayQuery UF_API rayCast( const pod::Ray&, const pod::World&, const pod::PhysicsBody*, float = FLT_MAX );
+		pod::Constraint& UF_API constrain( uf::Object&, uf::Object& );
+		pod::Constraint& UF_API constrain( pod::PhysicsBody&, pod::PhysicsBody& );
+		pod::Constraint& UF_API constrain( pod::World&, uf::Object&, uf::Object& );
+		pod::Constraint& UF_API constrain( pod::World&, pod::PhysicsBody&, pod::PhysicsBody& );
+		
+		void UF_API unconstrain( pod::PhysicsBody& );
+		void UF_API unconstrain( uf::Object& );
 	}
 }

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

@@ -1,26 +1,57 @@
 #pragma once
 
-#include "impl.h"
+#include "structs.h"
 
 namespace impl {
+	/*FORCE_INLINE*/ pod::SolverBodyContext solverBodyContext( const pod::PhysicsBody& body );
+
+	float computeEffectiveMass( const pod::SolverBodyContext& a, const pod::SolverBodyContext& b, const pod::JacobianRow& row );
 	float computeEffectiveMass( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& n );
+	float computeMassMatrixLine( const pod::SolverBodyContext& a, const pod::SolverBodyContext& b, const pod::JacobianRow& rowI, const pod::JacobianRow& rowJ );
+	float computeAngularMassMatrixLine( const pod::SolverBodyContext& a, const pod::SolverBodyContext& b, const pod::Vector3f& n_i, const pod::Vector3f& n_j );
+
 	void applyImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& impulse );
+	void applyImpulseTo(
+		pod::PhysicsBody& a, pod::PhysicsBody& b,
+		const pod::Vector3f& rA, const pod::Vector3f& rB,
+		const pod::Vector3f& direction, float magnitude,
+		float& accumulatedImpulse,
+		float minLimit = -std::numeric_limits<float>::max(), float maxLimit = std::numeric_limits<float>::max()
+	);
+	void applyImpulseTo(
+		pod::PhysicsBody& a, pod::PhysicsBody& b,
+		const pod::Vector3f& rA, const pod::Vector3f& rB,
+		const pod::Vector3f& impulse,
+		pod::Vector3f& accumulatedImpulse,
+		float minLimit = -std::numeric_limits<float>::max(), float maxLimit = std::numeric_limits<float>::max()
+	);
+	
 	void applyPseudoImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& impulse );
+	void applyPseudoImpulseTo(
+		pod::PhysicsBody& a, pod::PhysicsBody& b,
+		const pod::Vector3f& rA, const pod::Vector3f& rB,
+		const pod::Vector3f& direction, float magnitude,
+		float& accumulatedImpulse,
+		float minLimit = -std::numeric_limits<float>::max(), float maxLimit = std::numeric_limits<float>::max()
+	);
+	void applyPseudoImpulseTo(
+		pod::PhysicsBody& a, pod::PhysicsBody& b,
+		const pod::Vector3f& rA, const pod::Vector3f& rB,
+		const pod::Vector3f& impulse,
+		pod::Vector3f& accumulatedImpulse,
+		float minLimit = -std::numeric_limits<float>::max(), float maxLimit = std::numeric_limits<float>::max()
+	);
+
+	float accumulateImpulseTo(
+		float magnitude, float& accumulatedImpulse,
+		float minLimit = -std::numeric_limits<float>::max(), float maxLimit = std::numeric_limits<float>::max()
+	);
+	pod::Vector3f accumulateImpulseTo(
+		const pod::Vector3f& impulse, pod::Vector3f& accumulatedImpulse,
+		float minLimit = -std::numeric_limits<float>::max(), float maxLimit = std::numeric_limits<float>::max()
+	);
+
 	void applyRollingResistance( pod::PhysicsBody& body, float dt );
-	void bindManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt = 0 );
-	bool generateContactsGjk( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
-	bool generateContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
-	void computeLocalContacts( pod::Manifold& manifold );
-	bool similarContact( const pod::Contact& a, const pod::Contact& b, float distSqThreshold = 1.0e-2f, float normThreshold = 0.9f );
-	void reduceContacts( pod::Manifold& manifold );
-	void mergeContacts( pod::Manifold& manifold );
-	void retrieveContacts( pod::Manifold& current, const pod::Manifold& previous, float distanceThreshold = 0.1f, float separationThreshold = 0.1f, float decay = 0.85f );
-	void prepareManifoldCache( uf::stl::unordered_map<size_t, pod::Manifold>& cache, const uf::stl::vector<pod::Island>& islands, const uf::stl::vector<pod::PhysicsBody*>& bodies );
-	void updateManifoldCache( const uf::stl::vector<pod::Manifold>& manifolds, uf::stl::unordered_map<size_t, pod::Manifold>& cache );
-	void pruneManifoldCache( uf::stl::unordered_map<size_t, pod::Manifold>& cache );
-	void warmupContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& c, float dt );
-	void warmupManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Manifold& manifold, float dt );
 	void snapVelocity( pod::PhysicsBody& body, float dt, float threshold = 0.01f );
-	void positionCorrection( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& contact );
 	void integrate( pod::PhysicsBody& body, float dt );
 }

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

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "impl.h"
+#include "structs.h"
 
 #include "narrowphase/aabb.h"
 #include "narrowphase/obb.h"

engine/inc/uf/utils/math/physics/narrowphase/aabb.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../impl.h"
+#include "../structs.h"
 
 namespace impl {
 	bool aabbAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );
@@ -12,4 +12,11 @@ namespace impl {
 	bool aabbHull( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );
 
 	void drawAabb( const pod::PhysicsBody& body );
+}
+
+namespace uf {
+	namespace physics {
+		pod::PhysicsBody& UF_API create( 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::AABB& aabb, float mass = 0.0f, const pod::Vector3f& = {} );
+	}
 }

engine/inc/uf/utils/math/physics/narrowphase/capsule.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../impl.h"
+#include "../structs.h"
 
 namespace impl {
 	bool capsuleAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );
@@ -12,4 +12,11 @@ namespace impl {
 	bool capsuleHull( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );
 
 	void drawCapsule( const pod::PhysicsBody& body );
+}
+
+namespace uf {
+	namespace physics {
+		pod::PhysicsBody& UF_API create( uf::Object& object, const pod::Capsule& capsule, 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/narrowphase/epa.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../impl.h"
+#include "../structs.h"
 
 namespace impl {
 	void addOrRemoveBorder( uf::stl::vector<std::pair<pod::SupportPoint, pod::SupportPoint>>& edges, std::pair<pod::SupportPoint, pod::SupportPoint> e);

engine/inc/uf/utils/math/physics/narrowphase/gjk.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../impl.h"
+#include "../structs.h"
 
 namespace impl {
 	pod::Vector3f support( const pod::PhysicsBody& body, const pod::Vector3f& dir );

engine/inc/uf/utils/math/physics/narrowphase/hull.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../impl.h"
+#include "../structs.h"
 
 namespace impl {
 	bool hullAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );

engine/inc/uf/utils/math/physics/narrowphase/mesh.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../impl.h"
+#include "../structs.h"
 
 namespace impl {
 	bool meshAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );
@@ -12,4 +12,11 @@ namespace impl {
 	bool meshHull( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );
 
 	void drawMesh( const pod::PhysicsBody& body );
+}
+
+namespace uf {
+	namespace physics {
+		pod::PhysicsBody& UF_API create( uf::Object&, const uf::Mesh& mesh, float mass = 0.0f, const pod::Vector3f& = {}, bool convex = false );
+		pod::PhysicsBody& UF_API create( pod::World&, uf::Object&, const uf::Mesh& mesh, float mass = 0.0f, const pod::Vector3f& = {}, bool convex = false );
+	}
 }

engine/inc/uf/utils/math/physics/narrowphase/obb.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../impl.h"
+#include "../structs.h"
 
 namespace impl {
 	bool obbAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );
@@ -12,4 +12,11 @@ namespace impl {
 	bool obbHull( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );
 
 	void drawObb( const pod::PhysicsBody& body );
+}
+
+namespace uf {
+	namespace physics {
+		pod::PhysicsBody& UF_API create( 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::OBB& obb, float mass = 0.0f, const pod::Vector3f& = {} );
+	}
 }

engine/inc/uf/utils/math/physics/narrowphase/plane.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../impl.h"
+#include "../structs.h"
 
 namespace impl {
 	bool planeAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );
@@ -12,4 +12,11 @@ namespace impl {
 	bool planeHull( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );
 
 	void drawPlane( const pod::PhysicsBody& body );
+}
+
+namespace uf {
+	namespace physics {
+		pod::PhysicsBody& UF_API create( 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::Plane& plane, float mass = 0.0f, const pod::Vector3f& = {} );
+	}
 }

engine/inc/uf/utils/math/physics/narrowphase/ray.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../impl.h"
+#include "../structs.h"
 
 namespace impl {
 	bool rayTriangleIntersect( const pod::Ray& ray, const pod::Triangle& tri, float& t, float& u, float& v );
@@ -15,4 +15,12 @@ namespace impl {
 	bool rayHull( const pod::Ray& r, const pod::PhysicsBody& body, pod::RayQuery& rayHit );
 
 	void drawRay( const pod::Ray& r, const pod::RayQuery& query );
+}
+
+namespace uf {
+	namespace physics {
+		pod::RayQuery UF_API rayCast( const pod::Ray&, const pod::PhysicsBody&, float = FLT_MAX );
+		pod::RayQuery UF_API rayCast( const pod::Ray&, const pod::World&, float = FLT_MAX );
+		pod::RayQuery UF_API rayCast( const pod::Ray&, const pod::World&, const pod::PhysicsBody*, float = FLT_MAX );
+	}
 }

engine/inc/uf/utils/math/physics/narrowphase/sphere.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../impl.h"
+#include "../structs.h"
 
 namespace impl {
 	bool sphereAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );
@@ -12,4 +12,11 @@ namespace impl {
 	bool sphereHull( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );
 
 	void drawSphere( const pod::PhysicsBody& body );
+}
+
+namespace uf {
+	namespace physics {
+		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( uf::Object& object, const pod::Sphere& sphere, float mass = 0.0f, const pod::Vector3f& = {} );
+	}
 }

engine/inc/uf/utils/math/physics/narrowphase/triangle.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../impl.h"
+#include "../structs.h"
 
 namespace impl {
 	bool triangleTriangle( const pod::TriangleWithNormal& a, const pod::TriangleWithNormal& b, pod::Manifold& manifold );
@@ -20,4 +20,11 @@ namespace impl {
 	bool triangleHull( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold );
 
 	void drawTriangle( const pod::PhysicsBody& body );
+}
+
+namespace uf {
+	namespace physics {
+		pod::PhysicsBody& UF_API create( uf::Object& object, const pod::TriangleWithNormal& tri, float mass = 0.0f, const pod::Vector3f& = {} );
+		pod::PhysicsBody& UF_API create( pod::World&, uf::Object& object, const pod::TriangleWithNormal& tri, float mass = 0.0f, const pod::Vector3f& = {} );
+	}
 }

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

@@ -1,12 +1,12 @@
 #pragma once
 
-#include "impl.h"
+#include "structs.h"
+
+#include "constraints/contact.h"
 
 #include "solvers/block.h"
 #include "solvers/iterativeImpulse.h"
 
 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 = 4 );
 }

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

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../impl.h"
+#include "../structs.h"
 
 namespace impl {
 	/*FORCE_INLINE*/ bool block2x2Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );

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

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../impl.h"
+#include "../structs.h"
 
 namespace impl {
 	void iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Contact& contact, float dt );

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

@@ -0,0 +1,489 @@
+#pragma once
+
+#include <uf/utils/math/vector.h>
+#include <uf/utils/math/quaternion.h>
+#include <uf/utils/math/matrix.h>
+#include <uf/utils/math/shapes.h>
+#include <uf/utils/math/transform.h>
+
+#include <uf/utils/memory/vector.h>
+#include <uf/utils/memory/unordered_map.h>
+#include <uf/utils/memory/unordered_set.h>
+#include <uf/utils/memory/stack.h>
+
+#include <uf/engine/object/object.h>
+#include <uf/utils/mesh/mesh.h>
+
+#include <cfloat>
+
+
+#define EPS 1.0e-6f
+#define EPS2 (EPS * EPS)
+#define ASSERT_COLLIDER_TYPES( A, B ) UF_ASSERT( a.collider.type == pod::ShapeType::A && b.collider.type == pod::ShapeType::B );
+
+#define REORIENT_NORMALS_ON_FETCH 0
+#define REORIENT_NORMALS_ON_CONTACT 1
+
+#define REVERSE_COLLIDER( a, b, fun )\
+	auto start = manifold.points.size();\
+	if ( !::fun( b, a, manifold ) ) return false;\
+	for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal;\
+	return true;
+
+// Forward declates
+namespace pod {
+	struct World;
+	struct PhysicsBody; 
+}
+
+// GJK
+namespace pod {
+	struct SupportPoint {
+		pod::Vector3f p;
+		pod::Vector3f pA;
+		pod::Vector3f pB;
+	};
+
+	struct Simplex {
+		uf::stl::vector<pod::SupportPoint> pts;
+	};
+
+	struct Face {
+		pod::SupportPoint a, b, c;
+		pod::Vector3f normal;
+		float distance;
+	};
+}
+
+// Constraints
+namespace pod {
+	enum class ConstraintType {
+		BALL_AND_SOCKET,
+		HINGE,
+		CONE_TWIST,
+		SLIDER,
+		DISTANCE,
+		WELD,
+		SPRING,
+		//CONTACT
+	};
+
+	// cannot provide default initialization values as that makes them non-trivial for unions
+	struct BallSocket {
+		pod::Vector3f localAnchorA;
+		pod::Vector3f localAnchorB;
+
+		pod::Vector3f accumulatedImpulse;
+	};
+
+	struct Hinge {
+		pod::Vector3f localAnchorA;
+		pod::Vector3f localAnchorB;
+
+		pod::Vector3f accumulatedImpulse;
+		pod::Vector2f accumulatedAngularImpulse;
+
+		pod::Vector3f localAxisA;
+		pod::Vector3f localAxisB;
+	};
+	
+	struct ConeTwist {
+		pod::Vector3f localAnchorA;
+		pod::Vector3f localAnchorB;
+
+		pod::Vector3f accumulatedImpulse;
+		pod::Vector3f accumulatedAngularImpulse;
+
+		pod::Vector3f localTwistAxisA;
+		pod::Vector3f localTwistAxisB;
+
+		pod::Vector3f localReferenceAxisA;
+		pod::Vector3f localReferenceAxisB;
+
+		float swingLimit; // M_PI / 4.0f;
+		float twistLimit; // M_PI / 8.0f;
+	};
+
+	struct Slider {
+		pod::Vector3f localAnchorA;
+		pod::Vector3f localAnchorB;
+
+		pod::Vector2f accumulatedLinearImpulse;
+		pod::Vector3f accumulatedAngularImpulse;
+
+		pod::Vector3f localAxisA;
+		pod::Vector3f localAxisB;
+
+		pod::Vector3f localReferenceAxisA;
+		pod::Vector3f localReferenceAxisB;
+
+		float lowerLimit;
+		float upperLimit;
+		float accumulatedLimitImpulse;
+	};
+
+	struct Distance {
+		pod::Vector3f localAnchorA;
+		pod::Vector3f localAnchorB;
+
+		float accumulatedImpulse;
+		float targetDistance;
+		bool isRope; // to-do: rename to something more generic
+	};
+
+	struct Weld {
+		pod::Vector3f localAnchorA;
+		pod::Vector3f localAnchorB;
+
+		pod::Vector3f accumulatedLinearImpulse;
+		pod::Vector3f accumulatedAngularImpulse;
+
+		pod::Vector3f localAxisA;
+		pod::Vector3f localAxisB;
+
+		pod::Vector3f localReferenceAxisA;
+		pod::Vector3f localReferenceAxisB;
+	};
+
+	struct Spring {
+		pod::Vector3f localAnchorA;
+		pod::Vector3f localAnchorB;
+		
+		float accumulatedImpulse;
+
+		float restLength;
+		float stiffness;
+		float damping;
+	};
+
+	struct Motor {
+		bool enabled = false;
+		float targetVelocity = 0.0f;
+		union {
+			float maxMotorForce;
+			float maxMotorTorque;
+		};
+		float accumulatedMotorImpulse = 0.0f;
+	};
+
+	// this /could/ mirror the above joint-based constraints
+	// but a lot of the code initializes contact structs as Contact{ point, normal, depth }, which would not easily align with the above structs
+	struct Contact {
+		pod::Vector3f point;
+		pod::Vector3f normal;
+		float penetration;
+
+		pod::Vector3f localA;
+		pod::Vector3f localB;
+
+		// warm-start cached values
+		int lifetime = 0;
+		pod::Vector3f tangent;
+		float accumulatedNormalImpulse = 0.0f;
+		float accumulatedTangentImpulse = 0.0f;
+		float accumulatedPseudoImpulse = 0.0f;
+	};
+
+	struct Manifold {
+		pod::PhysicsBody* a = NULL;
+		pod::PhysicsBody* b = NULL;
+		uf::stl::vector<pod::Contact> points;
+	};
+
+	struct Constraint {
+		pod::ConstraintType type = {};
+		pod::PhysicsBody* a = NULL;
+		pod::PhysicsBody* b = NULL;
+
+		union {
+			pod::BallSocket ballSocket;
+			pod::Hinge hinge;
+			pod::ConeTwist coneTwist;
+			pod::Slider slider;
+			pod::Distance distance;
+			pod::Weld weld;
+			pod::Spring spring;
+		};
+		pod::Motor motor;
+	};
+
+	struct SolverBodyContext {
+		float invM = 0;
+		pod::Matrix3f invI = {};
+	};
+
+	struct JacobianRow {
+		pod::Vector3f rA;
+		pod::Vector3f rB;
+		pod::Vector3f n;
+	};
+}
+
+// BVH
+namespace pod {
+	struct BVH {
+		typedef uint32_t index_t;
+		typedef std::pair<index_t,index_t> pair_t;
+		
+		struct PairHash {
+			size_t operator()( const pair_t& p ) const noexcept {
+				uint64_t a = (uint64_t) std::min(p.first, p.second);
+				uint64_t b = (uint64_t) std::max(p.first, p.second);
+				return (a << 32) ^ b;
+			}
+		};
+		struct PairEq {
+			bool operator()( const pair_t& a, const pair_t& b ) const noexcept {
+				return (a.first == b.first && a.second == b.second) || (a.first == b.second && a.second == b.first);
+			}
+		};
+
+		typedef uf::stl::vector<pair_t> pairs_t;
+		
+		struct Node {
+			BVH::index_t left = 0;
+			BVH::index_t right = 0;
+			BVH::index_t start = 0;
+			BVH::index_t flags = 0;
+
+			BVH::index_t getCount() const { return flags & 0x7FFFFFFF; }
+			bool isAsleep() const { return (flags & 0x80000000u) != 0; }
+			void setCount(BVH::index_t c) { flags = (flags & 0x80000000u) | (c & 0x7FFFFFFF); }
+			void setAsleep(bool a) { flags = (flags & 0x7FFFFFFF) | (a ? 0x80000000u : 0); }
+		};
+		struct FlatNode {
+			BVH::index_t start = 0;
+			BVH::index_t skipIndex = 0;
+			BVH::index_t flags = 0;
+
+			BVH::index_t getCount() const { return flags & 0x7FFFFFFF; }
+			bool isAsleep() const { return (flags & 0x80000000u) != 0; }
+			void setCount(BVH::index_t c) { flags = (flags & 0x80000000u) | (c & 0x7FFFFFFF); }
+			void setAsleep(bool a) { flags = (flags & 0x7FFFFFFF) | (a ? 0x80000000u : 0); }
+		};
+		struct UpdatePolicy {
+			enum class Decision {
+				NONE,	// do nothing
+				REFIT,   // refit bounds
+				REBUILD  // rebuild from scratch
+			};
+			float displacementThreshold = 0.25f; // 25% of AABB size
+			float overlapThreshold = 2.0f;	   // 2x growth in root surface area
+			float dirtyRatioThreshold = 0.3f;	// 30% dirty bodies
+			uint16_t maxFramesBeforeRebuild = 600;   // force rebuild every 600 frames
+		};
+
+		bool dirty = false;
+		uf::stl::vector<pod::BVH::index_t> indices;
+		uf::stl::vector<pod::BVH::Node> nodes;
+		uf::stl::vector<pod::BVH::FlatNode> flattened;
+		
+		uf::stl::vector<pod::AABB> bounds;
+		uf::stl::vector<pod::AABB> flatBounds;
+	};
+
+	struct RayQuery {
+		bool hit = false;
+		const pod::PhysicsBody* body = NULL;
+		const pod::PhysicsBody* invoker = NULL;
+		pod::Contact contact = { pod::Vector3f{}, pod::Vector3f{}, FLT_MAX };
+	};
+
+	struct Island {
+		bool awake = true;
+		uf::stl::vector<uint32_t> indices;
+		pod::BVH::pairs_t pairs;
+
+		uf::stl::vector<pod::Manifold> manifolds;
+		uf::stl::vector<pod::Constraint*> constraints;
+	};
+}
+
+// Colliders
+namespace pod {
+	enum class ShapeType {
+		AABB,
+		OBB,
+		SPHERE,
+		PLANE,
+		CAPSULE,
+		TRIANGLE,
+		MESH,
+		CONVEX_HULL,
+	};
+
+	struct MeshBVH {
+		pod::BVH* bvh;
+		const uf::Mesh* mesh;
+	};
+	typedef MeshBVH ConvexHullBVH;
+
+	typedef uint32_t CollisionMask;
+	struct Collider {
+		// what it is
+		enum CategoryMask : uint32_t {
+			CATEGORY_NONE			= 0,
+			CATEGORY_STATIC			= 1 << 0,
+			CATEGORY_DYNAMIC		= 1 << 1,
+			CATEGORY_PLAYER	  		= 1 << 2,
+			CATEGORY_NPC			= 1 << 3,
+			CATEGORY_TRIGGER		= 1 << 4,
+			CATEGORY_PROJECTILE		= 1 << 5,
+			CATEGORY_CHARACTER		= CATEGORY_PLAYER | CATEGORY_NPC,
+			CATEGORY_ALL			= 0xFFFFFFFF
+		};
+		// what it collides with
+		enum CollisionMask : uint32_t {
+			MASK_NONE				= 0,
+			MASK_STATIC				= CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_NPC | CATEGORY_PROJECTILE,
+			MASK_DYNAMIC			= CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_NPC,
+			MASK_PLAYER				= CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_NPC | CATEGORY_PROJECTILE,
+			MASK_NPC				= CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_PROJECTILE,
+			MASK_TRIGGER			= CATEGORY_PLAYER | CATEGORY_NPC,
+			MASK_PROJECTILE			= CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_NPC,
+			MASK_CHARACTER			= MASK_PLAYER | MASK_NPC,
+			MASK_ALL				= 0xFFFFFFFF
+		};
+
+		pod::ShapeType type;
+		pod::CollisionMask category = Collider::CATEGORY_ALL;
+		pod::CollisionMask mask = Collider::MASK_ALL;
+
+		union {
+			pod::AABB aabb;
+			pod::OBB obb;
+			pod::Sphere sphere;
+			pod::Plane plane;
+			pod::Capsule capsule;
+			pod::TriangleWithNormal triangle;
+			pod::MeshBVH mesh;
+			pod::ConvexHullBVH convexHull;
+		};
+	};
+
+	struct PhysicsMaterial {
+		float restitution = 0.2f;
+		float staticFriction = 0.5f;
+		float dynamicFriction = 0.3f;
+	};
+	
+	struct Activity {
+		bool awake = true;
+		bool grounded = false;
+		float sleepTimer = 0.0f;
+		int32_t islandID = -1;
+		static constexpr float sleepThreshold = 0.5f; // seconds
+		static constexpr float linearSleepEpsilon = 0.2f; // m/s
+		static constexpr float angularSleepEpsilon = 0.2f; // rad/s		
+	};
+}
+	
+namespace pod {
+	struct PhysicsBody {
+		pod::World* world = NULL;
+		uf::Object* object = NULL;
+		pod::Transform<>* transform = NULL;
+
+		bool isStatic = false;
+
+		float mass = 1.0f;
+		float inverseMass = 1.0f; // for fast division
+		int32_t viewIndex = -1; // -1 means it's not an aliased view
+
+		pod::Vector3f offset = {};
+
+		pod::Vector3f velocity = {};
+		pod::Vector3f forceAccumulator = {};
+
+		pod::Vector3f angularVelocity = {};
+		pod::Vector3f torqueAccumulator = {};
+
+		pod::Vector3f pseudoVelocity = {};
+		pod::Vector3f pseudoAngularVelocity = {};
+
+		pod::Vector3f inverseInertiaTensor = { 1, 1, 1 };
+
+		pod::Vector3f gravity = { NAN, NAN, NAN }; // an invalid gravity will fallback to world gravity
+
+		pod::AABB bounds;
+		pod::Collider collider;
+		pod::PhysicsMaterial material;
+		pod::Activity activity;
+	};
+}
+
+// Settings
+namespace pod {
+	struct PhysicsSettings {
+		bool warmupSolver = true; // cache manifold data to warm up the solver
+		bool blockContactSolver = true; // use BlockNxN solvers (where N = number of contacts for a manifold)
+		bool resolveBlockContact = true; // attempts to resolve an invalid BlockNxN solve with an BlockN-1xN-1 solve
+		bool useGjk = false; // currently don't have a way to broadphase mesh => narrowphase tri via GJK
+		
+		pod::CollisionMask debugDraw = pod::Collider::CATEGORY_NONE; // draws wireframe of collision bodies
+		bool async = false; // dedicated thread for physics sim
+		float timestep = 1.0f / 60.0f; // timestep for fixed step ticks
+		bool fixedStep = true; // run physics simulation with a fixed delta time (with accumulation), rather than rely on actual engine deltatime
+		uint32_t substeps = 4; // number of substeps per frame tick
+		uint32_t reserveCount = 32; // amount of elements to reserve for vectors used in this system, to-do: have it tie to a memory pool allocator
+
+		uint32_t broadphaseBvhCapacity = 1; // number of bodies per leaf node
+		uint32_t meshBvhCapacity = 1; // number of triangles per leaf node
+
+		// additionally flattens a BVH for linear iteration, rather than a recursive / stack-based traversal
+		bool flattenBvhBodies = true;
+		bool flattenBvhMeshes = true;
+		
+		// use surface area heuristics for building the BVH, rather than naive splits
+		bool useBvhSahBodies = true;
+		bool useBvhSahMeshes = true;
+
+		bool useSplitBvhs = true; // creates separate BVHs for static / dynamic objects
+
+		// to-do: find possibly better values for this
+		uint32_t solverIterations = 10;
+		bool ngsPositionSolver = false;
+		float baumgarteCorrectionPercent = 0.2f;
+		float baumgarteCorrectionSlop = 0.005f;
+		float maxLinearCorrection = 0.2f;
+		
+		float contactCFM = 1e-3f;
+		float jointCFM = 1e-4f;
+
+		uf::stl::unordered_map<size_t, pod::Manifold> manifoldsCache;
+		uint32_t manifoldCacheLifetime = 0; // 0 = derive from current settings
+
+		uint32_t frameCounter = 0;
+
+		// to-do: tweak this to not be annoying
+		pod::BVH::UpdatePolicy bvhUpdatePolicy = {
+			// triggers a refit
+			.displacementThreshold = 0.25f,
+			// triggers a rebuild
+			.overlapThreshold = 2.0f,
+			.dirtyRatioThreshold = 0.3,
+			.maxFramesBeforeRebuild = 60,
+		};
+
+		float groundedThreshold = 0.7f; // threshold before marking a body as grounded
+	};
+
+	struct World {
+		uf::stl::vector<pod::PhysicsBody*> bodies;
+		uf::stl::vector<pod::Constraint*> constraints;
+	
+		pod::Vector3f gravity = { 0, -9.81f, 0 };
+		pod::BVH dynamicBvh;
+		pod::BVH staticBvh;
+	};
+}
+
+namespace uf {
+	namespace physics {
+		typedef pod::Math::num_t num_t;
+		namespace time = uf::time; // to-do: have separate values from the physics system
+		
+		extern UF_API pod::World world;
+		extern UF_API pod::PhysicsSettings settings;
+	}
+}

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

@@ -133,7 +133,10 @@ namespace uf {
 		template<typename T> /*FORCE_INLINE*/ T  /*UF_API*/ max( const T& left, const T& right ); // returns the maximum of each component between two vectors
 		template<typename T> /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ min( const T& v ); // returns the minimum component of a vector
 		template<typename T> /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ max( const T& v ); // returns the maximum component of a vector
+		template<typename T> /*FORCE_INLINE*/ T  /*UF_API*/ min( const T& vector, typename T::type_t& min ); // applies min on a vector
+		template<typename T> /*FORCE_INLINE*/ T  /*UF_API*/ max( const T& vector, typename T::type_t& max ); // applies max on a vector
 		template<typename T> /*FORCE_INLINE*/ T  /*UF_API*/ clamp( const T& vector, const T& min, const T& max ); // clamps a vector between two bounds
+		template<typename T> /*FORCE_INLINE*/ T  /*UF_API*/ clamp( const T& vector, typename T::type_t& min, typename T::type_t& max ); // clamps a vector between a bound
 		template<typename T> /*FORCE_INLINE*/ T  /*UF_API*/ ceil( const T& vector ); // rounds each component of the the vector up
 		template<typename T> /*FORCE_INLINE*/ T  /*UF_API*/ floor( const T& vector ); // rounds each component of the vector down
 		template<typename T> /*FORCE_INLINE*/ T  /*UF_API*/ round( const T& vector );  // rounds each component of the vector

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

@@ -507,6 +507,14 @@ T uf::vector::min( const T& left, const T& right ) {
 	return res;
 }
 template<typename T>
+T uf::vector::min( const T& vector, typename T::type_t& min ) {
+	T res = vector;
+	FOR_EACH(T::size, {
+		res[i] = std::min( res[i], min );
+	});
+	return res;
+}
+template<typename T>
 typename T::type_t uf::vector::max( const T& v ) {
 	typename T::type_t res = v[0];
 	FOR_EACH(T::size - 1, {
@@ -527,11 +535,28 @@ T uf::vector::max( const T& left, const T& right ) {
 	});
 	return res;
 }
+template<typename T>
+T uf::vector::max( const T& vector, typename T::type_t& max ) {
+	T res = vector;
+	FOR_EACH(T::size, {
+		res[i] = std::max( res[i], max );
+	});
+	return res;
+}
+
 template<typename T>
 T uf::vector::clamp( const T& vector, const T& min, const T& max ) {
 	return uf::vector::max( min, uf::vector::min( vector, max ) );
 }
 template<typename T>
+T uf::vector::clamp( const T& vector, typename T::type_t& min, typename T::type_t& max ) {
+	T res = vector;
+	FOR_EACH(T::size, {
+		res[i] = std::clamp( res[i], min, max );
+	});
+	return res;
+}
+template<typename T>
 T uf::vector::ceil( const T& vector ) {
 	T res;
 	FOR_EACH(T::size, {

engine/src/ext/vulkan/buffer.cpp

@@ -146,7 +146,7 @@ bool ext::vulkan::Buffer::update( const void* data, VkDeviceSize length ) const
 
 	// to-do: fix this because it's a thorn in my side when a mesh needs to update
 	if ( length > allocationInfo.size ) {
-		UF_MSG_WARNING("Buffer update of {} exceeds buffer size of {}", length, allocationInfo.size);
+		//UF_MSG_WARNING("Buffer update of {} exceeds buffer size of {}", length, allocationInfo.size);
 
 		auto savedUsage = usage;
 		auto savedMemProps = memoryProperties;

engine/src/utils/math/physics/broadphase/island.cpp

@@ -37,9 +37,15 @@ namespace impl {
 }
 
 // to-do: rewrite this, I'm pretty sure it's faulty
-void impl::buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector<pod::PhysicsBody*>& bodies, uf::stl::vector<pod::Island>& islands ) {
+void impl::buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector<pod::PhysicsBody*>& bodies, const uf::stl::vector<pod::Constraint*>& constraints, uf::stl::vector<pod::Island>& islands ) {
 	UnionFind unionizer(bodies.size());
 
+	// map bodies to indices
+	uf::stl::unordered_map<pod::PhysicsBody*, pod::BVH::index_t> bodyToIndex;
+	for ( pod::BVH::index_t i = 0; i < bodies.size(); i++ ) {
+		bodyToIndex[bodies[i]] = i;
+	}
+
 	// union all pairs
 	for ( auto& [a, b] : pairs ) {
 		if ( !bodies[a]->isStatic && !bodies[b]->isStatic ) {
@@ -47,6 +53,17 @@ void impl::buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector<p
 		}
 	}
 
+	for ( auto* constraint : constraints ) {
+		auto itA = bodyToIndex.find(constraint->a);
+		auto itB = bodyToIndex.find(constraint->b);
+
+		if (itA != bodyToIndex.end() && itB != bodyToIndex.end()) {
+			if ( !constraint->a->isStatic && !constraint->b->isStatic ) {
+				unionizer.unite(itA->second, itB->second);
+			}
+		}
+	}
+
 	// map root to island index
 	typedef uf::stl::unordered_map<pod::BVH::index_t, pod::BVH::index_t> map_t;
 	STATIC_THREAD_LOCAL(map_t, rootToIsland);
@@ -87,6 +104,23 @@ void impl::buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector<p
 		}
 	}
 
+	for ( auto* constraint : constraints ) {
+		auto itA = bodyToIndex.find(constraint->a);
+		if (itA == bodyToIndex.end()) continue;
+
+		pod::BVH::index_t root = unionizer.find(itA->second);
+		if ( rootToIsland.find(root) != rootToIsland.end() ) {
+			pod::BVH::index_t islandID = rootToIsland[root];
+			islands[islandID].constraints.push_back(constraint);
+
+			// Wake bodies if connected by a constraint and one is awake
+			if ( constraint->a->activity.awake || constraint->b->activity.awake ) {
+				if (!constraint->a->isStatic) impl::wakeBody( *constraint->a );
+				if (!constraint->b->isStatic) impl::wakeBody( *constraint->b );
+			}
+		}
+	}
+
 	// update islands
 	for ( auto it = islands.begin(); it != islands.end(); ) {
 		auto& island = *it;

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

@@ -46,11 +46,11 @@ void impl::updateActivity( pod::PhysicsBody& body, float dt ) {
 	if ( !body.activity.awake ) return;
 
 	// check if body is moving
-	float linSpeedSq = uf::vector::magnitude( body.velocity );
-	float angSpeedSq = uf::vector::magnitude( body.angularVelocity );
+	float linSpeed2 = uf::vector::magnitude( body.velocity );
+	float angSpeed2 = uf::vector::magnitude( body.angularVelocity );
 
 	// body is nearly still
-	if ( linSpeedSq < pod::Activity::linearSleepEpsilon && angSpeedSq < pod::Activity::angularSleepEpsilon ) {
+	if ( linSpeed2 < pod::Activity::linearSleepEpsilon && angSpeed2 < pod::Activity::angularSleepEpsilon ) {
 		body.activity.sleepTimer += dt;
 		float threshold = pod::Activity::sleepThreshold;
 
@@ -200,7 +200,7 @@ pod::Vector3f impl::closestPointSegmentAabb( const pod::Vector3f& p1, const pod:
 
 	if ( len2 > EPS2 ) {
 		// parametric closest t from box center
-		t = uf::vector::dot( c - p1, d ) / len2;
+		t = uf::vector::dot( c - p1, d ) / len2; // sqrt?
 		t = std::clamp( t, 0.0f, 1.0f );
 	}
 
@@ -387,9 +387,9 @@ float impl::segmentTriangleDistanceSq( const pod::Vector3f& p0, const pod::Vecto
 
 // Separating Axis Theorem test
 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;
+	float mag2 = uf::vector::magnitude( axis );
+	if ( mag2 < EPS2 ) return true;
+	pod::Vector3f n = axis / std::sqrt( mag2 );
 
 	// project triangle
 	float p0 = uf::vector::dot( triangle.points[0], n );
@@ -417,9 +417,9 @@ bool impl::testSeparatingAxis( const pod::Triangle& triangle, const pod::OBB& bo
 	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 mag2 = uf::vector::magnitude(axis);
+	if ( mag2 < EPS2 ) return true;
+	pod::Vector3f n = axis / std::sqrt( mag2 );
 
 	float pA = uf::vector::dot( boxA.center, n );
 	float rA = impl::projectExtents( boxA, n, axesA );
@@ -711,11 +711,11 @@ pod::Vector3f impl::extentFromAxes( const pod::OBB& box, const pod::Vector3f axe
 }
 //
 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 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

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

@@ -0,0 +1,28 @@
+#include <uf/utils/math/physics/common.h>
+#include <uf/utils/math/physics/constraints.h>
+
+void impl::solveConstraint( pod::Constraint& constraint, float dt ) {
+	switch ( constraint.type ) {
+		case pod::ConstraintType::BALL_AND_SOCKET: {
+			return impl::solveBallSocketConstraint( constraint, dt );
+		} break;
+		case pod::ConstraintType::HINGE: {
+			return impl::solveHingeConstraint( constraint, dt );
+		} break;
+		case pod::ConstraintType::CONE_TWIST: {
+			return impl::solveConeTwistConstraint( constraint, dt );
+		} break;
+		case pod::ConstraintType::SLIDER: {
+			return impl::solveSliderConstraint( constraint, dt );
+		} break;
+		case pod::ConstraintType::DISTANCE: {
+			return impl::solveDistanceConstraint( constraint, dt );
+		} break;
+		case pod::ConstraintType::WELD: {
+			return impl::solveWeldConstraint( constraint, dt );
+		} break;
+		case pod::ConstraintType::SPRING: {
+			return impl::solveSpringConstraint( constraint, dt );
+		} break;
+	}
+}

engine/src/utils/math/physics/constraints/ballSocket.cpp

@@ -0,0 +1,70 @@
+#include <uf/utils/math/physics/impl.h>
+#include <uf/utils/math/physics/common.h>
+#include <uf/utils/math/physics/integration.h>
+#include <uf/utils/math/physics/constraints.h>
+
+void impl::solveBallSocketConstraint( pod::Constraint& constraint, float dt ) {
+	auto& joint = constraint.ballSocket;
+	auto& a = *constraint.a;
+	auto& b = *constraint.b;
+
+	auto ctxA = impl::solverBodyContext( a );
+	auto ctxB = impl::solverBodyContext( b );
+
+	auto tA = impl::getTransform( a );
+	auto tB = impl::getTransform( b );
+
+	auto rA = uf::quaternion::rotate( tA.orientation, joint.localAnchorA );
+	auto rB = uf::quaternion::rotate( tB.orientation, joint.localAnchorB );
+
+	pod::Matrix3f K = {};
+	pod::Vector3f axes[3] = { {1,0,0}, {0,1,0}, {0,0,1} };
+	for ( auto i = 0; i < 3; ++i ) {
+		auto rowI = pod::JacobianRow{ rA, rB, axes[i] };
+		for ( auto j = 0; j < 3; ++j ) {
+			auto rowJ = pod::JacobianRow{ rA, rB, axes[j] };
+			K(i, j) = impl::computeMassMatrixLine( ctxA, ctxB, rowI, rowJ );
+		}
+
+		K(i,i) += uf::physics::settings.jointCFM * ( 1.0f + ctxA.invM + ctxB.invM );
+	}
+
+	auto pA = tA.position + rA;
+	auto pB = tB.position + rB;
+
+	auto vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);
+	auto vB = b.velocity + uf::vector::cross(b.angularVelocity, rB);
+	
+	auto relativeVelocity = vB - vA;
+	auto positionError = pB - pA;
+
+	pod::Matrix3f Kinv = uf::matrix::inverse( K );
+	pod::Vector3f bias = positionError * (uf::physics::settings.baumgarteCorrectionPercent / dt);
+	pod::Vector3f rhs = -(relativeVelocity + bias);
+
+	pod::Vector3f impulse = uf::matrix::multiply( Kinv, rhs );
+	impl::applyImpulseTo( a, b, rA, rB, impulse, joint.accumulatedImpulse );
+}
+
+pod::Constraint& uf::physics::constrain( pod::World& world, pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& joint ) {
+	auto& constraint = uf::physics::constrain( world, a, b );
+	constraint.type = pod::ConstraintType::BALL_AND_SOCKET;
+	// transform joint into local space 
+	auto tA = impl::getTransform( a );
+	auto tB = impl::getTransform( b );
+
+	constraint.ballSocket.localAnchorA = uf::transform::applyInverse( tA, joint );
+	constraint.ballSocket.localAnchorB = uf::transform::applyInverse( tB, joint );
+	constraint.ballSocket.accumulatedImpulse = {};
+
+	return constraint;
+}
+pod::Constraint& uf::physics::constrain( pod::World& world, uf::Object& a, uf::Object& b, const pod::Vector3f& joint ) {
+	return constrain( world, a.getComponent<pod::PhysicsBody>(), b.getComponent<pod::PhysicsBody>(), joint );
+}
+pod::Constraint& uf::physics::constrain( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& joint ) {
+	return constrain( uf::physics::getWorld(), a, b, joint );
+}
+pod::Constraint& uf::physics::constrain( uf::Object& a, uf::Object& b, const pod::Vector3f& joint ) {
+	return constrain( uf::physics::getWorld(), a.getComponent<pod::PhysicsBody>(), b.getComponent<pod::PhysicsBody>(), joint );
+}

engine/src/utils/math/physics/constraints/coneTwist.cpp

@@ -0,0 +1,180 @@
+#include <uf/utils/math/physics/impl.h>
+#include <uf/utils/math/physics/common.h>
+#include <uf/utils/math/physics/integration.h>
+#include <uf/utils/math/physics/constraints.h>
+
+void impl::solveConeTwistConstraint( pod::Constraint& constraint, float dt ) {
+	auto& joint = constraint.coneTwist;
+	auto& a = *constraint.a;
+	auto& b = *constraint.b;
+
+	// solve linearly
+	impl::solveBallSocketConstraint( constraint, dt );
+	
+	auto ctxA = impl::solverBodyContext( a );
+	auto ctxB = impl::solverBodyContext( b );
+
+	auto tA = impl::getTransform( a );
+	auto tB = impl::getTransform( b );
+
+	auto taA = uf::quaternion::rotate( tA.orientation, joint.localTwistAxisA );
+	auto taB = uf::quaternion::rotate( tB.orientation, joint.localTwistAxisB );
+
+	auto raA = uf::quaternion::rotate( tA.orientation, joint.localReferenceAxisA );
+	auto raB = uf::quaternion::rotate( tB.orientation, joint.localReferenceAxisB );
+
+	auto relAngularVel = b.angularVelocity - a.angularVelocity;
+
+	// swing
+	auto swingAxis = uf::vector::cross( taA, taB );
+	float swingLength2 = uf::vector::dot( swingAxis, swingAxis );
+	float swingAngle = 0.0f;
+	float swingError = 0.0f;
+	bool swingActive = false;
+
+	if ( swingLength2 > EPS2 ) {
+		swingAxis = swingAxis / std::sqrt( swingLength2 );
+		float dot = uf::vector::dot( taA, taB );
+		swingAngle = std::atan2( std::sqrt(swingLength2), dot );
+
+		if ( swingAngle > joint.swingLimit ) {
+			swingError = swingAngle - joint.swingLimit;
+			swingActive = true;
+		} else {
+			joint.accumulatedAngularImpulse.x = 0.0f;
+		}
+	}
+
+	// twist
+	pod::Vector3f twistAxis = taA;
+	float proj = uf::vector::dot( raB, twistAxis );
+	pod::Vector3f refB_projected = raB - (twistAxis * proj);
+
+	float refBLength2 = uf::vector::dot(refB_projected, refB_projected);
+	float twistError = 0.0f;
+	bool twistActive = false;
+
+	if ( refBLength2 > EPS2 ) {
+		refB_projected = refB_projected / std::sqrt(refBLength2);
+		pod::Vector3f crossRef = uf::vector::cross( raA, refB_projected );
+		float sinTheta = uf::vector::dot( crossRef, twistAxis );
+		float cosTheta = uf::vector::dot( raA, refB_projected );
+		float twistAngle = std::atan2( sinTheta, cosTheta );
+
+		if ( twistAngle > joint.twistLimit ) {
+			twistError = twistAngle - joint.twistLimit;
+			twistActive = true;
+		} else if ( twistAngle < -joint.twistLimit ) {
+			twistError = twistAngle + joint.twistLimit;
+			twistActive = true;
+		} else {
+			joint.accumulatedAngularImpulse.y = 0.0f;
+		}
+	}
+
+	// block solve
+	if ( swingActive && twistActive ) {
+		pod::Vector3f axes[2] = { swingAxis, twistAxis };
+		pod::Matrix2f K = {};
+		for ( auto i = 0; i < 2; ++i ) {
+			for ( auto j = 0; j < 2; ++j ) {
+				K(i, j) = impl::computeAngularMassMatrixLine( ctxA, ctxB, axes[i], axes[j]);
+			}
+			K(i,i) += uf::physics::settings.jointCFM * ( 1.0f + ctxA.invM + ctxB.invM );
+		}
+
+		pod::Matrix2f Kinv = uf::matrix::inverse( K );
+		pod::Vector2f rhs = {
+			-(uf::vector::dot(relAngularVel, swingAxis) + ((uf::physics::settings.baumgarteCorrectionPercent / dt) * swingError)),
+			-(uf::vector::dot(relAngularVel, twistAxis) + ((uf::physics::settings.baumgarteCorrectionPercent / dt) * twistError)),
+		};
+		pod::Vector2f impulse = uf::matrix::multiply( Kinv, rhs );
+
+		float unconstrainedSwing = joint.accumulatedAngularImpulse.x + impulse[0];
+		float unconstrainedTwist = joint.accumulatedAngularImpulse.y + impulse[1];
+
+		bool swingValid = ( unconstrainedSwing <= 0.0f );
+		bool twistValid = ( twistError > 0.0f ) ? ( unconstrainedTwist <= 0.0f ) : ( unconstrainedTwist >= 0.0f );
+
+		if ( swingValid && twistValid ) {
+			float swingDelta = unconstrainedSwing - joint.accumulatedAngularImpulse.x;
+			float twistDelta = unconstrainedTwist - joint.accumulatedAngularImpulse.y;
+
+			joint.accumulatedAngularImpulse.x = unconstrainedSwing;
+			joint.accumulatedAngularImpulse.y = unconstrainedTwist;
+
+			auto impulseVector = (swingAxis * swingDelta) + (twistAxis * twistDelta);
+			if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply( ctxA.invI, impulseVector );
+			if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply( ctxB.invI, impulseVector );
+
+			return;
+		}
+	}
+
+	// swing fallback: solve 1D
+	if ( swingActive ) {
+		float invMassN = impl::computeAngularMassMatrixLine( ctxA, ctxB, swingAxis, swingAxis );
+		UF_ASSERT( invMassN > EPS );
+
+		float bias = (uf::physics::settings.baumgarteCorrectionPercent / dt) * swingError;
+		float j = -(uf::vector::dot(relAngularVel, swingAxis) + bias) / invMassN;
+		auto impulse = swingAxis * impl::accumulateImpulseTo( j, joint.accumulatedAngularImpulse.x, -FLT_MAX, 0.0f );
+		if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply( ctxA.invI, impulse );
+		if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply( ctxB.invI, impulse );
+	}
+
+	// twist fallback: solve 1D
+	if ( twistActive ) {
+		float invMassN = impl::computeAngularMassMatrixLine( ctxA, ctxB, twistAxis, twistAxis );
+		UF_ASSERT( invMassN > EPS );
+
+		float bias = (uf::physics::settings.baumgarteCorrectionPercent / dt) * twistError;
+		float j = -(uf::vector::dot(relAngularVel, twistAxis) + bias) / invMassN;
+		float min = (twistError > 0.0f) ? -FLT_MAX : 0.0f;
+		float max = (twistError > 0.0f) ? 0.0f : FLT_MAX;
+		auto impulse = twistAxis * impl::accumulateImpulseTo( j, joint.accumulatedAngularImpulse.y, min, max );
+		if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply( ctxA.invI, impulse );
+		if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply( ctxB.invI, impulse );
+	}
+}
+
+pod::Constraint& uf::physics::constrain( pod::World& world, pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& joint, const pod::Vector3f& axis, float swingLimit, float twistLimit ) {
+	auto& constraint = uf::physics::constrain( world, a, b );
+	constraint.type = pod::ConstraintType::CONE_TWIST;
+
+	// transform joint into local space
+	auto tA = impl::getTransform( a );
+	auto tB = impl::getTransform( b );
+
+	auto normAxis = uf::vector::normalize( axis );
+	auto worldRefAxis = uf::vector::normalize( impl::computeTangent( normAxis ) );
+
+	auto invOriA = uf::quaternion::inverse( tA.orientation );
+	auto invOriB = uf::quaternion::inverse( tB.orientation );
+
+	constraint.coneTwist.localAnchorA = uf::transform::applyInverse( tA, joint );
+	constraint.coneTwist.localAnchorB = uf::transform::applyInverse( tB, joint );
+	
+	constraint.coneTwist.accumulatedImpulse = {};
+	constraint.coneTwist.accumulatedAngularImpulse = {};
+
+	constraint.coneTwist.localTwistAxisA = uf::quaternion::rotate( invOriA, normAxis );
+	constraint.coneTwist.localTwistAxisB = uf::quaternion::rotate( invOriB, normAxis );
+
+	constraint.coneTwist.localReferenceAxisA = uf::quaternion::rotate( invOriA, worldRefAxis );
+	constraint.coneTwist.localReferenceAxisB = uf::quaternion::rotate( invOriB, worldRefAxis );
+
+	constraint.coneTwist.swingLimit = swingLimit;
+	constraint.coneTwist.twistLimit = twistLimit;
+
+	return constraint;
+}
+pod::Constraint& uf::physics::constrain( pod::World& world, uf::Object& a, uf::Object& b, const pod::Vector3f& joint, const pod::Vector3f& axis, float swingLimit, float twistLimit ) {
+	return constrain( world, a.getComponent<pod::PhysicsBody>(), b.getComponent<pod::PhysicsBody>(), joint, axis, swingLimit, twistLimit );
+}
+pod::Constraint& uf::physics::constrain( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& joint, const pod::Vector3f& axis, float swingLimit, float twistLimit ) {
+	return constrain( uf::physics::getWorld(), a, b, joint, axis, swingLimit, twistLimit );
+}
+pod::Constraint& uf::physics::constrain( uf::Object& a, uf::Object& b, const pod::Vector3f& joint, const pod::Vector3f& axis, float swingLimit, float twistLimit ) {
+	return constrain( uf::physics::getWorld(), a.getComponent<pod::PhysicsBody>(), b.getComponent<pod::PhysicsBody>(), joint, axis, swingLimit, twistLimit );
+}

engine/src/utils/math/physics/constraints/contact.cpp

@@ -0,0 +1,354 @@
+#include <uf/utils/math/physics/common.h>
+#include <uf/utils/math/physics/narrowphase.h>
+#include <uf/utils/math/physics/integration.h>
+#include <uf/utils/math/physics/solvers.h>
+#include <uf/utils/math/physics/constraints/contact.h>
+
+void impl::bindManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
+	manifold.a = &a;
+	manifold.b = &b;
+	manifold.points.clear();
+	manifold.points.reserve(4);
+}
+
+bool impl::generateContactsGjk( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
+	impl::bindManifold( a, b, manifold, dt );
+
+	pod::Simplex simplex;
+
+	if ( !impl::gjk(a,b,simplex) ) return false;
+	auto result = impl::epa( a, b, simplex );
+	if ( !impl::generateClippingManifold( a, b, result, manifold ) ) return false;
+
+	return true;
+}
+
+bool impl::generateContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
+	bool useGjk = uf::physics::settings.useGjk;
+	if ( a.collider.type == pod::ShapeType::MESH || b.collider.type == pod::ShapeType::MESH ) useGjk = false;
+	//if ( a.collider.type == pod::ShapeType::PLANE || b.collider.type == pod::ShapeType::PLANE ) useGjk = false;
+	if ( useGjk ) return generateContactsGjk( a, b, manifold, dt );
+	impl::bindManifold( a, b, manifold, dt );
+
+#define CHECK_CONTACT( A, B, fun )\
+	if ( a.collider.type == pod::ShapeType::A && b.collider.type == pod::ShapeType::B ) return fun( a, b, manifold );
+
+	CHECK_CONTACT( AABB, AABB, impl::aabbAabb );
+	CHECK_CONTACT( AABB, OBB, impl::aabbObb );
+	CHECK_CONTACT( AABB, SPHERE, impl::aabbSphere );
+	CHECK_CONTACT( AABB, PLANE, impl::aabbPlane );
+	CHECK_CONTACT( AABB, CAPSULE, impl::aabbCapsule );
+	CHECK_CONTACT( AABB, MESH, impl::aabbMesh );
+	CHECK_CONTACT( AABB, CONVEX_HULL, impl::aabbHull );
+
+	CHECK_CONTACT( OBB, AABB, impl::obbAabb );
+	CHECK_CONTACT( OBB, OBB, impl::obbObb );
+	CHECK_CONTACT( OBB, SPHERE, impl::obbSphere );
+	CHECK_CONTACT( OBB, PLANE, impl::obbPlane );
+	CHECK_CONTACT( OBB, CAPSULE, impl::obbCapsule );
+	CHECK_CONTACT( OBB, MESH, impl::obbMesh );
+	CHECK_CONTACT( OBB, CONVEX_HULL, impl::obbHull );
+
+	CHECK_CONTACT( SPHERE, AABB, impl::sphereAabb );
+	CHECK_CONTACT( SPHERE, OBB, impl::sphereObb );
+	CHECK_CONTACT( SPHERE, SPHERE, impl::sphereSphere );
+	CHECK_CONTACT( SPHERE, PLANE, impl::spherePlane );
+	CHECK_CONTACT( SPHERE, CAPSULE, impl::sphereCapsule );
+	CHECK_CONTACT( SPHERE, MESH, impl::sphereMesh );
+	CHECK_CONTACT( SPHERE, CONVEX_HULL, impl::sphereHull );
+
+	CHECK_CONTACT( PLANE, AABB, impl::planeAabb );
+	CHECK_CONTACT( PLANE, OBB, impl::planeObb );
+	CHECK_CONTACT( PLANE, SPHERE, impl::planeSphere );
+	CHECK_CONTACT( PLANE, PLANE, impl::planePlane );
+	CHECK_CONTACT( PLANE, CAPSULE, impl::planeCapsule );
+	CHECK_CONTACT( PLANE, MESH, impl::planeMesh );
+	CHECK_CONTACT( PLANE, CONVEX_HULL, impl::planeHull );
+
+	CHECK_CONTACT( CAPSULE, AABB, impl::capsuleAabb );
+	CHECK_CONTACT( CAPSULE, OBB, impl::capsuleObb );
+	CHECK_CONTACT( CAPSULE, SPHERE, impl::capsuleSphere );
+	CHECK_CONTACT( CAPSULE, PLANE, impl::capsulePlane );
+	CHECK_CONTACT( CAPSULE, CAPSULE, impl::capsuleCapsule );
+	CHECK_CONTACT( CAPSULE, MESH, impl::capsuleMesh );
+	CHECK_CONTACT( CAPSULE, CONVEX_HULL, impl::capsuleHull );
+
+	CHECK_CONTACT( MESH, AABB, impl::meshAabb );
+	CHECK_CONTACT( MESH, OBB, impl::meshObb );
+	CHECK_CONTACT( MESH, SPHERE, impl::meshSphere );
+	CHECK_CONTACT( MESH, PLANE, impl::meshPlane );
+	CHECK_CONTACT( MESH, CAPSULE, impl::meshCapsule );
+	CHECK_CONTACT( MESH, MESH, impl::meshMesh );
+	CHECK_CONTACT( MESH, CONVEX_HULL, impl::meshHull );
+
+	CHECK_CONTACT( CONVEX_HULL, AABB, impl::hullAabb );
+	CHECK_CONTACT( CONVEX_HULL, OBB, impl::hullObb );
+	CHECK_CONTACT( CONVEX_HULL, SPHERE, impl::hullSphere );
+	CHECK_CONTACT( CONVEX_HULL, PLANE, impl::hullPlane );
+	CHECK_CONTACT( CONVEX_HULL, CAPSULE, impl::hullCapsule );
+	CHECK_CONTACT( CONVEX_HULL, MESH, impl::hullMesh );
+	CHECK_CONTACT( CONVEX_HULL, CONVEX_HULL, impl::hullHull );
+
+	UF_EXCEPTION("unregistered contact: {} vs {}", (int) a.collider.type, (int) b.collider.type );
+	
+	return false;
+}
+
+void impl::computeLocalContacts( pod::Manifold& manifold ) {
+	if ( manifold.points.empty() ) return;
+
+	auto& a = *manifold.a;
+	auto& b = *manifold.b;
+
+	auto tA = impl::getTransform( a );
+	auto tB = impl::getTransform( b );
+
+	for ( auto& c : manifold.points ) {
+		c.localA = uf::transform::applyInverse( tA, c.point - c.normal * (c.penetration * 0.5f) );
+		c.localB = uf::transform::applyInverse( tB, c.point + c.normal * (c.penetration * 0.5f) );
+	}
+}
+
+bool impl::similarContact( const pod::Contact& a, const pod::Contact& b, float distSqThreshold, float normThreshold ) {
+	return uf::vector::distanceSquared(a.point, b.point) < distSqThreshold && uf::vector::dot(a.normal, b.normal) > normThreshold;
+}
+
+void impl::reduceContacts( pod::Manifold& manifold ) {
+	if ( manifold.points.size() <= 4 ) return;
+
+#if 1
+	int idx0 = 0, idx1 = 0, idx2 = 0, idx3 = 0;
+
+	// deepest
+	float maxPenetration = -FLT_MAX;
+	for ( int i = 0; i < manifold.points.size(); ++i ) {
+		if ( manifold.points[i].penetration > maxPenetration ) {
+			maxPenetration = manifold.points[i].penetration;
+			idx0 = i;
+		}
+	}
+
+	// furthest
+	float maxDistSq = -1.0f;
+	auto p0 = manifold.points[idx0].point;
+	for ( int i = 0; i < manifold.points.size(); ++i ) {
+		if ( i == idx0 ) continue;
+		float distSq = uf::vector::distanceSquared( p0, manifold.points[i].point );
+		if ( distSq > maxDistSq ) {
+			maxDistSq = distSq;
+			idx1 = i;
+		}
+	}
+
+	// max area
+	float maxAreaSq = -1.0f;
+	auto p1 = manifold.points[idx1].point;
+	auto edge0 = p1 - p0;
+	for ( int i = 0; i < manifold.points.size(); ++i ) {
+		if ( i == idx0 || i == idx1 ) continue;
+		auto edge1 = manifold.points[i].point - p0;
+		auto crossVec = uf::vector::cross( edge0, edge1 );
+		float areaSq = uf::vector::dot( crossVec, crossVec );
+		if ( areaSq > maxAreaSq ) {
+			maxAreaSq = areaSq;
+			idx2 = i;
+		}
+	}
+
+	// largest convex quad
+	float maxDistToCenterSq = -1.0f;
+	auto p2 = manifold.points[idx2].point;
+	auto center = (p0 + p1 + p2) / 3.0f;
+	for ( int i = 0; i < manifold.points.size(); ++i ) {
+		if ( i == idx0 || i == idx1 || i == idx2 ) continue;
+		float distSq = uf::vector::distanceSquared( center, manifold.points[i].point );
+		if ( distSq > maxDistToCenterSq ) {
+			maxDistToCenterSq = distSq;
+			idx3 = i;
+		}
+	}
+
+	// rebuild
+	pod::Manifold reducedManifold = manifold;
+	reducedManifold.points.clear();
+	reducedManifold.points.reserve( 4 );
+
+	reducedManifold.points.emplace_back( manifold.points[idx0] );
+	reducedManifold.points.emplace_back( manifold.points[idx1] );
+	reducedManifold.points.emplace_back( manifold.points[idx2] );
+	reducedManifold.points.emplace_back( manifold.points[idx3] );
+
+	manifold.points = std::move( reducedManifold.points );
+#else
+	STATIC_THREAD_LOCAL(uf::stl::vector<pod::Contact>, result);
+	result.reserve(4);
+
+	for ( auto& c : manifold.points ) {
+		if ( !uf::vector::isValid(c.point) ) continue;
+
+		bool merged = false;
+		for ( auto& r : result ) {
+			if ( !impl::similarContact(c, r) ) continue;
+			if ( c.penetration > r.penetration ) r = c;
+			merged = true;
+			break;
+		}
+		if ( !merged ) {
+			if ( result.size() < 4 ) {
+				result.emplace_back(c);
+			} else {
+				auto weakest = 0;
+				for ( auto i = 1; i < 4; i++ ) {
+					if ( result[i].penetration < result[weakest].penetration ) weakest = i;
+				}
+				if ( c.penetration > result[weakest].penetration ) result[weakest] = c;
+			}
+		}
+	}
+
+	manifold.points = result;
+#endif
+}
+
+void impl::mergeContacts( pod::Manifold& manifold ) {
+	STATIC_THREAD_LOCAL(uf::stl::vector<pod::Contact>, result);
+	result.reserve(4);
+
+	for ( auto& c : manifold.points ) {
+		bool merged = false;
+		for ( auto& r : result ) {
+			if ( !impl::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 );
+	}
+	
+	manifold.points = result;
+}
+
+void impl::retrieveContacts( pod::Manifold& current, const pod::Manifold& previous, float distanceThreshold, float separationThreshold, float decay ) {
+	auto& a = *current.a;
+	auto& b = *current.b;
+
+	auto tA = impl::getTransform( a );
+	auto tB = impl::getTransform( b );
+
+	uf::stl::vector<pod::Contact> merged = current.points;
+
+	float distSqThresh = distanceThreshold * distanceThreshold;
+	for ( const auto& oldContact : previous.points ) {
+		// reproject point according to current transform
+		auto worldA = uf::transform::apply( tA, oldContact.localA );
+		auto worldB = uf::transform::apply( tB, oldContact.localB );
+
+		auto delta = worldB - worldA;
+		auto normal = current.points.empty() ? oldContact.normal : current.points[0].normal;
+		float penetration = -uf::vector::dot( delta, normal );
+		if ( penetration < -separationThreshold ) continue;
+
+		pod::Vector3f projectedDelta = delta + normal * penetration;
+		float tangentialDriftSq = uf::vector::dot( projectedDelta, projectedDelta );
+		if ( tangentialDriftSq > distSqThresh ) continue;
+
+		pod::Contact validContact = oldContact;
+		validContact.point = (worldA + worldB) * 0.5f;
+		validContact.normal = normal;
+		validContact.penetration = penetration;
+		++validContact.lifetime;
+
+		validContact.accumulatedNormalImpulse *= decay;
+		validContact.accumulatedTangentImpulse *= decay;
+
+		bool isDuplicate = false;
+		for ( auto& c : merged ) {
+			if ( impl::similarContact( validContact, c ) ) {
+				c.accumulatedNormalImpulse = validContact.accumulatedNormalImpulse;
+				c.accumulatedTangentImpulse = validContact.accumulatedTangentImpulse;
+				c.lifetime = validContact.lifetime;
+				isDuplicate = true;
+				break;
+			}
+		}
+
+		if ( !isDuplicate ) merged.emplace_back( validContact );
+	}
+
+	current.points = merged;
+}
+
+void impl::prepareManifoldCache( uf::stl::unordered_map<size_t, pod::Manifold>& cache, const uf::stl::vector<pod::Island>& islands, const uf::stl::vector<pod::PhysicsBody*>& bodies ) {
+	for ( const auto& island : islands ) {
+		for ( const auto& pair : island.pairs ) {
+			auto& a = *bodies[pair.first];
+			auto& b = *bodies[pair.second];
+
+			cache[ impl::makePairKey( a, b ) ];
+		}
+	}
+}
+
+void impl::updateManifoldCache( const uf::stl::vector<pod::Manifold>& manifolds, uf::stl::unordered_map<size_t, pod::Manifold>& cache ) {
+	for ( const auto& m : manifolds ) {
+		auto it = cache.find( impl::makePairKey( *m.a, *m.b ) );
+		if ( it == cache.end() ) continue; // assert
+		it->second = m;
+	}
+}
+
+void impl::pruneManifoldCache( uf::stl::unordered_map<size_t, pod::Manifold>& cache ) {
+	auto cacheLifetime = uf::physics::settings.manifoldCacheLifetime;
+	if ( !cacheLifetime ) {
+		cacheLifetime = MAX(1, uf::physics::settings.substeps) * 2;
+	}
+	for ( auto itCache = cache.begin(); itCache != cache.end(); ) {
+		auto& manifold = itCache->second;
+
+		// prune points that are too old
+		for ( auto it = manifold.points.begin(); it != manifold.points.end(); ) {
+			if ( it->lifetime > cacheLifetime ) it = manifold.points.erase(it);
+			else ++it;
+		}
+
+		// empty manifold, kill it
+		if ( manifold.points.empty() ) itCache = cache.erase(itCache);
+		else ++itCache;
+	}
+}
+
+void impl::warmupContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& c, float dt ) {
+	if ( !c.lifetime ) return; // too new
+
+	// build relative offsets
+	pod::Vector3f rA = c.point - impl::getPosition( a );
+	pod::Vector3f rB = c.point - impl::getPosition( b );
+
+	// normal impulse
+	pod::Vector3f Pn = c.normal * c.accumulatedNormalImpulse;
+	impl::applyImpulseTo( a, b, rA, rB, Pn );
+
+	// tangent basis
+	pod::Vector3f Pt = c.tangent * c.accumulatedTangentImpulse;
+	impl::applyImpulseTo( a, b, rA, rB, Pt );
+}
+void impl::warmupManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Manifold& manifold, float dt ) {
+	for ( auto& contact : manifold.points ) {
+		impl::warmupContacts( a, b, contact, dt );
+	}
+}
+
+void impl::resolveManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
+	if ( uf::physics::settings.blockContactSolver ) {
+		if ( impl::blockSolver( a, b, manifold, dt ) ) return;
+	}
+	for ( auto& contact : manifold.points ) impl::iterativeImpulseSolver( a, b, contact, dt );
+}
+
+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 );
+}

engine/src/utils/math/physics/constraints/distance.cpp

@@ -0,0 +1,40 @@
+#include <uf/utils/math/physics/impl.h>
+#include <uf/utils/math/physics/common.h>
+#include <uf/utils/math/physics/integration.h>
+#include <uf/utils/math/physics/constraints.h>
+
+void impl::solveDistanceConstraint( pod::Constraint& constraint, float dt ) {
+	auto& a = *constraint.a;
+	auto& b = *constraint.b;
+	auto& joint = constraint.distance;
+
+	auto tA = impl::getTransform( a );
+	auto tB = impl::getTransform( b );
+
+	auto rA = uf::quaternion::rotate( tA.orientation, joint.localAnchorA );
+	auto rB = uf::quaternion::rotate( tB.orientation, joint.localAnchorB );
+
+	auto pA = tA.position + rA;
+	auto pB = tB.position + rB;
+
+	auto delta = pB - pA;
+	float currentDistance2 = uf::vector::magnitude( delta );
+	if ( currentDistance2 < EPS2 ) return;
+	float currentDistance = std::sqrt( currentDistance2 );
+
+	pod::Vector3f normal = delta / currentDistance;
+	float distanceError = currentDistance - joint.targetDistance;
+
+	auto vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);
+	auto vB = b.velocity + uf::vector::cross(b.angularVelocity, rB);
+	float relVelAlongNormal = uf::vector::dot( vB - vA, normal );
+
+	float invMassN = impl::computeEffectiveMass( a, b, rA, rB, normal );
+	if ( invMassN < EPS ) return;
+
+	float bias = (uf::physics::settings.baumgarteCorrectionPercent / dt) * distanceError;
+	if ( joint.isRope && currentDistance <= joint.targetDistance ) bias = 0;
+	float j = -(relVelAlongNormal + bias) / invMassN;
+
+	impl::applyImpulseTo( a, b, rA, rB, normal, j, joint.accumulatedImpulse, -FLT_MAX, joint.isRope ? 0 : FLT_MAX );
+}

engine/src/utils/math/physics/constraints/hinge.cpp

@@ -0,0 +1,84 @@
+#include <uf/utils/math/physics/impl.h>
+#include <uf/utils/math/physics/common.h>
+#include <uf/utils/math/physics/integration.h>
+#include <uf/utils/math/physics/constraints.h>
+
+void impl::solveHingeConstraint( pod::Constraint& constraint, float dt ) {
+	auto& a = *constraint.a;
+	auto& b = *constraint.b;
+	auto& joint = constraint.hinge;
+	auto& motor = constraint.motor;
+
+	auto tA = impl::getTransform( a );
+	auto tB = impl::getTransform( b );
+
+	// solve linearly first
+	impl::solveBallSocketConstraint( constraint, dt );
+
+	auto ctxA = impl::solverBodyContext( a );
+	auto ctxB = impl::solverBodyContext( b );
+
+	auto waA = uf::quaternion::rotate( tA.orientation, joint.localAxisA );
+	auto waB = uf::quaternion::rotate( tB.orientation, joint.localAxisB );
+	auto angularError = uf::vector::cross( waA, waB );
+
+	pod::Vector3f tangents[2];
+	tangents[0] = impl::computeTangent( waA );
+	tangents[1] = uf::vector::cross( waA, tangents[0] );
+
+	auto relAngularVel = b.angularVelocity - a.angularVelocity;
+
+	pod::Matrix2f K = {};
+	for ( auto i = 0; i < 2; ++i ) {
+		for ( auto j = 0; j < 2; ++j ) {
+			K(i, j) = impl::computeAngularMassMatrixLine( ctxA, ctxB, tangents[i], tangents[j]);
+		}
+		K(i,i) += uf::physics::settings.jointCFM * ( 1.0f + ctxA.invM + ctxB.invM );
+	}
+
+	pod::Matrix2f Kinv = uf::matrix::inverse(K);
+	pod::Vector2f rhs = {};
+	FOR_EACH( 2, {
+		rhs[i] = -(uf::vector::dot(relAngularVel, tangents[i]) + (uf::physics::settings.baumgarteCorrectionPercent / dt) * uf::vector::dot(angularError, tangents[i]));
+	});
+
+	pod::Vector2f impulse = uf::matrix::multiply(Kinv, rhs);
+
+	for ( auto i = 0; i < 2; ++i ) {
+		auto jDelta = impl::accumulateImpulseTo( impulse[i], joint.accumulatedAngularImpulse[i] );
+		auto j = tangents[i] * jDelta;
+		if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply( ctxA.invI, j );
+		if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply( ctxB.invI, j );
+	}
+
+	if ( motor.enabled ) {
+		impl::solve1DAngularMotor( a, b, waA, motor.targetVelocity, motor.maxMotorTorque, motor.accumulatedMotorImpulse, dt );
+	}
+}
+
+pod::Constraint& uf::physics::constrain( pod::World& world, pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& joint, const pod::Vector3f& axis ) {
+	auto& constraint = uf::physics::constrain( world, a, b );
+	constraint.type = pod::ConstraintType::HINGE;
+	// transform joint into local space 
+	auto tA = impl::getTransform( a );
+	auto tB = impl::getTransform( b );
+	auto normAxis = uf::vector::normalize( axis );
+
+	constraint.hinge.localAnchorA = uf::transform::applyInverse( tA, joint );
+	constraint.hinge.localAnchorB = uf::transform::applyInverse( tB, joint );
+	constraint.hinge.accumulatedImpulse = {};
+	constraint.hinge.localAxisA = uf::quaternion::rotate( uf::quaternion::inverse(tA.orientation), normAxis );
+	constraint.hinge.localAxisB = uf::quaternion::rotate( uf::quaternion::inverse(tB.orientation), normAxis );
+	constraint.hinge.accumulatedAngularImpulse = {};
+
+	return constraint;
+}
+pod::Constraint& uf::physics::constrain( pod::World& world, uf::Object& a, uf::Object& b, const pod::Vector3f& joint, const pod::Vector3f& axis ) {
+	return constrain( world, a.getComponent<pod::PhysicsBody>(), b.getComponent<pod::PhysicsBody>(), joint, axis );
+}
+pod::Constraint& uf::physics::constrain( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& joint, const pod::Vector3f& axis ) {
+	return constrain( uf::physics::getWorld(), a, b, joint, axis );
+}
+pod::Constraint& uf::physics::constrain( uf::Object& a, uf::Object& b, const pod::Vector3f& joint, const pod::Vector3f& axis ) {
+	return constrain( uf::physics::getWorld(), a.getComponent<pod::PhysicsBody>(), b.getComponent<pod::PhysicsBody>(), joint, axis );
+}

engine/src/utils/math/physics/constraints/motor.cpp

@@ -0,0 +1,45 @@
+#include <uf/utils/math/physics/impl.h>
+#include <uf/utils/math/physics/common.h>
+#include <uf/utils/math/physics/integration.h>
+#include <uf/utils/math/physics/constraints.h>
+
+void impl::solve1DLinearMotor(
+	pod::PhysicsBody& a, pod::PhysicsBody& b,
+	const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& axis,
+	float targetVelocity, float maxForce, float& accumulatedImpulse, float dt
+) {
+	auto vA = (a.velocity + uf::vector::cross(a.angularVelocity, rA));
+	auto vB = (b.velocity + uf::vector::cross(b.angularVelocity, rB));
+	auto relVel = vB - vA;
+	float currentSpeed = uf::vector::dot(relVel, axis);
+	float invMassN = impl::computeEffectiveMass( a, b, rA, rB, axis );
+
+	UF_ASSERT( invMassN > EPS );
+
+	float j = (targetVelocity - currentSpeed) / invMassN;
+	float maxImpulse = maxForce * dt;
+	impl::applyImpulseTo(a, b, rA, rB, axis, j, accumulatedImpulse, -maxImpulse, maxImpulse);
+}
+
+void impl::solve1DAngularMotor(
+	pod::PhysicsBody& a, pod::PhysicsBody& b,
+	const pod::Vector3f& axis,
+	float targetVelocity, float maxTorque,
+	float& accumulatedImpulse, float dt
+) {
+	auto relAngularVel = b.angularVelocity - a.angularVelocity;
+	float currentSpeed = uf::vector::dot(relAngularVel, axis);
+	
+	auto ctxA = impl::solverBodyContext( a );
+	auto ctxB = impl::solverBodyContext( b );
+	float invMassN = impl::computeAngularMassMatrixLine( ctxA, ctxB, axis ,axis );
+	UF_ASSERT( invMassN > EPS );
+
+	float j = (targetVelocity - currentSpeed) / invMassN;
+	float maxImpulse = maxTorque * dt;
+	auto jDelta = impl::accumulateImpulseTo( j, accumulatedImpulse, -maxImpulse, maxImpulse  );
+
+	pod::Vector3f impulse = axis * jDelta;
+	if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply( ctxA.invI, impulse );
+	if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply( ctxB.invI, impulse );
+}

engine/src/utils/math/physics/constraints/slider.cpp

@@ -0,0 +1,104 @@
+#include <uf/utils/math/physics/impl.h>
+#include <uf/utils/math/physics/common.h>
+#include <uf/utils/math/physics/integration.h>
+#include <uf/utils/math/physics/constraints.h>
+
+void impl::solveSliderConstraint( pod::Constraint& constraint, float dt ) {
+	auto& a = *constraint.a;
+	auto& b = *constraint.b;
+	auto& joint = constraint.slider;
+	auto& motor = constraint.motor;
+
+	auto tA = impl::getTransform( a );
+	auto tB = impl::getTransform( b );
+
+	auto rA = uf::quaternion::rotate( tA.orientation, joint.localAnchorA );
+	auto rB = uf::quaternion::rotate( tB.orientation, joint.localAnchorB );
+
+	auto pA = tA.position + rA;
+	auto pB = tB.position + rB;
+
+	auto waA = uf::quaternion::rotate( tA.orientation, joint.localAxisA );
+	auto waB = uf::quaternion::rotate( tB.orientation, joint.localAxisB );
+	
+	auto wrA = uf::quaternion::rotate( tA.orientation, joint.localReferenceAxisA );
+	auto wrB = uf::quaternion::rotate( tB.orientation, joint.localReferenceAxisB );
+
+	auto ctxA = impl::solverBodyContext( a );
+	auto ctxB = impl::solverBodyContext( b );
+
+	auto relAngularVel = b.angularVelocity - a.angularVelocity;
+
+	auto vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);
+	auto vB = b.velocity + uf::vector::cross(b.angularVelocity, rB);
+	
+	auto relVel = vB - vA;
+	auto angularError = uf::vector::cross(waA, waB) + uf::vector::cross(wrA, wrB);
+	auto positionError = pB - pA;
+
+	{
+		pod::Matrix3f K = {};
+		pod::Vector3f axes[3] = { {1,0,0}, {0,1,0}, {0,0,1} };
+		for ( auto i = 0; i < 3; ++i ) {
+			for ( auto j = 0; j < 3; ++j ) {
+				K(i, j) = impl::computeAngularMassMatrixLine( ctxA, ctxB, axes[i], axes[j]);
+			}
+			K(i,i) += uf::physics::settings.jointCFM * ( 1.0f + ctxA.invM + ctxB.invM );
+		}
+
+		pod::Matrix3f Kinv = uf::matrix::inverse( K );
+		pod::Vector3f bias = angularError * (uf::physics::settings.baumgarteCorrectionPercent / dt);
+		pod::Vector3f rhs = -(relAngularVel + bias);
+
+		pod::Vector3f impulse = uf::matrix::multiply( Kinv, rhs );
+		joint.accumulatedAngularImpulse += impulse;
+
+		if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply( ctxA.invI, impulse );
+		if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply( ctxB.invI, impulse );
+	}
+	{
+		pod::Vector3f t1 = wrA;
+		pod::Vector3f t2 = uf::vector::cross( waA, wrA );
+
+		pod::Vector3f axes[2] = { t1, t2 };
+		pod::Matrix2f K = {};
+		for ( auto i = 0; i < 2; ++i ) {
+			auto rowI = pod::JacobianRow{ rA, rB, axes[i] };
+			for ( auto j = 0; j < 2; ++j ) {
+				auto rowJ = pod::JacobianRow{ rA, rB, axes[j] };
+				K(i, j) = impl::computeMassMatrixLine( ctxA, ctxB, rowI, rowJ );
+			}
+		}
+
+		pod::Matrix2f Kinv = uf::matrix::inverse( K );
+		pod::Vector2f rhs = {
+			-(uf::vector::dot(relVel, t1) + (uf::vector::dot(positionError, t1) * (uf::physics::settings.baumgarteCorrectionPercent / dt))),
+			-(uf::vector::dot(relVel, t2) + (uf::vector::dot(positionError, t2) * (uf::physics::settings.baumgarteCorrectionPercent / dt)))
+		};
+
+		pod::Vector2f impulse = uf::matrix::multiply( Kinv, rhs );
+		joint.accumulatedLinearImpulse += impulse;
+
+		for( int i = 0; i < 2; i++ ) {
+			impl::applyImpulseTo( a, b, rA, rB, axes[i], impulse[i], joint.accumulatedLinearImpulse[i] );
+		}
+	}
+
+	if ( motor.enabled ) {
+		impl::solve1DLinearMotor( a, b, rA, rB, waA, motor.targetVelocity, motor.maxMotorForce, motor.accumulatedMotorImpulse, dt );
+	}
+
+	float currentDist = uf::vector::dot( positionError, waA );
+	if ( currentDist < joint.lowerLimit || currentDist > joint.upperLimit ) {
+		float invMassN = impl::computeEffectiveMass( a, b, rA, rB, waA );
+		UF_ASSERT( invMassN > EPS );
+
+		float limitError = (currentDist < joint.lowerLimit) ? (currentDist - joint.lowerLimit) : (currentDist - joint.upperLimit);
+		float bias = (uf::physics::settings.baumgarteCorrectionPercent / dt) * limitError;
+		float j = -(uf::vector::dot(relVel, waA) + bias) / invMassN;
+
+		float min = ( currentDist < joint.lowerLimit ) ? 0.0f : -FLT_MAX;
+		float max = ( currentDist < joint.lowerLimit ) ? FLT_MAX : 0.0f;
+		impl::applyImpulseTo( a, b, rA, rB, waA, j, joint.accumulatedLimitImpulse, min, max );
+	}
+}

engine/src/utils/math/physics/constraints/spring.cpp

@@ -0,0 +1,44 @@
+#include <uf/utils/math/physics/impl.h>
+#include <uf/utils/math/physics/common.h>
+#include <uf/utils/math/physics/integration.h>
+#include <uf/utils/math/physics/constraints.h>
+
+void impl::solveSpringConstraint( pod::Constraint& constraint, float dt ) {
+	auto& a = *constraint.a;
+	auto& b = *constraint.b;
+	auto& joint = constraint.spring;
+
+	auto tA = impl::getTransform( a );
+	auto tB = impl::getTransform( b );
+
+	auto rA = uf::quaternion::rotate( tA.orientation, joint.localAnchorA );
+	auto rB = uf::quaternion::rotate( tB.orientation, joint.localAnchorB );
+
+	auto worldAnchorA = tA.position + rA;
+	auto worldAnchorB = tB.position + rB;
+	auto delta = worldAnchorB - worldAnchorA;
+
+	float currentDistance2 = uf::vector::magnitude( delta );
+	if ( currentDistance2 < EPS ) return;
+	float currentDistance = std::sqrt( currentDistance2 );
+
+	pod::Vector3f normal = delta / currentDistance;
+	float distanceError = currentDistance - joint.restLength;
+
+	auto vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);
+	auto vB = b.velocity + uf::vector::cross(b.angularVelocity, rB);
+	float relVelAlongNormal = uf::vector::dot( vB - vA, normal );
+
+	float invMassN = impl::computeEffectiveMass( a, b, rA, rB, normal );
+	if ( invMassN < EPS ) return;
+
+	float gamma = joint.damping + (dt * joint.stiffness);
+    gamma = (gamma > EPS) ? (1.0f / (dt * gamma)) : 0.0f;
+
+	float beta = dt * joint.stiffness * gamma;
+
+	float bias = distanceError * (beta / dt);
+    float j = -(relVelAlongNormal + bias + (gamma * joint.accumulatedImpulse)) / (invMassN + gamma);
+
+	impl::applyImpulseTo( a, b, rA, rB, normal, j, joint.accumulatedImpulse );
+}

engine/src/utils/math/physics/constraints/weld.cpp

@@ -0,0 +1,47 @@
+#include <uf/utils/math/physics/impl.h>
+#include <uf/utils/math/physics/common.h>
+#include <uf/utils/math/physics/integration.h>
+#include <uf/utils/math/physics/constraints.h>
+
+void impl::solveWeldConstraint( pod::Constraint& constraint, float dt ) {
+	auto& a = *constraint.a;
+	auto& b = *constraint.b;
+	auto& joint = constraint.weld;
+
+	// solve linearly
+	impl::solveBallSocketConstraint( constraint, dt );
+
+	// solve angularly
+	auto tA = impl::getTransform( a );
+	auto tB = impl::getTransform( b );
+
+	auto ctxA = impl::solverBodyContext( a );
+	auto ctxB = impl::solverBodyContext( b );
+
+	auto waA = uf::quaternion::rotate( tA.orientation, joint.localAxisA );
+	auto waB = uf::quaternion::rotate( tB.orientation, joint.localAxisB );
+
+	auto wrA = uf::quaternion::rotate( tA.orientation, joint.localReferenceAxisA );
+	auto wrB = uf::quaternion::rotate( tB.orientation, joint.localReferenceAxisB );
+
+	auto relAngularVel = b.angularVelocity - a.angularVelocity;
+	auto angularError = uf::vector::cross(waA, waB) + uf::vector::cross(wrA, wrB);
+
+	pod::Matrix3f K = {};
+	pod::Vector3f axes[3] = { {1,0,0}, {0,1,0}, {0,0,1} };
+	for ( auto i = 0; i < 3; ++i ) {
+		for ( auto j = 0; j < 3; ++j ) {
+			K(i, j) = impl::computeAngularMassMatrixLine( ctxA, ctxB, axes[i], axes[j]);
+		}
+		K(i,i) += uf::physics::settings.jointCFM * ( 1.0f + ctxA.invM + ctxB.invM );
+	}
+
+	auto bias = angularError * (uf::physics::settings.baumgarteCorrectionPercent / dt);
+	auto rhs = -(relAngularVel + bias);
+	auto impulse = uf::matrix::multiply( uf::matrix::inverse(K), rhs );
+	
+	joint.accumulatedAngularImpulse += impulse;
+
+	if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply( ctxA.invI, impulse );
+	if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply( ctxB.invI, impulse );
+}

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

@@ -1,11 +1,13 @@
 #include <uf/utils/math/physics/impl.h>
-#include <uf/engine/scene/scene.h>
 
 #include <uf/utils/math/physics/common.h>
 #include <uf/utils/math/physics/narrowphase.h>
 #include <uf/utils/math/physics/broadphase.h>
 #include <uf/utils/math/physics/integration.h>
 #include <uf/utils/math/physics/solvers.h>
+#include <uf/utils/math/physics/constraints.h>
+
+#include <uf/engine/scene/scene.h>
 
 #define UF_PHYSICS_TEST 0
 
@@ -85,6 +87,7 @@ void uf::physics::substep( pod::World& world, float dt, int32_t substeps ) {
 }
 void uf::physics::step( pod::World& world, float dt ) {
 	auto& bodies = world.bodies;
+	auto& constraints = world.constraints;
 	auto& dynamicBvh = world.dynamicBvh;
 	auto& staticBvh = world.staticBvh;
 
@@ -123,7 +126,7 @@ void uf::physics::step( pod::World& world, float dt ) {
 
 	// build islands from overlaps
 	STATIC_THREAD_LOCAL(uf::stl::vector<pod::Island>, islands);
-	impl::buildIslands( pairs, bodies, islands );
+	impl::buildIslands( pairs, bodies, constraints, islands );
 
 	if ( uf::physics::settings.warmupSolver ) impl::prepareManifoldCache( uf::physics::settings.manifoldsCache, islands, bodies );
 
@@ -132,6 +135,7 @@ void uf::physics::step( pod::World& world, float dt ) {
 	auto tasks = uf::thread::schedule(true);
 	for ( auto& island : islands ) tasks.queue([&]{
 		auto& manifolds = island.manifolds;
+		auto& constraints = island.constraints;
 		manifolds.clear();
 
 		// sleeping island, skip (asleep islands shouldn't ever be in here)
@@ -150,10 +154,10 @@ void uf::physics::step( pod::World& world, float dt ) {
 			impl::computeLocalContacts( manifold );
 
 			// bodies with meshes already reorient the normal to the triangle's center
-			// do not do it for meshes because it'll reorient to the mesh's origin
-			// do not do it for planes
 			bool shouldReorient = true;
+			// do not do it for meshes because it'll reorient to the mesh's origin
 			if ( a.collider.type == pod::ShapeType::MESH || b.collider.type == pod::ShapeType::MESH ) shouldReorient = false;
+			// do not do it for planes
 			//if ( a.collider.type == pod::ShapeType::PLANE || b.collider.type == pod::ShapeType::PLANE ) shouldReorient = false;
 			if ( shouldReorient ) {
 				for ( auto& c : manifold.points ) c.normal = impl::orientNormalToAB( a, b, c.normal );
@@ -191,6 +195,12 @@ void uf::physics::step( pod::World& world, float dt ) {
 		impl::solveContacts( manifolds, dt );
 		// do position correction
 		impl::solvePositions( manifolds, dt );
+		// solve constraints
+		for ( uint32_t i = 0; i < uf::physics::settings.solverIterations; ++i ) {
+			for ( auto& constraint : constraints ) {
+				impl::solveConstraint( *constraint, dt );
+			}
+		}
 		// cache manifold positions
 		if ( uf::physics::settings.warmupSolver ) {
 			impl::updateManifoldCache( manifolds, uf::physics::settings.manifoldsCache );
@@ -201,11 +211,7 @@ void uf::physics::step( pod::World& world, float dt ) {
 	if ( uf::physics::settings.warmupSolver ) impl::pruneManifoldCache( uf::physics::settings.manifoldsCache );
 
 	if ( uf::physics::settings.debugDraw ) {
-		for ( auto& island : islands ) {
-			for ( auto& manifold : island.manifolds ) {
-				impl::drawManifold( manifold );
-			}
-		}
+		for ( auto& island : islands ) for ( auto& manifold : island.manifolds ) impl::drawManifold( manifold );
 	}
 
 	for ( auto* b : bodies ) {
@@ -411,7 +417,6 @@ void uf::physics::setAngularVelocity( pod::PhysicsBody& body, const pod::Quatern
 
 	impl::wakeBody( body );
 	body.angularVelocity = axis * ( angle / dt );
-	UF_MSG_DEBUG("axis={}, angle={}, dt={}", uf::vector::toString( axis ), angle, dt );
 }
 void uf::physics::applyAngularVelocity( pod::PhysicsBody& body, const pod::Vector3f& v ) {
 	impl::wakeBody( body );
@@ -431,7 +436,6 @@ void uf::physics::applyAngularVelocity( pod::PhysicsBody& body, const pod::Quate
 
 	impl::wakeBody( body );
 	body.angularVelocity += axis * ( angle / dt );
-	UF_MSG_DEBUG("axis={}, angle={}, dt={}", uf::vector::toString( axis ), angle, dt );
 }
 void uf::physics::applyRotation( pod::PhysicsBody& body, const pod::Quaternion<>& q ) {
 	impl::wakeBody( body );
@@ -440,6 +444,10 @@ void uf::physics::applyRotation( pod::PhysicsBody& body, const pod::Quaternion<>
 void uf::physics::applyRotation( pod::PhysicsBody& body, const pod::Vector3f& axis, float angle ) {
 	uf::physics::applyRotation( body, uf::quaternion::axisAngle( axis, angle ) );
 }
+pod::World& uf::physics::getWorld() {
+	auto& scene = uf::scene::getCurrentScene();
+	return scene.getComponent<pod::World>();
+}
 
 // body creation
 pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, float mass, const pod::Vector3f& offset ) {
@@ -468,121 +476,14 @@ pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, fl
 
 	return body;
 }
-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.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;
-	body.collider.sphere = sphere;
-	body.bounds = impl::computeAABB( body );
-	uf::physics::updateInertia( body );
-	return body;
-}
-pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, const pod::Plane& plane, float mass, const pod::Vector3f& offset ) {
-	auto& body = uf::physics::create( world, object, mass, offset );
-	body.collider.type = pod::ShapeType::PLANE;
-	body.collider.plane = plane;
-	body.bounds = impl::computeAABB( body );
-	uf::physics::updateInertia( body );
-	return body;
-}
-pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, const pod::Capsule& capsule, float mass, const pod::Vector3f& offset ) {
-	auto& body = uf::physics::create( world, object, mass, offset );
-	body.collider.type = pod::ShapeType::CAPSULE;
-	body.collider.capsule = capsule;
-	body.bounds = impl::computeAABB( body );
-	uf::physics::updateInertia( body );
-	return body;
-}
-pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, const pod::TriangleWithNormal& tri, float mass, const pod::Vector3f& offset ) {
-	auto& body = uf::physics::create( world, object, mass, offset );
-	body.collider.type = pod::ShapeType::TRIANGLE;
-	body.collider.triangle = tri;
-	if ( uf::vector::magnitude( body.collider.triangle.normal ) < 0.001f ) {
-		body.collider.triangle.normal = impl::triangleNormal( (const pod::Triangle&) tri );
-	}
-	body.bounds = impl::computeAABB( body );
-	uf::physics::updateInertia( body );
-	return body;
-}
-pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, const uf::Mesh& mesh, float mass, const pod::Vector3f& offset, bool convex ) {
-	auto& body = uf::physics::create( world, object, mass, offset );
-	if ( !convex ) {
-		body.collider.type = pod::ShapeType::MESH;
-		body.collider.mesh.mesh = &mesh;
-		body.collider.mesh.bvh = new pod::BVH;
-		
-		auto& bvh = *body.collider.mesh.bvh;
-		impl::buildMeshBVH( bvh, mesh, uf::physics::settings.meshBvhCapacity );
-	} else {
-		body.collider.type = pod::ShapeType::CONVEX_HULL;
-		body.collider.convexHull.mesh = &mesh;
-		body.collider.convexHull.bvh = new pod::BVH;
-		
-		auto& bvh = *body.collider.convexHull.bvh;
-		impl::buildConvexHullBVH( bvh, mesh/*, uf::physics::settings.meshBvhCapacity*/ );
-	}
-
-	body.bounds = impl::computeAABB( body );
-	uf::physics::updateInertia( body );
-	return body;
-}
 
 pod::PhysicsBody& uf::physics::create( uf::Object& object, float mass, const pod::Vector3f& offset ) {
 	// bind to scene
 	// auto& root = object.getRootParent<>(); // in the event a scene is being initialized that is not the root scene, use the root parent instead
 	// auto& world = root.getComponent<pod::World>();
-	auto& scene = uf::scene::getCurrentScene();
-	auto& world = scene.getComponent<pod::World>();
-	return create( world, object, mass, offset );
+	return create( getWorld(), object, mass, offset );
 	
 }
-pod::PhysicsBody& uf::physics::create( uf::Object& object, const pod::AABB& aabb, float mass, const pod::Vector3f& offset ) {
-	auto& scene = uf::scene::getCurrentScene();
-	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>();
-	return create( world, object, sphere, mass, offset );
-}
-pod::PhysicsBody& uf::physics::create( uf::Object& object, const pod::Plane& plane, float mass, const pod::Vector3f& offset ) {
-	auto& scene = uf::scene::getCurrentScene();
-	auto& world = scene.getComponent<pod::World>();
-	return create( world, object, plane, mass, offset );
-}
-pod::PhysicsBody& uf::physics::create( uf::Object& object, const pod::Capsule& capsule, float mass, const pod::Vector3f& offset ) {
-	auto& scene = uf::scene::getCurrentScene();
-	auto& world = scene.getComponent<pod::World>();
-	return create( world, object, capsule, mass, offset );
-}
-pod::PhysicsBody& uf::physics::create( uf::Object& object, const uf::Mesh& mesh, float mass, const pod::Vector3f& offset, bool convex ) {
-	auto& scene = uf::scene::getCurrentScene();
-	auto& world = scene.getComponent<pod::World>();
-	return create( world, object, mesh, mass, offset, convex );
-}
 
 void uf::physics::destroy( uf::Object& object ) {
 	if ( !object.hasComponent<pod::PhysicsBody>() ) return;
@@ -591,6 +492,9 @@ void uf::physics::destroy( uf::Object& object ) {
 }
 void uf::physics::destroy( pod::PhysicsBody& body ) {
 	auto& world = *body.world;
+
+	uf::physics::unconstrain( body );
+
 	// remove from world
 	for ( auto it = world.bodies.begin(); it != world.bodies.end(); ++it ) {
 		if ( (*it)->object != body.object ) continue;
@@ -607,44 +511,40 @@ void uf::physics::destroy( pod::PhysicsBody& body ) {
 	}
 }
 
-pod::RayQuery uf::physics::rayCast( const pod::Ray& ray, const pod::PhysicsBody& body, float maxDistance ) {
-	return rayCast( ray, *body.world, &body, maxDistance );
+pod::Constraint& uf::physics::constrain( pod::World& world, pod::PhysicsBody& a, pod::PhysicsBody& b ) {
+	// allocate constraint struct (pointer cringe because the vector WILL resize)
+	auto* pointer = world.constraints.emplace_back(new pod::Constraint);
+	auto& constraint = *pointer;
+	constraint.a = &a;
+	constraint.b = &b;
+	return constraint;
 }
-pod::RayQuery uf::physics::rayCast( const pod::Ray& ray, const pod::World& world, float maxDistance ) {
-	return rayCast( ray, world, NULL, maxDistance );
+pod::Constraint& uf::physics::constrain( pod::World& world, uf::Object& a, uf::Object& b ) {
+	return constrain( world, a.getComponent<pod::PhysicsBody>(), b.getComponent<pod::PhysicsBody>() );
+}
+pod::Constraint& uf::physics::constrain( pod::PhysicsBody& a, pod::PhysicsBody& b ) {
+	return constrain( getWorld(), a, b );
+}
+pod::Constraint& uf::physics::constrain( uf::Object& a, uf::Object& b ) {
+	return constrain( getWorld(), a.getComponent<pod::PhysicsBody>(), b.getComponent<pod::PhysicsBody>() );
 }
-pod::RayQuery uf::physics::rayCast( const pod::Ray& ray, const pod::World& world, const pod::PhysicsBody* body, float maxDistance ) {
-	pod::RayQuery rayHit;
-	rayHit.invoker = body;
-	rayHit.contact.penetration = maxDistance;
 
-	auto& dynamicBvh = world.dynamicBvh;
-	auto& staticBvh = world.staticBvh;
-	auto& bodies = world.bodies;
-
-	STATIC_THREAD_LOCAL(uf::stl::vector<pod::BVH::index_t>, candidates);
-	impl::queryBVH( dynamicBvh, ray, candidates );
-	if ( uf::physics::settings.useSplitBvhs ) impl::queryBVH( staticBvh, ray, candidates );
-
-	for ( auto i : candidates ) {
-		auto* b = bodies[i];
-		
-		if ( body == b ) continue;
-
-		switch ( b->collider.type ) {
-			case pod::ShapeType::AABB: impl::rayAabb( ray, *b, rayHit ); break;
-			case pod::ShapeType::OBB: impl::rayObb( ray, *b, rayHit ); break;
-			case pod::ShapeType::SPHERE: impl::raySphere( ray, *b, rayHit ); break;
-			case pod::ShapeType::PLANE: impl::rayPlane( ray, *b, rayHit ); break;
-			case pod::ShapeType::CAPSULE: impl::rayCapsule( ray, *b, rayHit ); break;
-			case pod::ShapeType::MESH: impl::rayMesh( ray, *b, rayHit ); break;
-			case pod::ShapeType::CONVEX_HULL: impl::rayHull( ray, *b, rayHit ); break;
+void uf::physics::unconstrain( pod::PhysicsBody& body ) {
+	auto& world = *body.world;
+	auto& constraints = world.constraints;
+	// remove all constraints that reference this body
+	for ( auto it = constraints.begin(); it != constraints.end(); ) {
+		auto* constraint = *it;
+		if ( constraint->a == &body || constraint->b == &body ) {
+			it = constraints.erase(it);
+			delete constraint;
+		} else {
+			++it;
 		}
 	}
-	
-	if ( uf::physics::settings.debugDraw ) impl::drawRay( ray, rayHit );
-
-	return rayHit;
+}
+void uf::physics::unconstrain( uf::Object& object ) {
+	return unconstrain( object.getComponent<pod::PhysicsBody>() );
 }
 
 #if UF_PHYSICS_TEST

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

@@ -1,30 +1,62 @@
+#include <uf/utils/math/physics/impl.h>
 #include <uf/utils/math/physics/common.h>
 #include <uf/utils/math/physics/integration.h>
 #include <uf/utils/math/physics/narrowphase.h>
 
+pod::SolverBodyContext impl::solverBodyContext( const pod::PhysicsBody& body ) {
+	return { body.isStatic ? 0.0f : body.inverseMass, impl::computeWorldInverseInertia( body ) };
+}
+
+float impl::computeEffectiveMass( const pod::SolverBodyContext& a, const pod::SolverBodyContext& b, const pod::JacobianRow& row ) {
+	float inverseMass = a.invM + b.invM;
+	float angularTerm = 0.0f;
+
+	if ( a.invM > 0.0f ) {
+		auto crossA = uf::vector::cross(row.rA, row.n);
+		auto I_crossA = uf::matrix::multiply(a.invI, crossA);
+		angularTerm += uf::vector::dot(uf::vector::cross(I_crossA, row.rA), row.n);
+	}
+	if ( b.invM > 0.0f ) {
+		auto crossB = uf::vector::cross(row.rB, row.n);
+		auto I_crossB = uf::matrix::multiply(b.invI, crossB);
+		angularTerm += uf::vector::dot(uf::vector::cross(I_crossB, row.rB), row.n);
+	}
+
+	return inverseMass + angularTerm;
+}
+// to-do: convert below to just use the above
 float impl::computeEffectiveMass( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& n ) {
-	float inverseMass = 0.0f;
-	if ( !a.isStatic ) inverseMass += a.inverseMass;
-	if ( !b.isStatic ) inverseMass += b.inverseMass;
+	auto ctxA = impl::solverBodyContext( a );
+	auto ctxB = impl::solverBodyContext( b );
+	auto row = pod::JacobianRow{ rA, rB, n };
+	return impl::computeEffectiveMass( ctxA, ctxB, row );
+}
 
-	float angularTermA = 0.0f;
-	float angularTermB = 0.0f;
+float impl::computeMassMatrixLine( const pod::SolverBodyContext& a, const pod::SolverBodyContext& b, const pod::JacobianRow& rowI, const pod::JacobianRow& rowJ ) {
+	float termLinear = (a.invM + b.invM) * uf::vector::dot(rowI.n, rowJ.n);
+	float angularTerm = 0.0f;
 
-	if ( !a.isStatic ) {
-		auto invIa = impl::computeWorldInverseInertia(a);
-		auto crossA = uf::vector::cross(rA, n);
-		auto I_crossA = uf::matrix::multiply(invIa, crossA);
-		angularTermA = uf::vector::dot(uf::vector::cross(I_crossA, rA), n);
-	}
-	if ( !b.isStatic ) {
-		auto invIb = impl::computeWorldInverseInertia(b);
-		auto crossB = uf::vector::cross(rB, n);
-		auto I_crossB = uf::matrix::multiply(invIb, crossB);
-		angularTermB = uf::vector::dot(uf::vector::cross(I_crossB, rB), n);
+	if ( a.invM > 0.0f ) {
+		pod::Vector3f raXnj = uf::vector::cross(rowJ.rA, rowJ.n);
+		pod::Vector3f Ia_raXnj = uf::matrix::multiply( a.invI, raXnj );
+		pod::Vector3f crossA = uf::vector::cross(Ia_raXnj, rowI.rA);
+		angularTerm += uf::vector::dot(rowI.n, crossA);
 	}
 
-	// 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)
-	return inverseMass + angularTermA + angularTermB;
+	if ( b.invM > 0.0f ) {
+		pod::Vector3f rbXnj = uf::vector::cross(rowJ.rB, rowJ.n);
+		pod::Vector3f Ib_rbXnj = uf::matrix::multiply( b.invI, rbXnj );
+		pod::Vector3f crossB = uf::vector::cross(Ib_rbXnj, rowI.rB);
+		angularTerm += uf::vector::dot(rowI.n, crossB);
+	}
+
+	return termLinear + angularTerm;
+}
+float impl::computeAngularMassMatrixLine( const pod::SolverBodyContext& a, const pod::SolverBodyContext& b, const pod::Vector3f& n_i, const pod::Vector3f& n_j ) {
+	float kVal = 0.0f;
+	if ( a.invM > 0.0f ) kVal += uf::vector::dot(n_i, uf::matrix::multiply(a.invI, n_j));
+	if ( b.invM > 0.0f ) kVal += uf::vector::dot(n_i, uf::matrix::multiply(b.invI, n_j));
+	return kVal;
 }
 
 void impl::applyImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& impulse ) {
@@ -52,6 +84,76 @@ void impl::applyPseudoImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const
 	}
 }
 
+
+// accumulates then applies the change in impulse given an impulse direction and magnitude
+void impl::applyImpulseTo(
+	pod::PhysicsBody& a, pod::PhysicsBody& b,
+	const pod::Vector3f& rA, const pod::Vector3f& rB,
+	const pod::Vector3f& direction, float magnitude,
+	float& accumulatedImpulse,
+	float minLimit, float maxLimit
+) {
+	auto j = accumulateImpulseTo( magnitude, accumulatedImpulse, minLimit, maxLimit );
+	impl::applyImpulseTo( a, b, rA, rB, direction * j );
+}
+
+// accumulates then applies the change in impulse given an impulse vector
+void impl::applyImpulseTo(
+	pod::PhysicsBody& a, pod::PhysicsBody& b,
+	const pod::Vector3f& rA, const pod::Vector3f& rB,
+	const pod::Vector3f& impulse, pod::Vector3f& accumulatedImpulse,
+	float minLimit, float maxLimit
+) {
+	auto j = accumulateImpulseTo( impulse, accumulatedImpulse, minLimit, maxLimit );
+	impl::applyImpulseTo( a, b, rA, rB, j );
+}
+
+void impl::applyPseudoImpulseTo(
+	pod::PhysicsBody& a, pod::PhysicsBody& b,
+	const pod::Vector3f& rA, const pod::Vector3f& rB,
+	const pod::Vector3f& direction, float magnitude,
+	float& accumulatedImpulse,
+	float minLimit, float maxLimit
+) {
+	auto j = accumulateImpulseTo( magnitude, accumulatedImpulse, minLimit, maxLimit );
+	impl::applyPseudoImpulseTo( a, b, rA, rB, direction * j );
+}
+
+void impl::applyPseudoImpulseTo(
+	pod::PhysicsBody& a, pod::PhysicsBody& b,
+	const pod::Vector3f& rA, const pod::Vector3f& rB,
+	const pod::Vector3f& impulse, pod::Vector3f& accumulatedImpulse,
+	float minLimit, float maxLimit
+) {
+	auto j = accumulateImpulseTo( impulse, accumulatedImpulse, minLimit, maxLimit );
+	impl::applyPseudoImpulseTo( a, b, rA, rB, j );
+}
+
+// accumulates an scalar impulse
+float impl::accumulateImpulseTo(
+	float magnitude, float& accumulatedImpulse,
+	float minLimit, float maxLimit
+) {
+	auto jOld = accumulatedImpulse;
+	auto jNew = std::clamp( jOld + magnitude, minLimit, maxLimit );
+	auto jDelta = jNew - jOld;
+	accumulatedImpulse = jNew;
+
+	return jDelta;
+}
+// accumulates an impulse vector
+pod::Vector3f impl::accumulateImpulseTo(
+	const pod::Vector3f& impulse, pod::Vector3f& accumulatedImpulse,
+	float minLimit, float maxLimit
+) {
+	auto jOld = accumulatedImpulse;
+	auto jNew = uf::vector::clamp( jOld + impulse, minLimit, maxLimit );
+	auto jDelta = jNew - jOld;
+	accumulatedImpulse = jNew;
+
+	return jDelta;
+}
+
 void impl::applyRollingResistance( pod::PhysicsBody& body, float dt ) {
 	if ( body.isStatic ) return;
 
@@ -62,383 +164,23 @@ void impl::applyRollingResistance( pod::PhysicsBody& body, float dt ) {
 	body.angularVelocity *= std::max(0.0f, 1.0f - rollingFriction * dt);
 }
 
-void impl::bindManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
-	manifold.a = &a;
-	manifold.b = &b;
-	manifold.dt = dt;
-	manifold.points.clear();
-}
-
-bool impl::generateContactsGjk( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
-	impl::bindManifold( a, b, manifold, dt );
-
-	pod::Simplex simplex;
-
-	if ( !impl::gjk(a,b,simplex) ) return false;
-	auto result = impl::epa( a, b, simplex );
-	if ( !impl::generateClippingManifold( a, b, result, manifold ) ) return false;
-
-	return true;
-}
-
-bool impl::generateContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
-	bool useGjk = uf::physics::settings.useGjk;
-	if ( a.collider.type == pod::ShapeType::MESH || b.collider.type == pod::ShapeType::MESH ) useGjk = false;
-	//if ( a.collider.type == pod::ShapeType::PLANE || b.collider.type == pod::ShapeType::PLANE ) useGjk = false;
-	if ( useGjk ) return generateContactsGjk( a, b, manifold, dt );
-	impl::bindManifold( a, b, manifold, dt );
-
-#define CHECK_CONTACT( A, B, fun )\
-	if ( a.collider.type == pod::ShapeType::A && b.collider.type == pod::ShapeType::B ) return fun( a, b, manifold );
-
-	CHECK_CONTACT( AABB, AABB, impl::aabbAabb );
-	CHECK_CONTACT( AABB, OBB, impl::aabbObb );
-	CHECK_CONTACT( AABB, SPHERE, impl::aabbSphere );
-	CHECK_CONTACT( AABB, PLANE, impl::aabbPlane );
-	CHECK_CONTACT( AABB, CAPSULE, impl::aabbCapsule );
-	CHECK_CONTACT( AABB, MESH, impl::aabbMesh );
-	CHECK_CONTACT( AABB, CONVEX_HULL, impl::aabbHull );
-
-	CHECK_CONTACT( OBB, AABB, impl::obbAabb );
-	CHECK_CONTACT( OBB, OBB, impl::obbObb );
-	CHECK_CONTACT( OBB, SPHERE, impl::obbSphere );
-	CHECK_CONTACT( OBB, PLANE, impl::obbPlane );
-	CHECK_CONTACT( OBB, CAPSULE, impl::obbCapsule );
-	CHECK_CONTACT( OBB, MESH, impl::obbMesh );
-	CHECK_CONTACT( OBB, CONVEX_HULL, impl::obbHull );
-
-	CHECK_CONTACT( SPHERE, AABB, impl::sphereAabb );
-	CHECK_CONTACT( SPHERE, OBB, impl::sphereObb );
-	CHECK_CONTACT( SPHERE, SPHERE, impl::sphereSphere );
-	CHECK_CONTACT( SPHERE, PLANE, impl::spherePlane );
-	CHECK_CONTACT( SPHERE, CAPSULE, impl::sphereCapsule );
-	CHECK_CONTACT( SPHERE, MESH, impl::sphereMesh );
-	CHECK_CONTACT( SPHERE, CONVEX_HULL, impl::sphereHull );
-
-	CHECK_CONTACT( PLANE, AABB, impl::planeAabb );
-	CHECK_CONTACT( PLANE, OBB, impl::planeObb );
-	CHECK_CONTACT( PLANE, SPHERE, impl::planeSphere );
-	CHECK_CONTACT( PLANE, PLANE, impl::planePlane );
-	CHECK_CONTACT( PLANE, CAPSULE, impl::planeCapsule );
-	CHECK_CONTACT( PLANE, MESH, impl::planeMesh );
-	CHECK_CONTACT( PLANE, CONVEX_HULL, impl::planeHull );
-
-	CHECK_CONTACT( CAPSULE, AABB, impl::capsuleAabb );
-	CHECK_CONTACT( CAPSULE, OBB, impl::capsuleObb );
-	CHECK_CONTACT( CAPSULE, SPHERE, impl::capsuleSphere );
-	CHECK_CONTACT( CAPSULE, PLANE, impl::capsulePlane );
-	CHECK_CONTACT( CAPSULE, CAPSULE, impl::capsuleCapsule );
-	CHECK_CONTACT( CAPSULE, MESH, impl::capsuleMesh );
-	CHECK_CONTACT( CAPSULE, CONVEX_HULL, impl::capsuleHull );
-
-	CHECK_CONTACT( MESH, AABB, impl::meshAabb );
-	CHECK_CONTACT( MESH, OBB, impl::meshObb );
-	CHECK_CONTACT( MESH, SPHERE, impl::meshSphere );
-	CHECK_CONTACT( MESH, PLANE, impl::meshPlane );
-	CHECK_CONTACT( MESH, CAPSULE, impl::meshCapsule );
-	CHECK_CONTACT( MESH, MESH, impl::meshMesh );
-	CHECK_CONTACT( MESH, CONVEX_HULL, impl::meshHull );
-
-	CHECK_CONTACT( CONVEX_HULL, AABB, impl::hullAabb );
-	CHECK_CONTACT( CONVEX_HULL, OBB, impl::hullObb );
-	CHECK_CONTACT( CONVEX_HULL, SPHERE, impl::hullSphere );
-	CHECK_CONTACT( CONVEX_HULL, PLANE, impl::hullPlane );
-	CHECK_CONTACT( CONVEX_HULL, CAPSULE, impl::hullCapsule );
-	CHECK_CONTACT( CONVEX_HULL, MESH, impl::hullMesh );
-	CHECK_CONTACT( CONVEX_HULL, CONVEX_HULL, impl::hullHull );
-
-	UF_EXCEPTION("unregistered contact: {} vs {}", (int) a.collider.type, (int) b.collider.type );
-	
-	return false;
-}
-
-void impl::computeLocalContacts( pod::Manifold& manifold ) {
-	if ( manifold.points.empty() ) return;
-
-	auto& a = *manifold.a;
-	auto& b = *manifold.b;
-
-	auto tA = impl::getTransform( a );
-	auto tB = impl::getTransform( b );
-
-	for ( auto& c : manifold.points ) {
-		c.localA = uf::transform::applyInverse( tA, c.point - c.normal * (c.penetration * 0.5f) );
-		c.localB = uf::transform::applyInverse( tB, c.point + c.normal * (c.penetration * 0.5f) );
-	}
-}
-
-bool impl::similarContact( const pod::Contact& a, const pod::Contact& b, float distSqThreshold, float normThreshold ) {
-	return uf::vector::distanceSquared(a.point, b.point) < distSqThreshold && uf::vector::dot(a.normal, b.normal) > normThreshold;
-}
-
-void impl::reduceContacts( pod::Manifold& manifold ) {
-	if ( manifold.points.size() <= 4 ) return;
-
-#if 1
-	int idx0 = 0, idx1 = 0, idx2 = 0, idx3 = 0;
-
-	// deepest
-	float maxPenetration = -FLT_MAX;
-	for ( int i = 0; i < manifold.points.size(); ++i ) {
-		if ( manifold.points[i].penetration > maxPenetration ) {
-			maxPenetration = manifold.points[i].penetration;
-			idx0 = i;
-		}
-	}
-
-	// furthest
-	float maxDistSq = -1.0f;
-	auto p0 = manifold.points[idx0].point;
-	for ( int i = 0; i < manifold.points.size(); ++i ) {
-		if ( i == idx0 ) continue;
-		float distSq = uf::vector::distanceSquared( p0, manifold.points[i].point );
-		if ( distSq > maxDistSq ) {
-			maxDistSq = distSq;
-			idx1 = i;
-		}
-	}
-
-	// max area
-	float maxAreaSq = -1.0f;
-	auto p1 = manifold.points[idx1].point;
-	auto edge0 = p1 - p0;
-	for ( int i = 0; i < manifold.points.size(); ++i ) {
-		if ( i == idx0 || i == idx1 ) continue;
-		auto edge1 = manifold.points[i].point - p0;
-		auto crossVec = uf::vector::cross( edge0, edge1 );
-		float areaSq = uf::vector::dot( crossVec, crossVec );
-		if ( areaSq > maxAreaSq ) {
-			maxAreaSq = areaSq;
-			idx2 = i;
-		}
-	}
-
-	// largest convex quad
-	float maxDistToCenterSq = -1.0f;
-	auto p2 = manifold.points[idx2].point;
-	auto center = (p0 + p1 + p2) / 3.0f;
-	for ( int i = 0; i < manifold.points.size(); ++i ) {
-		if ( i == idx0 || i == idx1 || i == idx2 ) continue;
-		float distSq = uf::vector::distanceSquared( center, manifold.points[i].point );
-		if ( distSq > maxDistToCenterSq ) {
-			maxDistToCenterSq = distSq;
-			idx3 = i;
-		}
-	}
-
-	// rebuild
-	pod::Manifold reducedManifold = manifold;
-	reducedManifold.points.clear();
-	reducedManifold.points.reserve( 4 );
-
-	reducedManifold.points.emplace_back( manifold.points[idx0] );
-	reducedManifold.points.emplace_back( manifold.points[idx1] );
-	reducedManifold.points.emplace_back( manifold.points[idx2] );
-	reducedManifold.points.emplace_back( manifold.points[idx3] );
-
-	manifold.points = std::move( reducedManifold.points );
-#else
-	STATIC_THREAD_LOCAL(uf::stl::vector<pod::Contact>, result);
-	result.reserve(4);
-
-	for ( auto& c : manifold.points ) {
-		if ( !uf::vector::isValid(c.point) ) continue;
-
-		bool merged = false;
-		for ( auto& r : result ) {
-			if ( !impl::similarContact(c, r) ) continue;
-			if ( c.penetration > r.penetration ) r = c;
-			merged = true;
-			break;
-		}
-		if ( !merged ) {
-			if ( result.size() < 4 ) {
-				result.emplace_back(c);
-			} else {
-				auto weakest = 0;
-				for ( auto i = 1; i < 4; i++ ) {
-					if ( result[i].penetration < result[weakest].penetration ) weakest = i;
-				}
-				if ( c.penetration > result[weakest].penetration ) result[weakest] = c;
-			}
-		}
-	}
-
-	manifold.points = result;
-#endif
-}
-
-void impl::mergeContacts( pod::Manifold& manifold ) {
-	STATIC_THREAD_LOCAL(uf::stl::vector<pod::Contact>, result);
-	result.reserve(4);
-
-	for ( auto& c : manifold.points ) {
-		bool merged = false;
-		for ( auto& r : result ) {
-			if ( !impl::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 );
-	}
-	
-	manifold.points = result;
-}
-
-void impl::retrieveContacts( pod::Manifold& current, const pod::Manifold& previous, float distanceThreshold, float separationThreshold, float decay ) {
-	auto& a = *current.a;
-	auto& b = *current.b;
-
-	auto tA = impl::getTransform( a );
-	auto tB = impl::getTransform( b );
-
-	uf::stl::vector<pod::Contact> merged = current.points;
-
-	float distSqThresh = distanceThreshold * distanceThreshold;
-	for ( const auto& oldContact : previous.points ) {
-		// reproject point according to current transform
-		auto worldA = uf::transform::apply( tA, oldContact.localA );
-		auto worldB = uf::transform::apply( tB, oldContact.localB );
-
-		auto delta = worldB - worldA;
-		auto normal = current.points.empty() ? oldContact.normal : current.points[0].normal;
-		float penetration = -uf::vector::dot( delta, normal );
-		if ( penetration < -separationThreshold ) continue;
-
-		pod::Vector3f projectedDelta = delta + normal * penetration;
-		float tangentialDriftSq = uf::vector::dot( projectedDelta, projectedDelta );
-		if ( tangentialDriftSq > distSqThresh ) continue;
-
-		pod::Contact validContact = oldContact;
-		validContact.point = (worldA + worldB) * 0.5f;
-		validContact.normal = normal;
-		validContact.penetration = penetration;
-		++validContact.lifetime;
-
-		validContact.accumulatedNormalImpulse *= decay;
-		validContact.accumulatedTangentImpulse *= decay;
-
-		bool isDuplicate = false;
-		for ( auto& c : merged ) {
-			if ( impl::similarContact( validContact, c ) ) {
-				c.accumulatedNormalImpulse = validContact.accumulatedNormalImpulse;
-				c.accumulatedTangentImpulse = validContact.accumulatedTangentImpulse;
-				c.lifetime = validContact.lifetime;
-				isDuplicate = true;
-				break;
-			}
-		}
-
-		if ( !isDuplicate ) merged.emplace_back( validContact );
-	}
-
-	current.points = merged;
-}
-
-void impl::prepareManifoldCache( uf::stl::unordered_map<size_t, pod::Manifold>& cache, const uf::stl::vector<pod::Island>& islands, const uf::stl::vector<pod::PhysicsBody*>& bodies ) {
-	for ( const auto& island : islands ) {
-		for ( const auto& pair : island.pairs ) {
-			auto& a = *bodies[pair.first];
-			auto& b = *bodies[pair.second];
-
-			cache[ impl::makePairKey( a, b ) ];
-		}
-	}
-}
-
-void impl::updateManifoldCache( const uf::stl::vector<pod::Manifold>& manifolds, uf::stl::unordered_map<size_t, pod::Manifold>& cache ) {
-	for ( const auto& m : manifolds ) {
-		auto it = cache.find( impl::makePairKey( *m.a, *m.b ) );
-		if ( it == cache.end() ) continue; // assert
-		it->second = m;
-	}
-}
-
-void impl::pruneManifoldCache( uf::stl::unordered_map<size_t, pod::Manifold>& cache ) {
-	auto cacheLifetime = uf::physics::settings.manifoldCacheLifetime;
-	if ( !cacheLifetime ) {
-		cacheLifetime = MAX(1, uf::physics::settings.substeps) * 2;
-	}
-	for ( auto itCache = cache.begin(); itCache != cache.end(); ) {
-		auto& manifold = itCache->second;
-
-		// prune points that are too old
-		for ( auto it = manifold.points.begin(); it != manifold.points.end(); ) {
-			if ( it->lifetime > cacheLifetime ) it = manifold.points.erase(it);
-			else ++it;
-		}
-
-		// empty manifold, kill it
-		if ( manifold.points.empty() ) itCache = cache.erase(itCache);
-		else ++itCache;
-	}
-}
-
-void impl::warmupContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& c, float dt ) {
-	if ( !c.lifetime ) return; // too new
-
-	// build relative offsets
-	pod::Vector3f rA = c.point - impl::getPosition( a );
-	pod::Vector3f rB = c.point - impl::getPosition( b );
-
-	// normal impulse
-	pod::Vector3f Pn = c.normal * c.accumulatedNormalImpulse;
-	impl::applyImpulseTo( a, b, rA, rB, Pn );
-
-	// tangent basis
-	pod::Vector3f Pt = c.tangent * c.accumulatedTangentImpulse;
-	impl::applyImpulseTo( a, b, rA, rB, Pt );
-}
-void impl::warmupManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Manifold& manifold, float dt ) {
-	for ( auto& contact : manifold.points ) {
-		impl::warmupContacts( a, b, contact, dt );
-	}
-}
-
 // snap velocity for grounded bodies
 void impl::snapVelocity( pod::PhysicsBody& body, float dt, float threshold ) {
 	if ( !body.activity.grounded || !body.activity.awake ) return;
 
-	float thresholdSq = threshold * threshold;
+	float threshold2 = threshold * threshold;
 	// snap velocity if body is grounded and nearly still
-	float linSpeedSq = uf::vector::magnitude( body.velocity );
-	float angSpeedSq = uf::vector::magnitude( body.angularVelocity );
+	float linSpeed2 = uf::vector::magnitude( body.velocity );
+	float angSpeed2 = uf::vector::magnitude( body.angularVelocity );
 
 	// cancel out vertical component
-	if ( fabs(body.velocity.y) < thresholdSq ) body.velocity.y = 0.0f;
+	if ( fabs(body.velocity.y) < threshold2 ) body.velocity.y = 0.0f;
 	// cancel out velocity entirely
-	if ( linSpeedSq < thresholdSq ) body.velocity = {};
+	if ( linSpeed2 < threshold2 ) body.velocity = {};
 	// cancel out rotational velocity entirely
-	if ( angSpeedSq < thresholdSq ) body.angularVelocity = {};
+	if ( angSpeed2 < threshold2 ) body.angularVelocity = {};
 }
 
-// baumgarte position correction
-void impl::positionCorrection( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& contact ) {
-	if ( uf::physics::settings.baumgarteCorrectionPercent <= 0 ) return;
-	if ( a.isStatic && b.isStatic ) return;
-
-	// penetration depth beyond slop
-	float penetration = std::max( contact.penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f );
-	if ( penetration <= 0.0f ) return;
-
-	// compute correction magnitude
-	float invMassA = ( a.isStatic ? 0.0f : a.inverseMass );
-	float invMassB = ( b.isStatic ? 0.0f : b.inverseMass );
-	float totalInvMass = invMassA + invMassB;
-	if ( totalInvMass <= EPS ) return;
-
-	// apply correction vector
-	pod::Vector3f correction = contact.normal * (penetration / totalInvMass) * uf::physics::settings.baumgarteCorrectionPercent;
-	
-	if ( !a.isStatic ) a.transform->position -= correction * invMassA;
-	if ( !b.isStatic ) b.transform->position += correction * invMassB;
-}
-
-
 void impl::integrate( pod::PhysicsBody& body, float dt ) {
 	// only integrate awake and dynamic bodies
 	if ( !body.activity.awake || body.isStatic || body.mass == 0 ) return;

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

@@ -1,3 +1,4 @@
+#include <uf/utils/math/physics/impl.h>
 #include <uf/utils/math/physics/common.h>
 #include <uf/utils/math/physics/narrowphase.h>
 
@@ -96,4 +97,17 @@ void impl::drawAabb( const pod::PhysicsBody& body ) {
 	// vertical edges
 	impl::addLine( corners[0], corners[4] ); impl::addLine( corners[1], corners[5] );
 	impl::addLine( corners[2], corners[6] ); impl::addLine( corners[3], corners[7] );
+}
+
+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.aabb = aabb;
+	body.bounds = impl::computeAABB( body );
+
+	uf::physics::updateInertia( body );
+	return body;
+}
+pod::PhysicsBody& uf::physics::create( uf::Object& object, const pod::AABB& aabb, float mass, const pod::Vector3f& offset ) {
+	return create( uf::physics::getWorld(), object, aabb, mass, offset );
 }

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

@@ -1,3 +1,4 @@
+#include <uf/utils/math/physics/impl.h>
 #include <uf/utils/math/physics/common.h>
 #include <uf/utils/math/physics/narrowphase.h>
 
@@ -126,4 +127,17 @@ void impl::drawCapsule( const pod::PhysicsBody& body ) {
 	impl::addLine( p1 + forwardOffset, p1 - forwardOffset );
 	impl::addLine( p2 + rightOffset, p2 - rightOffset );
 	impl::addLine( p2 + forwardOffset, p2 - forwardOffset );
+}
+
+pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, const pod::Capsule& capsule, float mass, const pod::Vector3f& offset ) {
+	auto& body = uf::physics::create( world, object, mass, offset );
+	body.collider.type = pod::ShapeType::CAPSULE;
+	body.collider.capsule = capsule;
+	body.bounds = impl::computeAABB( body );
+	uf::physics::updateInertia( body );
+	return body;
+}
+
+pod::PhysicsBody& uf::physics::create( uf::Object& object, const pod::Capsule& capsule, float mass, const pod::Vector3f& offset ) {
+	return create( uf::physics::getWorld(), object, capsule, mass, offset );
 }

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

@@ -1,3 +1,4 @@
+#include <uf/utils/math/physics/impl.h>
 #include <uf/utils/math/physics/common.h>
 #include <uf/utils/math/physics/narrowphase.h>
 #include <uf/utils/math/physics/broadphase/bvh.h>
@@ -201,4 +202,30 @@ void impl::drawMesh( const pod::PhysicsBody& body ) {
 		impl::addLine( tri.points[1], tri.points[2] );
 		impl::addLine( tri.points[2], tri.points[0] );
 	}
+}
+
+pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, const uf::Mesh& mesh, float mass, const pod::Vector3f& offset, bool convex ) {
+	auto& body = uf::physics::create( world, object, mass, offset );
+	if ( !convex ) {
+		body.collider.type = pod::ShapeType::MESH;
+		body.collider.mesh.mesh = &mesh;
+		body.collider.mesh.bvh = new pod::BVH;
+		
+		auto& bvh = *body.collider.mesh.bvh;
+		impl::buildMeshBVH( bvh, mesh, uf::physics::settings.meshBvhCapacity );
+	} else {
+		body.collider.type = pod::ShapeType::CONVEX_HULL;
+		body.collider.convexHull.mesh = &mesh;
+		body.collider.convexHull.bvh = new pod::BVH;
+		
+		auto& bvh = *body.collider.convexHull.bvh;
+		impl::buildConvexHullBVH( bvh, mesh/*, uf::physics::settings.meshBvhCapacity*/ );
+	}
+
+	body.bounds = impl::computeAABB( body );
+	uf::physics::updateInertia( body );
+	return body;
+}
+pod::PhysicsBody& uf::physics::create( uf::Object& object, const uf::Mesh& mesh, float mass, const pod::Vector3f& offset, bool convex ) {
+	return create( uf::physics::getWorld(), object, mesh, mass, offset, convex );
 }

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

@@ -1,3 +1,4 @@
+#include <uf/utils/math/physics/impl.h>
 #include <uf/utils/math/physics/common.h>
 #include <uf/utils/math/physics/narrowphase.h>
 
@@ -256,15 +257,12 @@ bool impl::obbCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod
 	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 mag2 = uf::vector::magnitude(axis);
+		if ( mag2 < EPS2 ) return true;
+		pod::Vector3f n = axis / std::sqrt( mag2 );
 
 		float pA = uf::vector::dot(box.center, n);
-		float rA = box.extent.x * std::fabs(uf::vector::dot(axesA[0], n)) +
-				   box.extent.y * std::fabs(uf::vector::dot(axesA[1], n)) +
-				   box.extent.z * std::fabs(uf::vector::dot(axesA[2], n));
-
+		float rA = impl::projectExtents( box, n, axesA );
 		float pB = uf::vector::dot(cB, n);
 		float rB = halfHeight * std::fabs(uf::vector::dot(capAxis, n)) + radius;
 
@@ -323,4 +321,18 @@ void impl::drawObb( const pod::PhysicsBody& body ) {
 	// vertical edges
 	impl::addLine( corners[0], corners[4] ); impl::addLine( corners[1], corners[5] );
 	impl::addLine( corners[2], corners[6] ); impl::addLine( corners[3], corners[7] );
+}
+
+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( uf::Object& object, const pod::OBB& obb, float mass, const pod::Vector3f& offset ) {
+	return create( uf::physics::getWorld(), object, obb, mass, offset );
 }

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

@@ -1,3 +1,4 @@
+#include <uf/utils/math/physics/impl.h>
 #include <uf/utils/math/physics/common.h>
 #include <uf/utils/math/physics/narrowphase.h>
 
@@ -84,4 +85,17 @@ void impl::drawPlane( const pod::PhysicsBody& body ) {
 
 	impl::addLine( p0, p2 );
 	impl::addLine( p1, p3 );
+}
+
+pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, const pod::Plane& plane, float mass, const pod::Vector3f& offset ) {
+	auto& body = uf::physics::create( world, object, mass, offset );
+	body.collider.type = pod::ShapeType::PLANE;
+	body.collider.plane = plane;
+	body.bounds = impl::computeAABB( body );
+	uf::physics::updateInertia( body );
+	return body;
+}
+
+pod::PhysicsBody& uf::physics::create( uf::Object& object, const pod::Plane& plane, float mass, const pod::Vector3f& offset ) {
+	return create( uf::physics::getWorld(), object, plane, mass, offset );
 }

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

@@ -1,3 +1,4 @@
+#include <uf/utils/math/physics/impl.h>
 #include <uf/utils/math/physics/common.h>
 #include <uf/utils/math/physics/narrowphase.h>
 #include <uf/utils/math/physics/broadphase/bvh.h>
@@ -304,4 +305,44 @@ void impl::drawRay( const pod::Ray& ray, const pod::RayQuery& query ) {
 	auto  end = ray.origin + ray.direction * query.contact.penetration;
 
 	impl::addTransientLine( start, end, query.hit ? pod::Vector4f{ 0, 1, 0, 1 } : pod::Vector4f{ 1, 0, 0, 1 }, query.invoker, query.body );
+}
+
+pod::RayQuery uf::physics::rayCast( const pod::Ray& ray, const pod::PhysicsBody& body, float maxDistance ) {
+	return rayCast( ray, body.world ? *body.world : uf::physics::getWorld(), &body, maxDistance );
+}
+pod::RayQuery uf::physics::rayCast( const pod::Ray& ray, const pod::World& world, float maxDistance ) {
+	return rayCast( ray, world, NULL, maxDistance );
+}
+pod::RayQuery uf::physics::rayCast( const pod::Ray& ray, const pod::World& world, const pod::PhysicsBody* body, float maxDistance ) {
+	pod::RayQuery rayHit;
+	rayHit.invoker = body;
+	rayHit.contact.penetration = maxDistance;
+
+	auto& dynamicBvh = world.dynamicBvh;
+	auto& staticBvh = world.staticBvh;
+	auto& bodies = world.bodies;
+
+	STATIC_THREAD_LOCAL(uf::stl::vector<pod::BVH::index_t>, candidates);
+	impl::queryBVH( dynamicBvh, ray, candidates );
+	if ( uf::physics::settings.useSplitBvhs ) impl::queryBVH( staticBvh, ray, candidates );
+
+	for ( auto i : candidates ) {
+		auto* b = bodies[i];
+		
+		if ( body == b ) continue;
+
+		switch ( b->collider.type ) {
+			case pod::ShapeType::AABB: impl::rayAabb( ray, *b, rayHit ); break;
+			case pod::ShapeType::OBB: impl::rayObb( ray, *b, rayHit ); break;
+			case pod::ShapeType::SPHERE: impl::raySphere( ray, *b, rayHit ); break;
+			case pod::ShapeType::PLANE: impl::rayPlane( ray, *b, rayHit ); break;
+			case pod::ShapeType::CAPSULE: impl::rayCapsule( ray, *b, rayHit ); break;
+			case pod::ShapeType::MESH: impl::rayMesh( ray, *b, rayHit ); break;
+			case pod::ShapeType::CONVEX_HULL: impl::rayHull( ray, *b, rayHit ); break;
+		}
+	}
+	
+	if ( uf::physics::settings.debugDraw ) impl::drawRay( ray, rayHit );
+
+	return rayHit;
 }

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

@@ -1,3 +1,4 @@
+#include <uf/utils/math/physics/impl.h>
 #include <uf/utils/math/physics/common.h>
 #include <uf/utils/math/physics/narrowphase.h>
 
@@ -97,4 +98,17 @@ void impl::drawSphere( const pod::PhysicsBody& body ) {
 		impl::addLine( transform.position + xz1, transform.position + xz2 );
 		impl::addLine( transform.position + yz1, transform.position + yz2 );
 	}
+}
+
+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;
+	body.collider.sphere = sphere;
+	body.bounds = impl::computeAABB( body );
+	uf::physics::updateInertia( body );
+	return body;
+}
+
+pod::PhysicsBody& uf::physics::create( uf::Object& object, const pod::Sphere& sphere, float mass, const pod::Vector3f& offset ) {
+	return create( uf::physics::getWorld(), object, sphere, mass, offset );
 }

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

@@ -1,3 +1,4 @@
+#include <uf/utils/math/physics/impl.h>
 #include <uf/utils/math/physics/common.h>
 #include <uf/utils/math/physics/narrowphase.h>
 
@@ -94,7 +95,8 @@ bool impl::triangleTriangle( const pod::TriangleWithNormal& a, const pod::Triang
 		for ( auto j = 0; j < 3; j++ ) {
 			auto eb = b.points[(j+1)%3] - b.points[j];
 			auto axis = uf::vector::cross(ea, eb);
-			if ( uf::vector::magnitude( axis ) > EPS2 ) axes[axesCount++] = uf::vector::normalize(axis);
+			auto mag2 = uf::vector::magnitude( axis );
+			if ( mag2 > EPS2 ) axes[axesCount++] = axis / std::sqrt(mag2);
 		}
 	}
 
@@ -151,8 +153,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( edge, refNormal ) );
+		//auto edgeNormal = uf::vector::normalize( uf::vector::cross( refNormal, edge ) );
+		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;
 	}
@@ -364,4 +366,20 @@ void impl::drawTriangle( const pod::PhysicsBody& body ) {
 	impl::addLine( v0, v1 );
 	impl::addLine( v1, v2 );
 	impl::addLine( v2, v0 );
+}
+
+pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, const pod::TriangleWithNormal& tri, float mass, const pod::Vector3f& offset ) {
+	auto& body = uf::physics::create( world, object, mass, offset );
+	body.collider.type = pod::ShapeType::TRIANGLE;
+	body.collider.triangle = tri;
+	if ( uf::vector::magnitude( body.collider.triangle.normal ) < 0.001f ) {
+		body.collider.triangle.normal = impl::triangleNormal( (const pod::Triangle&) tri );
+	}
+	body.bounds = impl::computeAABB( body );
+	uf::physics::updateInertia( body );
+	return body;
+}
+
+pod::PhysicsBody& uf::physics::create( uf::Object& object, const pod::TriangleWithNormal& triangle, float mass, const pod::Vector3f& offset ) {
+	return create( uf::physics::getWorld(), object, triangle, mass, offset );
 }

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

@@ -1,3 +1,4 @@
+#include <uf/utils/math/physics/impl.h>
 #include <uf/utils/math/physics/common.h>
 #include <uf/utils/math/physics/integration.h>
 #include <uf/utils/math/physics/solvers/block.h>
@@ -6,43 +7,28 @@ namespace impl {
 	template<size_t N, typename T = float>
 	bool blockNxNSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
 		pod::Matrix<T,N> K = {};
-		
-		float invMassA = ( a.isStatic ? 0.0f : a.inverseMass );
-		float invMassB = ( b.isStatic ? 0.0f : b.inverseMass );
-		
-		pod::Matrix3f invIa = impl::computeWorldInverseInertia( a );
-		pod::Matrix3f invIb = impl::computeWorldInverseInertia( b );
+
+		auto ctxA = pod::SolverBodyContext{ a.isStatic ? 0.0f : a.inverseMass, impl::computeWorldInverseInertia(a) };
+		auto ctxB = pod::SolverBodyContext{ b.isStatic ? 0.0f : b.inverseMass, impl::computeWorldInverseInertia(b) };
 
 		auto pA = impl::getPosition( a, true );
 		auto pB = impl::getPosition( b, true );
 
+		auto gA = uf::physics::getGravity( a );
+		auto gB = uf::physics::getGravity( b );
+		float vSlop = std::sqrt( std::max( uf::vector::magnitude( gA ), uf::vector::magnitude( gB ) ) ) * dt;
+
 		for ( auto i = 0; i < N; i++ ) {
-			pod::Vector3f rA_i = manifold.points[i].point - pA;
-			pod::Vector3f rB_i = manifold.points[i].point - pB;
-			pod::Vector3f n_i = manifold.points[i].normal;
-			
+			auto& pI = manifold.points[i];
+			auto rowI = pod::JacobianRow{ pI.point - pA, pI.point - pB, pI.normal };
+
 			for ( auto j = 0; j < N; j++ ) {
-				pod::Vector3f rA_j = manifold.points[j].point - pA;
-				pod::Vector3f rB_j = manifold.points[j].point - pB;
-				pod::Vector3f n_j = manifold.points[j].normal;
-
-				float termLinear = (invMassA + invMassB) * uf::vector::dot(n_i, n_j);
-
-				pod::Vector3f raXnj = uf::vector::cross(rA_j, n_j);
-				pod::Vector3f rbXnj = uf::vector::cross(rB_j, n_j);
-
-				pod::Vector3f Ia_raXnj = uf::matrix::multiply( invIa, raXnj );
-				pod::Vector3f Ib_rbXnj = uf::matrix::multiply( invIb, 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;
+				auto& pJ = manifold.points[j];
+				auto rowJ = pod::JacobianRow{ pJ.point - pA, pJ.point - pB, pJ.normal };
+				K(i,j) = impl::computeMassMatrixLine( ctxA, ctxB, rowI, rowJ );
 			}
 
-			K(i,i) += 1e-3f;
+			K(i,i) += uf::physics::settings.contactCFM * ( 1.0f + ctxA.invM + ctxB.invM );
 		}
 
 		pod::Vector<T,N> rhs = {};
@@ -56,7 +42,7 @@ namespace impl {
 			float vRel = uf::vector::dot((vB - vA), contact.normal);
 
 			float e = std::min(a.material.restitution, b.material.restitution);
-			float restitutionBias = (vRel < -1.0f) ? -e * vRel : 0.0f;
+			float restitutionBias = (vRel < -vSlop) ? -e * vRel : 0.0f;
 
 			rhs[i] = -vRel + restitutionBias;
 			K_lambda[i] = contact.accumulatedNormalImpulse;
@@ -113,10 +99,8 @@ namespace impl {
 
 			// normal impulse
 			{
-				float newLambda = contact.accumulatedNormalImpulse + dLambda[i];
-				dLambda[i] = newLambda - contact.accumulatedNormalImpulse;
-				contact.accumulatedNormalImpulse = newLambda;
-				impl::applyImpulseTo( a, b, rA, rB, manifold.points[i].normal * dLambda[i] );
+				float jN = dLambda[i];
+				impl::applyImpulseTo( a, b, rA, rB, contact.normal, jN, contact.accumulatedNormalImpulse );
 			}
 			// pseudo impulse
 			if ( !uf::physics::settings.ngsPositionSolver ) {
@@ -128,14 +112,8 @@ namespace impl {
 				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);
+				float jP = (penetrationBias - pseudoVelAlongNormal) / invMassN;
+				impl::applyPseudoImpulseTo(a, b, rA, rB, contact.normal, jP, contact.accumulatedPseudoImpulse, 0 );
 			}
 			// tangent friction
 			{
@@ -146,8 +124,8 @@ namespace impl {
 				pod::Vector3f tangent = rv - contact.normal * uf::vector::dot(rv, contact.normal);
 				float tMag2 = uf::vector::magnitude(tangent);
 				if ( tMag2 > EPS2 ) {
-					contact.tangent = tangent / std::sqrt(tMag2);
-				} else if ( uf::vector::magnitude(contact.tangent) < EPS ) {
+					contact.tangent = tangent / std::sqrt( tMag2 );
+				} else if ( uf::vector::magnitude(contact.tangent) < EPS2 ) {
 					contact.tangent = impl::computeTangent( contact.normal );
 				}
 
@@ -167,14 +145,7 @@ namespace impl {
 				float mu = std::sqrt(a.material.dynamicFriction * b.material.dynamicFriction);
 				float maxFriction = mu * contact.accumulatedNormalImpulse;
 			*/
-
-				float jtOld = contact.accumulatedTangentImpulse;
-				float jtNew = std::clamp(jtOld + jt, -maxFriction, maxFriction);
-				float jtDelta = jtNew - jtOld;
-				contact.accumulatedTangentImpulse = jtNew;
-				jt = jtDelta;
-
-				impl::applyImpulseTo(a, b, rA, rB, contact.tangent * jt);
+				impl::applyImpulseTo( a, b, rA, rB, contact.tangent, jt, contact.accumulatedTangentImpulse, -maxFriction, maxFriction );
 			}
 		}
 

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

@@ -1,3 +1,4 @@
+#include <uf/utils/math/physics/impl.h>
 #include <uf/utils/math/physics/common.h>
 #include <uf/utils/math/physics/integration.h>
 #include <uf/utils/math/physics/solvers/iterativeImpulse.h>
@@ -8,27 +9,25 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod
 
 	float invMassN = impl::computeEffectiveMass(a, b, rA, rB, contact.normal);
 
+
 	// normal impulse
 	{
 		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;
 
+		auto gA = uf::physics::getGravity( a );
+		auto gB = uf::physics::getGravity( b );
+		float vSlop = std::sqrt( std::max( uf::vector::magnitude( gA ), uf::vector::magnitude( gB ) ) ) * dt;
+
 		float restitutionBias = 0.0f;
 		float e = std::min(a.material.restitution, b.material.restitution);
 		float velAlongNormal = uf::vector::dot(rv, contact.normal);
-		if ( velAlongNormal < -1.0f ) restitutionBias = -e * velAlongNormal;
+		if ( velAlongNormal < -vSlop ) restitutionBias = -e * velAlongNormal;
 		float targetVelocity = restitutionBias;
 
-		float jn = (targetVelocity - velAlongNormal) / invMassN;
-
-		float jnOld = contact.accumulatedNormalImpulse;
-		float jnNew = std::max(0.0f, jnOld + jn);
-		float jnDelta = jnNew - jnOld;
-		contact.accumulatedNormalImpulse = jnNew;
-		jn = jnDelta;
-
-		impl::applyImpulseTo(a, b, rA, rB, contact.normal * jn);
+		float jN = (targetVelocity - velAlongNormal) / invMassN;
+		impl::applyImpulseTo(a, b, rA, rB, contact.normal, jN, contact.accumulatedNormalImpulse, 0 );
 	}
 	// pseudo impulse
 	if ( !uf::physics::settings.ngsPositionSolver ) {
@@ -39,14 +38,8 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod
 		pod::Vector3f pseudoVb = b.pseudoVelocity + uf::vector::cross(b.pseudoAngularVelocity, rB);
 		float pseudoVelAlongNormal = uf::vector::dot(pseudoVb - pseudoVa, 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);
+		float jP = (penetrationBias - pseudoVelAlongNormal) / invMassN;
+		impl::applyPseudoImpulseTo(a, b, rA, rB, contact.normal, jP, contact.accumulatedPseudoImpulse, 0 );
 	}
 	// tangent friction
 	{
@@ -56,30 +49,23 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod
 		pod::Vector3f tangent = rv - contact.normal * uf::vector::dot(rv, contact.normal);
 		float tMag2 = uf::vector::magnitude(tangent);
 		if ( tMag2 > EPS2 ) {
-			contact.tangent = tangent / std::sqrt(tMag2);
-		} else if ( uf::vector::magnitude(contact.tangent) < EPS ) {
+			contact.tangent = tangent / std::sqrt( tMag2 );
+		} else if ( uf::vector::magnitude(contact.tangent) < EPS2 ) {
 			contact.tangent = impl::computeTangent( contact.normal );
 		}
 
 		float invMassT = impl::computeEffectiveMass(a, b, rA, rB, contact.tangent);
 		float vt = uf::vector::dot(rv, contact.tangent);
-		float jt = -vt / invMassT;
+		float jT = -vt / invMassT;
 
 		float mu_s = std::sqrt(a.material.staticFriction * b.material.staticFriction);
 		float mu_d = std::sqrt(a.material.dynamicFriction * b.material.dynamicFriction);
 
 		float normalForce = contact.accumulatedNormalImpulse;
-		if ( std::fabs(jt) > normalForce * mu_s ) {
-			jt = -normalForce * mu_d;
+		if ( std::fabs(jT) > normalForce * mu_s ) {
+			jT = -normalForce * mu_d;
 		}
-
 		float maxFriction = mu_s * normalForce;
-		float jtOld = contact.accumulatedTangentImpulse;
-		float jtNew = std::clamp(jtOld + jt, -maxFriction, maxFriction);
-		float jtDelta = jtNew - jtOld;
-		contact.accumulatedTangentImpulse = jtNew;
-		jt = jtDelta;
-
-		impl::applyImpulseTo(a, b, rA, rB, contact.tangent * jt);
+		impl::applyImpulseTo( a, b, rA, rB, contact.tangent, jT, contact.accumulatedTangentImpulse, -maxFriction, maxFriction );
 	}
 }

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

@@ -2,17 +2,6 @@
 #include <uf/utils/math/physics/integration.h>
 #include <uf/utils/math/physics/solvers.h>
 
-void impl::resolveManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
-	if ( uf::physics::settings.blockContactSolver ) {
-		if ( impl::blockSolver( a, b, manifold, dt ) ) return;
-	}
-	for ( auto& contact : manifold.points ) impl::iterativeImpulseSolver( a, b, contact, dt );
-}
-
-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 );
-}
 void impl::solvePositions( uf::stl::vector<pod::Manifold>& manifolds, float dt, uint32_t iterations ) {
 	if ( !uf::physics::settings.ngsPositionSolver ) return;
 	if ( uf::physics::settings.baumgarteCorrectionPercent <= 0 ) return;
@@ -26,50 +15,51 @@ void impl::solvePositions( uf::stl::vector<pod::Manifold>& manifolds, float dt,
 			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;
+				auto ctxA = impl::solverBodyContext( a );
+				auto ctxB = impl::solverBodyContext( b );
 
-				float penetration = uf::vector::dot( worldB - worldA, c.normal );
+				auto rA = uf::quaternion::rotate( tA.orientation, c.localA );
+				auto rB = uf::quaternion::rotate( tB.orientation, c.localB );
+
+				auto pA = tA.position + rA;
+				auto pB = tB.position + rB;
+				
+				auto row = pod::JacobianRow{ rA, rB, c.normal };
+
+				float penetration = uf::vector::dot( pB - pA, c.normal );
 
 				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 invMassN = impl::computeEffectiveMass( ctxA, ctxB, row );
 				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;
+				if ( ctxA.invM > 0.0f ) {
+					pod::Vector3f translation = P * ctxA.invM;
 					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));
-
+					pod::Vector3f deltaAngleA = uf::matrix::multiply(ctxA.invI, uf::vector::cross(rA, -P));
 					float angleA2 = uf::vector::magnitude( deltaAngleA );
 					if ( angleA2 > EPS2 ) {
-						float angleA = std::sqrt( angleA2);
+						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;
+				if ( ctxB.invM > 0.0f ) {
+					pod::Vector3f translation = P * ctxB.invM;
 					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));
-
+					pod::Vector3f deltaAngleB = uf::matrix::multiply(ctxB.invI, uf::vector::cross(rB, P));
 					float angleB2 = uf::vector::magnitude( deltaAngleB );
 					if ( angleB2 > EPS2 ) {
-						float angleB = std::sqrt( angleB2);
+						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);

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

@@ -30,6 +30,8 @@ namespace impl {
 
 
 // list of unit tests to "standardly" verify the system works, but honestly this is a mess
+// to-do: clean up all of this
+
 TEST(SphereSphere_Collision, {
 	pod::World world;
 	uf::Object objA, objB;
@@ -119,7 +121,7 @@ TEST(SphereAabb_Collision, {
 	bool collided = impl::sphereAabb(bodyA, bodyB, m);
 	EXPECT_TRUE(collided);
 	EXPECT_TRUE(!m.points.empty());
-	EXPECT_TRUE(m.points[0].penetration > 0.0f);
+	EXPECT_GT(m.points[0].penetration, 0.0f);
 })
 
 TEST(SpherePlane_Collision, {
@@ -174,7 +176,7 @@ TEST(CapsuleCapsule_Collision, {
 	bool collided = impl::capsuleCapsule(bodyA, bodyB, m);
 	EXPECT_TRUE(collided);
 	EXPECT_TRUE(!m.points.empty());
-	EXPECT_TRUE(m.points[0].penetration > 0.0f);
+	EXPECT_GT(m.points[0].penetration, 0.0f);
 })
 
 TEST(RayAabb_Miss, {
@@ -206,7 +208,7 @@ TEST(Gjk_SphereSphereOverlap, {
 	bool inside = gjk(a, b, simplex);
 	EXPECT_TRUE(inside);
 	auto contact = epa(a, b, simplex);
-	EXPECT_TRUE(contact.penetration > 0.0f);
+	EXPECT_GT(contact.penetration, 0.0f);
 })
 #endif
 
@@ -238,8 +240,8 @@ TEST(PhysicsStep_SpherePlane_Bounce, {
 	PHYSICS_STEP(1)
 
 	// After bouncing, sphere should be near plane surface, not sinking below
-	EXPECT_TRUE(sphere.transform->position.y >= 0.9f);
-	EXPECT_TRUE(fabs(sphere.velocity.y) < 10.0f); // should have reversed sign at least once
+	EXPECT_GE(sphere.transform->position.y, 0.9f);
+	EXPECT_LT(fabs(sphere.velocity.y), 10.0f); // should have reversed sign at least once
 })
 
 TEST(PhysicsStep_AabbStacking, {
@@ -255,8 +257,8 @@ TEST(PhysicsStep_AabbStacking, {
 	PHYSICS_STEP(5);
 
 	// After time, falling cube should rest on top of static one
-	EXPECT_TRUE(falling.transform->position.y > 1.9f);
-	EXPECT_TRUE(fabs(falling.velocity.y) < 0.1f);
+	EXPECT_GT(falling.transform->position.y, 1.9f);
+	EXPECT_LT(fabs(falling.velocity.y),0.1f);
 })
 
 TEST(PhysicsStep_SphereSphere_HeadOn, {
@@ -274,8 +276,8 @@ TEST(PhysicsStep_SphereSphere_HeadOn, {
 	PHYSICS_STEP(5);
 
 	// Expect velocities swapped (perfect elastic bounce with equal masses)
-	EXPECT_TRUE(A.velocity.x < 0.0f);
-	EXPECT_TRUE(B.velocity.x > 0.0f);
+	EXPECT_LT(A.velocity.x,0.0f);
+	EXPECT_GT(B.velocity.x, 0.0f);
 })
 
 TEST(PhysicsStep_RaycastDynamic, {
@@ -291,7 +293,7 @@ TEST(PhysicsStep_RaycastDynamic, {
 	pod::Ray ray{ {0,0,-5}, {0,0,1} };
 	pod::RayQuery q = uf::physics::rayCast(ray, world, 100.0f);
 	EXPECT_TRUE(q.hit);
-	EXPECT_TRUE(fabs(q.contact.point.z - 10.0f) <= 1.0f); // near where it moved
+	EXPECT_LE(fabs(q.contact.point.z - 10.0f), 1.0f); // near where it moved
 })
 
 TEST(SphereSphere_TouchingButNotOverlapping, {
@@ -365,7 +367,7 @@ TEST(PhysicsStep_StaticFriction_Slips, {
 
 	PHYSICS_STEP(1);
 
-	EXPECT_TRUE(fabs(bodyA.transform->position.x) > 0.1f); // It should slide
+	EXPECT_GT(fabs(bodyA.transform->position.x), 0.1f); // It should slide
 })
 
 // not really a good way to check as these are solver-dependent
@@ -400,7 +402,7 @@ TEST(CapsulePlane_Slope_Slip, {
 
 	PHYSICS_STEP(5);
 
-	EXPECT_TRUE(fabs(bodyA.transform->position.z) > 1.0f); // Should have slid downhill
+	EXPECT_GT(fabs(bodyA.transform->position.z), 1.0f); // Should have slid downhill
 })
 
 TEST(CapsulePlane_RestingContact, {
@@ -787,7 +789,7 @@ TEST(MeshSphere_Collision, {
 	bool collided = impl::meshSphere(bodyA, bodyB, m);
 	EXPECT_TRUE(collided);
 	EXPECT_TRUE(!m.points.empty());
-	if ( !m.points.empty() ) EXPECT_TRUE(m.points[0].penetration > 0.0f);
+	if ( !m.points.empty() ) EXPECT_GT(m.points[0].penetration, 0.0f);
 })
 
 TEST(MeshSphere_NoCollision, {
@@ -864,7 +866,7 @@ TEST(RayMesh_Hit, {
 	pod::RayQuery hit = uf::physics::rayCast(ray, world, 100.0f);
 
 	EXPECT_TRUE(hit.hit);
-	EXPECT_TRUE(hit.contact.penetration > 0.0f);
+	EXPECT_GT(hit.contact.penetration, 0.0f);
 })
 
 TEST(RayMesh_Miss, {
@@ -1250,4 +1252,371 @@ TEST(TriangleCapsule_Collision_EdgeAlignment, {
 	bool collided = impl::triangleCapsule(bodyA, bodyB, m);
 
 	EXPECT_TRUE(collided);
+})
+
+TEST(BallSocketJoint_Constraint, {
+	pod::World world;
+	uf::Object objA, objB;
+
+	auto& bodyA = uf::physics::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 0.0f);
+	auto& bodyB = uf::physics::create(world, objB, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
+
+	bodyA.transform->position = {0, 0, 0};
+	bodyB.transform->position = {5, 0, 0};
+
+	pod::Constraint constraint;
+	pod::BallSocket ballSocket = {};
+	ballSocket.localAnchorA = {1, 0, 0};
+	ballSocket.localAnchorB = {-1, 0, 0};
+
+	constraint.type = pod::ConstraintType::HINGE;
+	constraint.a = &bodyA;
+	constraint.b = &bodyB;
+	constraint.ballSocket = ballSocket;
+
+	float dt = 1.0f / 60.0f;
+
+	for (int i = 0; i < 10; ++i) {
+		bodyB.velocity.y -= 9.8f * dt;
+
+		for (int solverIters = 0; solverIters < 10; ++solverIters) {
+			impl::solveBallSocketConstraint( constraint, dt );
+		}
+
+		bodyB.transform->position += bodyB.velocity * dt;
+	}
+
+	auto tA = impl::getTransform(bodyA);
+	auto tB = impl::getTransform(bodyB);
+	pod::Vector3f worldAnchorA = tA.position + uf::quaternion::rotate(tA.orientation, constraint.ballSocket.localAnchorA);
+	pod::Vector3f worldAnchorB = tB.position + uf::quaternion::rotate(tB.orientation, constraint.ballSocket.localAnchorB);
+
+	float errorSq = uf::vector::distanceSquared(worldAnchorA, worldAnchorB);
+	EXPECT_LT( errorSq, 0.2f );
+})
+
+TEST(Hinge_Constraint, {
+	pod::World world;
+	uf::Object objA, objB;
+
+	auto& bodyA = uf::physics::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 0.0f);
+	auto& bodyB = uf::physics::create(world, objB, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
+
+	bodyA.transform->position = {0, 0, 0};
+	bodyA.transform->orientation = {0, 0, 0, 1};
+
+	bodyB.transform->position = {3, 1, 0}; // expected to be at {2, 0, 0}
+	bodyB.transform->orientation = uf::quaternion::axisAngle({0, 0, 1}, M_PI / 4.0f);
+
+	bodyB.angularVelocity = {0, 2.0f, 0};
+
+	pod::Hinge hinge = {};
+	hinge.localAnchorA = {1, 0, 0};
+	hinge.localAnchorB = {-1, 0, 0};
+	hinge.localAxisA = {0, 1, 0};
+	hinge.localAxisB = {0, 1, 0};
+
+	pod::Constraint constraint;
+	constraint.type = pod::ConstraintType::HINGE;
+	constraint.a = &bodyA;
+	constraint.b = &bodyB;
+	constraint.hinge = hinge;
+
+	float dt = 1.0f / 60.0f;
+
+	for (int i = 0; i < 10; ++i) {
+		bodyB.velocity.y -= 9.81f * dt;
+
+		for (int solverIters = 0; solverIters < 10; ++solverIters) {
+			impl::solveHingeConstraint( constraint, dt );
+		}
+
+		bodyB.transform->position += bodyB.velocity * dt;
+
+		float angularSpeed2 = uf::vector::magnitude( bodyB.angularVelocity );
+		if ( angularSpeed2 > 0.0001f ) {
+			float angularSpeed = std::sqrt( angularSpeed2 );
+			pod::Quaternion<> dq = uf::quaternion::axisAngle( bodyB.angularVelocity / angularSpeed, angularSpeed * dt );
+			bodyB.transform->orientation = uf::quaternion::multiply(dq, bodyB.transform->orientation);
+			bodyB.transform->orientation = uf::quaternion::normalize(bodyB.transform->orientation);
+		}
+	}
+
+	auto tA = impl::getTransform(bodyA);
+	auto tB = impl::getTransform(bodyB);
+	pod::Vector3f worldAnchorA = tA.position + uf::quaternion::rotate(tA.orientation, constraint.hinge.localAnchorA);
+	pod::Vector3f worldAnchorB = tB.position + uf::quaternion::rotate(tB.orientation, constraint.hinge.localAnchorB);
+
+	float posErrorSq = uf::vector::distanceSquared(worldAnchorA, worldAnchorB);
+	EXPECT_LT( posErrorSq, 0.01f );
+
+	pod::Vector3f worldAxisA = uf::quaternion::rotate(tA.orientation, constraint.hinge.localAxisA);
+	pod::Vector3f worldAxisB = uf::quaternion::rotate(tB.orientation, constraint.hinge.localAxisB);
+
+	float axisDot = uf::vector::dot(worldAxisA, worldAxisB);
+	EXPECT_GT( axisDot, 0.99f );
+})
+
+TEST(ConeTwist_Constraint, {
+    pod::World world;
+    uf::Object objA, objB;
+
+    auto& bodyA = uf::physics::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 0.0f);
+    auto& bodyB = uf::physics::create(world, objB, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
+
+    bodyA.transform->position = {0, 0, 0};
+    bodyA.transform->orientation = {0, 0, 0, 1};
+
+    bodyB.transform->position = {3, 1, 0};
+    bodyB.transform->orientation = {0, 0, 0, 1};
+
+    bodyB.angularVelocity = {10.0f, 0.0f, 10.0f};
+
+    pod::ConeTwist coneTwist = {};
+    coneTwist.localAnchorA = {1, 0, 0};
+    coneTwist.localAnchorB = {-1, 0, 0};
+
+    coneTwist.localTwistAxisA = {1, 0, 0};
+    coneTwist.localTwistAxisB = {1, 0, 0};
+
+    coneTwist.localReferenceAxisA = {0, 1, 0};
+    coneTwist.localReferenceAxisB = {0, 1, 0};
+
+    coneTwist.swingLimit = M_PI / 4.0f; // 45 degrees
+    coneTwist.twistLimit = M_PI / 8.0f; // 22.5 degrees
+
+    pod::Constraint constraint;
+    constraint.type = pod::ConstraintType::CONE_TWIST;
+    constraint.a = &bodyA;
+    constraint.b = &bodyB;
+    constraint.coneTwist = coneTwist;
+
+    float dt = 1.0f / 60.0f;
+
+    for (int i = 0; i < 60; ++i) {
+        bodyB.velocity.y -= 9.81f * dt;
+
+        for (int solverIters = 0; solverIters < 10; ++solverIters) {
+        	impl::solveConeTwistConstraint( constraint, dt );
+        }
+
+        bodyB.transform->position += bodyB.velocity * dt;
+
+        float angularSpeedSq = uf::vector::dot(bodyB.angularVelocity, bodyB.angularVelocity);
+        if ( angularSpeedSq > 0.0001f ) {
+            float angularSpeed = std::sqrt( angularSpeedSq );
+            pod::Quaternion<> dq = uf::quaternion::axisAngle( bodyB.angularVelocity / angularSpeed, angularSpeed * dt );
+            bodyB.transform->orientation = uf::quaternion::multiply(dq, bodyB.transform->orientation);
+            bodyB.transform->orientation = uf::quaternion::normalize(bodyB.transform->orientation);
+        }
+    }
+
+    auto tA = impl::getTransform(bodyA);
+    auto tB = impl::getTransform(bodyB);
+
+    pod::Vector3f worldAnchorA = tA.position + uf::quaternion::rotate(tA.orientation, constraint.coneTwist.localAnchorA);
+    pod::Vector3f worldAnchorB = tB.position + uf::quaternion::rotate(tB.orientation, constraint.coneTwist.localAnchorB);
+
+    float posErrorSq = uf::vector::distanceSquared(worldAnchorA, worldAnchorB);
+    EXPECT_LT( posErrorSq, 0.01f );
+
+    pod::Vector3f worldTwistA = uf::quaternion::rotate(tA.orientation, constraint.coneTwist.localTwistAxisA);
+    pod::Vector3f worldTwistB = uf::quaternion::rotate(tB.orientation, constraint.coneTwist.localTwistAxisB);
+
+    float swingDot = uf::vector::dot(worldTwistA, worldTwistB);
+
+    swingDot = std::clamp(swingDot, -1.0f, 1.0f);
+    float swingAngle = std::acos(swingDot);
+
+    EXPECT_LE( swingAngle, coneTwist.swingLimit + 0.05f );
+
+    pod::Vector3f worldRefA = uf::quaternion::rotate(tA.orientation, constraint.coneTwist.localReferenceAxisA);
+    pod::Vector3f worldRefB = uf::quaternion::rotate(tB.orientation, constraint.coneTwist.localReferenceAxisB);
+
+    pod::Vector3f projectedRefB = worldRefB - (worldTwistA * uf::vector::dot(worldRefB, worldTwistA));
+    float projectedLength2 = uf::vector::dot(projectedRefB, projectedRefB);
+    if ( projectedLength2 > 0.0001f ) {
+        projectedRefB = projectedRefB / std::sqrt(projectedLength2);
+
+        pod::Vector3f crossRef = uf::vector::cross(worldRefA, projectedRefB);
+        float sinTheta = uf::vector::dot(crossRef, worldTwistA);
+        float cosTheta = uf::vector::dot(worldRefA, projectedRefB);
+        float twistAngle = std::atan2(sinTheta, cosTheta);
+
+        EXPECT_LE( std::fabs(twistAngle), coneTwist.twistLimit + 0.05f );
+    }
+})
+
+TEST(DistanceJoint_Constraint, {
+    pod::World world;
+    uf::Object objA, objB;
+
+    auto& bodyA = uf::physics::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 0.0f);
+    auto& bodyB = uf::physics::create(world, objB, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
+
+    bodyA.transform->position = {0, 10, 0};
+    bodyB.transform->position = {0, 5, 0};
+
+    pod::Constraint constraint;
+    constraint.type = pod::ConstraintType::DISTANCE;
+    constraint.a = &bodyA;
+    constraint.b = &bodyB;
+
+    constraint.distance.localAnchorA = {0, 0, 0};
+    constraint.distance.localAnchorB = {0, 0, 0};
+    constraint.distance.targetDistance = 5.0f;
+    constraint.distance.isRope = false;
+    constraint.distance.accumulatedImpulse = 0.0f;
+
+    float dt = 1.0f / 60.0f;
+
+    for (int i = 0; i < 60; ++i) {
+        bodyB.velocity.y -= 9.8f * dt;
+
+        for (int solverIters = 0; solverIters < 10; ++solverIters) {
+            impl::solveDistanceConstraint( constraint, dt );
+        }
+
+        bodyB.transform->position += bodyB.velocity * dt;
+    }
+
+    auto tA = impl::getTransform(bodyA);
+    auto tB = impl::getTransform(bodyB);
+    pod::Vector3f worldAnchorA = tA.position + uf::quaternion::rotate(tA.orientation, constraint.distance.localAnchorA);
+    pod::Vector3f worldAnchorB = tB.position + uf::quaternion::rotate(tB.orientation, constraint.distance.localAnchorB);
+
+    float currentDistance = std::sqrt(uf::vector::distanceSquared(worldAnchorA, worldAnchorB));
+    float error = std::abs(currentDistance - constraint.distance.targetDistance);
+
+    EXPECT_LT( error, 0.1f );
+})
+
+TEST(WeldJoint_Constraint, {
+    pod::World world;
+    uf::Object objA, objB;
+
+    auto& bodyA = uf::physics::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 0.0f);
+    auto& bodyB = uf::physics::create(world, objB, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
+
+    bodyA.transform->position = {0, 0, 0};
+    bodyB.transform->position = {2, 0, 0};
+
+    pod::Constraint constraint;
+    constraint.type = pod::ConstraintType::WELD;
+    constraint.a = &bodyA;
+    constraint.b = &bodyB;
+
+    constraint.weld.localAnchorA = {1, 0, 0};
+    constraint.weld.localAnchorB = {-1, 0, 0};
+    constraint.weld.localAxisA = {1, 0, 0};
+    constraint.weld.localAxisB = {1, 0, 0};
+    constraint.weld.localReferenceAxisA = {0, 1, 0};
+    constraint.weld.localReferenceAxisB = {0, 1, 0};
+
+    float dt = 1.0f / 60.0f;
+
+    for (int i = 0; i < 30; ++i) {
+        bodyB.velocity.y -= 10.0f * dt;
+        bodyB.angularVelocity.z += 5.0f * dt;
+
+        for (int solverIters = 0; solverIters < 10; ++solverIters) {
+            impl::solveWeldConstraint( constraint, dt );
+        }
+
+        bodyB.transform->position += bodyB.velocity * dt;
+    }
+
+    auto tA = impl::getTransform(bodyA);
+    auto tB = impl::getTransform(bodyB);
+    pod::Vector3f worldAnchorA = tA.position + uf::quaternion::rotate(tA.orientation, constraint.weld.localAnchorA);
+    pod::Vector3f worldAnchorB = tB.position + uf::quaternion::rotate(tB.orientation, constraint.weld.localAnchorB);
+
+    float posErrorSq = uf::vector::distanceSquared(worldAnchorA, worldAnchorB);
+    EXPECT_LT( posErrorSq, 0.1f );
+})
+
+TEST(SliderJoint_Constraint, {
+    pod::World world;
+    uf::Object objA, objB;
+
+    auto& bodyA = uf::physics::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 0.0f);
+    auto& bodyB = uf::physics::create(world, objB, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
+
+    bodyA.transform->position = {0, 0, 0};
+    bodyB.transform->position = {2, 0, 0};
+
+    pod::Constraint constraint;
+    constraint.type = pod::ConstraintType::SLIDER;
+    constraint.a = &bodyA;
+    constraint.b = &bodyB;
+
+    constraint.slider.localAnchorA = {0, 0, 0};
+    constraint.slider.localAnchorB = {0, 0, 0};
+    constraint.slider.localAxisA = {1, 0, 0};
+    constraint.slider.localAxisB = {1, 0, 0};
+    constraint.slider.localReferenceAxisA = {0, 1, 0};
+    constraint.slider.localReferenceAxisB = {0, 1, 0};
+    constraint.slider.lowerLimit = -5.0f;
+    constraint.slider.upperLimit = 5.0f;
+
+    float dt = 1.0f / 60.0f;
+
+    for (int i = 0; i < 60; ++i) {
+        bodyB.velocity.x += 50.0f * dt;
+
+        for (int solverIters = 0; solverIters < 10; ++solverIters) {
+            impl::solveSliderConstraint( constraint, dt );
+        }
+
+        bodyB.transform->position += bodyB.velocity * dt;
+    }
+
+    auto tA = impl::getTransform(bodyA);
+    auto tB = impl::getTransform(bodyB);
+    pod::Vector3f relPos = tB.position - tA.position;
+    float slideDistance = uf::vector::dot(relPos, constraint.slider.localAxisA);
+
+    EXPECT_LT( slideDistance, constraint.slider.upperLimit + 0.2f );
+})
+
+TEST(SpringJoint_Constraint, {
+    pod::World world;
+    uf::Object objA, objB;
+
+    auto& bodyA = uf::physics::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 0.0f);
+    auto& bodyB = uf::physics::create(world, objB, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f);
+
+    bodyA.transform->position = {0, 10, 0};
+    bodyB.transform->position = {0, 0, 0};
+
+    pod::Constraint constraint;
+    constraint.type = pod::ConstraintType::SPRING;
+    constraint.a = &bodyA;
+    constraint.b = &bodyB;
+
+    constraint.spring.localAnchorA = {0, 0, 0};
+    constraint.spring.localAnchorB = {0, 0, 0};
+    constraint.spring.restLength = 5.0f;
+    constraint.spring.stiffness = 50.0f;
+    constraint.spring.damping = 5.0f;
+    constraint.spring.accumulatedImpulse = 0.0f;
+
+    float dt = 1.0f / 60.0f;
+
+    for (int i = 0; i < 180; ++i) {
+        bodyB.velocity.y -= 9.8f * dt;
+
+        for (int solverIters = 0; solverIters < 10; ++solverIters) {
+            impl::solveSpringConstraint( constraint, dt );
+        }
+
+        bodyB.transform->position += bodyB.velocity * dt;
+    }
+
+    auto tA = impl::getTransform(bodyA);
+    auto tB = impl::getTransform(bodyB);
+    float distance = std::sqrt(uf::vector::distanceSquared(tA.position, tB.position));
+
+    EXPECT_LT( distance, constraint.spring.restLength + 2.0f );
+    EXPECT_GT( distance, constraint.spring.restLength - 0.1f );
 })