From 7758389c6cc60149f670a359293d1c993dcc2bfb Mon Sep 17 00:00:00 2001 From: ecker Date: Thu, 21 May 2026 20:42:54 -0500 Subject: [PATCH] more neurotic code cleanup, added joint constraints --- .../uf/utils/math/physics/broadphase/island.h | 2 +- .../inc/uf/utils/math/physics/constraints.h | 12 + .../math/physics/constraints/ballSocket.h | 16 + .../math/physics/constraints/coneTwist.h | 16 + .../utils/math/physics/constraints/contact.h | 22 + .../uf/utils/math/physics/constraints/hinge.h | 16 + engine/inc/uf/utils/math/physics/impl.h | 330 +------------ .../inc/uf/utils/math/physics/integration.h | 53 ++- .../uf/utils/math/physics/narrowphase/aabb.h | 7 + .../utils/math/physics/narrowphase/capsule.h | 7 + .../uf/utils/math/physics/narrowphase/mesh.h | 7 + .../uf/utils/math/physics/narrowphase/obb.h | 7 + .../uf/utils/math/physics/narrowphase/plane.h | 7 + .../uf/utils/math/physics/narrowphase/ray.h | 8 + .../utils/math/physics/narrowphase/sphere.h | 7 + .../utils/math/physics/narrowphase/triangle.h | 7 + engine/inc/uf/utils/math/physics/solvers.h | 4 +- engine/inc/uf/utils/math/physics/structs.h | 376 +++++++++++++++ engine/inc/uf/utils/math/vector.h | 3 + engine/inc/uf/utils/math/vector/pod.inl | 25 + .../utils/math/physics/broadphase/island.cpp | 36 +- engine/src/utils/math/physics/common.cpp | 30 +- engine/src/utils/math/physics/constraints.cpp | 17 + .../math/physics/constraints/ballSocket.cpp | 86 ++++ .../math/physics/constraints/coneTwist.cpp | 201 ++++++++ .../math/physics/constraints/contact.cpp | 354 ++++++++++++++ .../utils/math/physics/constraints/hinge.cpp | 87 ++++ engine/src/utils/math/physics/impl.cpp | 204 +++----- engine/src/utils/math/physics/integration.cpp | 442 +++--------------- .../utils/math/physics/narrowphase/aabb.cpp | 13 + .../math/physics/narrowphase/capsule.cpp | 13 + .../utils/math/physics/narrowphase/mesh.cpp | 26 ++ .../utils/math/physics/narrowphase/obb.cpp | 25 +- .../utils/math/physics/narrowphase/plane.cpp | 13 + .../utils/math/physics/narrowphase/ray.cpp | 40 ++ .../utils/math/physics/narrowphase/sphere.cpp | 13 + .../math/physics/narrowphase/triangle.cpp | 23 +- .../src/utils/math/physics/solvers/block.cpp | 29 +- .../math/physics/solvers/iterativeImpulse.cpp | 40 +- .../physics/{solvers.cpp => solvers/ngs.cpp} | 15 +- engine/src/utils/math/physics/tests.inl | 223 ++++++++- 41 files changed, 1903 insertions(+), 959 deletions(-) create mode 100644 engine/inc/uf/utils/math/physics/constraints.h create mode 100644 engine/inc/uf/utils/math/physics/constraints/ballSocket.h create mode 100644 engine/inc/uf/utils/math/physics/constraints/coneTwist.h create mode 100644 engine/inc/uf/utils/math/physics/constraints/contact.h create mode 100644 engine/inc/uf/utils/math/physics/constraints/hinge.h create mode 100644 engine/inc/uf/utils/math/physics/structs.h create mode 100644 engine/src/utils/math/physics/constraints.cpp create mode 100644 engine/src/utils/math/physics/constraints/ballSocket.cpp create mode 100644 engine/src/utils/math/physics/constraints/coneTwist.cpp create mode 100644 engine/src/utils/math/physics/constraints/contact.cpp create mode 100644 engine/src/utils/math/physics/constraints/hinge.cpp rename engine/src/utils/math/physics/{solvers.cpp => solvers/ngs.cpp} (74%) diff --git a/engine/inc/uf/utils/math/physics/broadphase/island.h b/engine/inc/uf/utils/math/physics/broadphase/island.h index 63280972..b6aa089c 100644 --- a/engine/inc/uf/utils/math/physics/broadphase/island.h +++ b/engine/inc/uf/utils/math/physics/broadphase/island.h @@ -3,5 +3,5 @@ #include "../impl.h" namespace impl { - void buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector& bodies, uf::stl::vector& islands ); + void buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector& bodies, const uf::stl::vector& constraints, uf::stl::vector& islands ); } \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/constraints.h b/engine/inc/uf/utils/math/physics/constraints.h new file mode 100644 index 00000000..956069cb --- /dev/null +++ b/engine/inc/uf/utils/math/physics/constraints.h @@ -0,0 +1,12 @@ +#pragma once + +#include "impl.h" + +#include "constraints/contact.h" +#include "constraints/ballSocket.h" +#include "constraints/hinge.h" +#include "constraints/coneTwist.h" + +namespace impl { + void solveConstraint( pod::Constraint& constraint, float dt ); +} \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/constraints/ballSocket.h b/engine/inc/uf/utils/math/physics/constraints/ballSocket.h new file mode 100644 index 00000000..44112d06 --- /dev/null +++ b/engine/inc/uf/utils/math/physics/constraints/ballSocket.h @@ -0,0 +1,16 @@ +#pragma once + +#include "../impl.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& ); + } +} \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/constraints/coneTwist.h b/engine/inc/uf/utils/math/physics/constraints/coneTwist.h new file mode 100644 index 00000000..8b086b23 --- /dev/null +++ b/engine/inc/uf/utils/math/physics/constraints/coneTwist.h @@ -0,0 +1,16 @@ +#pragma once + +#include "../impl.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 ); + } +} \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/constraints/contact.h b/engine/inc/uf/utils/math/physics/constraints/contact.h new file mode 100644 index 00000000..229c6f98 --- /dev/null +++ b/engine/inc/uf/utils/math/physics/constraints/contact.h @@ -0,0 +1,22 @@ +#pragma once + +#include "../impl.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& cache, const uf::stl::vector& islands, const uf::stl::vector& bodies ); + void updateManifoldCache( const uf::stl::vector& manifolds, uf::stl::unordered_map& cache ); + void pruneManifoldCache( uf::stl::unordered_map& 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& manifolds, float dt ); +} \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/constraints/hinge.h b/engine/inc/uf/utils/math/physics/constraints/hinge.h new file mode 100644 index 00000000..3b6d654a --- /dev/null +++ b/engine/inc/uf/utils/math/physics/constraints/hinge.h @@ -0,0 +1,16 @@ +#pragma once + +#include "../impl.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& ); + } +} \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/impl.h b/engine/inc/uf/utils/math/physics/impl.h index 8cd47cb8..6031d41b 100644 --- a/engine/inc/uf/utils/math/physics/impl.h +++ b/engine/inc/uf/utils/math/physics/impl.h @@ -2,23 +2,11 @@ #include #include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include #include - #include +#include "structs.h" + #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 ); @@ -32,293 +20,6 @@ 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 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 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 pairs_t; - typedef uf::stl::vector 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 indices; - uf::stl::vector nodes; - uf::stl::vector flattened; - - uf::stl::vector bounds; - uf::stl::vector 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 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 indices; - pod::BVH::pairs_t pairs; - - uf::stl::vector 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 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 bodies; - - pod::Vector3f gravity = { 0, -9.81f, 0 }; - pod::BVH dynamicBvh; - pod::BVH staticBvh; - }; -} - namespace uf { namespace physics { typedef pod::Math::num_t num_t; @@ -370,29 +71,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& ); } } \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/integration.h b/engine/inc/uf/utils/math/physics/integration.h index 4fe139b2..9b4dfd8b 100644 --- a/engine/inc/uf/utils/math/physics/integration.h +++ b/engine/inc/uf/utils/math/physics/integration.h @@ -5,22 +5,47 @@ namespace impl { float computeEffectiveMass( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& n ); 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::max(), float maxLimit = std::numeric_limits::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::max(), float maxLimit = std::numeric_limits::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::max(), float maxLimit = std::numeric_limits::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::max(), float maxLimit = std::numeric_limits::max() + ); + + float accumulateImpulseTo( + float magnitude, float& accumulatedImpulse, + float minLimit = -std::numeric_limits::max(), float maxLimit = std::numeric_limits::max() + ); + pod::Vector3f accumulateImpulseTo( + const pod::Vector3f& impulse, pod::Vector3f& accumulatedImpulse, + float minLimit = -std::numeric_limits::max(), float maxLimit = std::numeric_limits::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& cache, const uf::stl::vector& islands, const uf::stl::vector& bodies ); - void updateManifoldCache( const uf::stl::vector& manifolds, uf::stl::unordered_map& cache ); - void pruneManifoldCache( uf::stl::unordered_map& 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 ); } \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/narrowphase/aabb.h b/engine/inc/uf/utils/math/physics/narrowphase/aabb.h index bd340460..146e6194 100644 --- a/engine/inc/uf/utils/math/physics/narrowphase/aabb.h +++ b/engine/inc/uf/utils/math/physics/narrowphase/aabb.h @@ -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& = {} ); + } } \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/narrowphase/capsule.h b/engine/inc/uf/utils/math/physics/narrowphase/capsule.h index 144d48a8..41397c1e 100644 --- a/engine/inc/uf/utils/math/physics/narrowphase/capsule.h +++ b/engine/inc/uf/utils/math/physics/narrowphase/capsule.h @@ -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& = {} ); + } } \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/narrowphase/mesh.h b/engine/inc/uf/utils/math/physics/narrowphase/mesh.h index 6a60a4af..c12b224c 100644 --- a/engine/inc/uf/utils/math/physics/narrowphase/mesh.h +++ b/engine/inc/uf/utils/math/physics/narrowphase/mesh.h @@ -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 ); + } } \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/narrowphase/obb.h b/engine/inc/uf/utils/math/physics/narrowphase/obb.h index 8820c06f..e213f061 100644 --- a/engine/inc/uf/utils/math/physics/narrowphase/obb.h +++ b/engine/inc/uf/utils/math/physics/narrowphase/obb.h @@ -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& = {} ); + } } \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/narrowphase/plane.h b/engine/inc/uf/utils/math/physics/narrowphase/plane.h index 95b4c871..300e01c1 100644 --- a/engine/inc/uf/utils/math/physics/narrowphase/plane.h +++ b/engine/inc/uf/utils/math/physics/narrowphase/plane.h @@ -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& = {} ); + } } \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/narrowphase/ray.h b/engine/inc/uf/utils/math/physics/narrowphase/ray.h index 971ecce4..fac51fd7 100644 --- a/engine/inc/uf/utils/math/physics/narrowphase/ray.h +++ b/engine/inc/uf/utils/math/physics/narrowphase/ray.h @@ -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 ); + } } \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/narrowphase/sphere.h b/engine/inc/uf/utils/math/physics/narrowphase/sphere.h index dccdc524..a3cce687 100644 --- a/engine/inc/uf/utils/math/physics/narrowphase/sphere.h +++ b/engine/inc/uf/utils/math/physics/narrowphase/sphere.h @@ -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& = {} ); + } } \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/narrowphase/triangle.h b/engine/inc/uf/utils/math/physics/narrowphase/triangle.h index 08da2e80..db641d5f 100644 --- a/engine/inc/uf/utils/math/physics/narrowphase/triangle.h +++ b/engine/inc/uf/utils/math/physics/narrowphase/triangle.h @@ -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& = {} ); + } } \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/solvers.h b/engine/inc/uf/utils/math/physics/solvers.h index 60a44b06..e3d05d9a 100644 --- a/engine/inc/uf/utils/math/physics/solvers.h +++ b/engine/inc/uf/utils/math/physics/solvers.h @@ -2,11 +2,11 @@ #include "impl.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& manifolds, float dt ); void solvePositions( uf::stl::vector& manifolds, float dt, uint32_t iterations = 4 ); } \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/structs.h b/engine/inc/uf/utils/math/physics/structs.h new file mode 100644 index 00000000..70466bd8 --- /dev/null +++ b/engine/inc/uf/utils/math/physics/structs.h @@ -0,0 +1,376 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +// 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 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, + //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; + }; + + // 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 points; + }; + + struct Constraint { + pod::ConstraintType type = {}; + pod::PhysicsBody* a = NULL; + pod::PhysicsBody* b = NULL; + + union { + pod::BallSocket ballSocket; + pod::Hinge hinge; + pod::ConeTwist coneTwist; + }; + }; +} + +// BVH +namespace pod { + struct BVH { + typedef uint32_t index_t; + typedef std::pair 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 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 indices; + uf::stl::vector nodes; + uf::stl::vector flattened; + + uf::stl::vector bounds; + uf::stl::vector 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 indices; + pod::BVH::pairs_t pairs; + + uf::stl::vector manifolds; + uf::stl::vector 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; + + uf::stl::unordered_map 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 bodies; + uf::stl::vector constraints; + + pod::Vector3f gravity = { 0, -9.81f, 0 }; + pod::BVH dynamicBvh; + pod::BVH staticBvh; + }; +} \ No newline at end of file diff --git a/engine/inc/uf/utils/math/vector.h b/engine/inc/uf/utils/math/vector.h index 5a8ec9b1..d3944fc5 100644 --- a/engine/inc/uf/utils/math/vector.h +++ b/engine/inc/uf/utils/math/vector.h @@ -133,7 +133,10 @@ namespace uf { template /*FORCE_INLINE*/ T /*UF_API*/ max( const T& left, const T& right ); // returns the maximum of each component between two vectors template /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ min( const T& v ); // returns the minimum component of a vector template /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ max( const T& v ); // returns the maximum component of a vector + template /*FORCE_INLINE*/ T /*UF_API*/ min( const T& vector, typename T::type_t& min ); // applies min on a vector + template /*FORCE_INLINE*/ T /*UF_API*/ max( const T& vector, typename T::type_t& max ); // applies max on a vector template /*FORCE_INLINE*/ T /*UF_API*/ clamp( const T& vector, const T& min, const T& max ); // clamps a vector between two bounds + template /*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 /*FORCE_INLINE*/ T /*UF_API*/ ceil( const T& vector ); // rounds each component of the the vector up template /*FORCE_INLINE*/ T /*UF_API*/ floor( const T& vector ); // rounds each component of the vector down template /*FORCE_INLINE*/ T /*UF_API*/ round( const T& vector ); // rounds each component of the vector diff --git a/engine/inc/uf/utils/math/vector/pod.inl b/engine/inc/uf/utils/math/vector/pod.inl index 0d48c75e..c7bfea3e 100644 --- a/engine/inc/uf/utils/math/vector/pod.inl +++ b/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 +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::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 +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 T uf::vector::clamp( const T& vector, const T& min, const T& max ) { return uf::vector::max( min, uf::vector::min( vector, max ) ); } template +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 T uf::vector::ceil( const T& vector ) { T res; FOR_EACH(T::size, { diff --git a/engine/src/utils/math/physics/broadphase/island.cpp b/engine/src/utils/math/physics/broadphase/island.cpp index e88a4319..3e7a288f 100644 --- a/engine/src/utils/math/physics/broadphase/island.cpp +++ b/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& bodies, uf::stl::vector& islands ) { +void impl::buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector& bodies, const uf::stl::vector& constraints, uf::stl::vector& islands ) { UnionFind unionizer(bodies.size()); + // map bodies to indices + uf::stl::unordered_map 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

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 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

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; diff --git a/engine/src/utils/math/physics/common.cpp b/engine/src/utils/math/physics/common.cpp index 9ce89a0b..a28344d3 100644 --- a/engine/src/utils/math/physics/common.cpp +++ b/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 diff --git a/engine/src/utils/math/physics/constraints.cpp b/engine/src/utils/math/physics/constraints.cpp new file mode 100644 index 00000000..b3a0d08a --- /dev/null +++ b/engine/src/utils/math/physics/constraints.cpp @@ -0,0 +1,17 @@ +#include +#include + +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; + } +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/constraints/ballSocket.cpp b/engine/src/utils/math/physics/constraints/ballSocket.cpp new file mode 100644 index 00000000..173ea5b9 --- /dev/null +++ b/engine/src/utils/math/physics/constraints/ballSocket.cpp @@ -0,0 +1,86 @@ +#include +#include +#include + +void impl::solveBallSocketConstraint( pod::Constraint& constraint, float dt ) { + auto& a = *constraint.a; + auto& b = *constraint.b; + auto& joint = constraint.ballSocket; + + 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 positionError = worldAnchorB - worldAnchorA; + + auto vA = a.velocity + uf::vector::cross(a.angularVelocity, rA); + auto vB = b.velocity + uf::vector::cross(b.angularVelocity, rB); + auto relativeVelocity = vB - vA; + + float invMassA = a.isStatic ? 0.0f : a.inverseMass; + float invMassB = b.isStatic ? 0.0f : b.inverseMass; + float sumInvMass = invMassA + invMassB; + + pod::Matrix3f invIa = impl::computeWorldInverseInertia( a ); + pod::Matrix3f invIb = impl::computeWorldInverseInertia( b ); + + // effective mass matrix + 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 ) { + float termLinear = (i == j) ? sumInvMass : 0.0f; + + pod::Vector3f raXnj = uf::vector::cross(rA, axes[j]); + pod::Vector3f rbXnj = uf::vector::cross(rB, axes[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); + pod::Vector3f crossB = uf::vector::cross(Ib_rbXnj, rB); + + float termAngular = uf::vector::dot(axes[i], crossA + crossB); + + K(i, j) = termLinear + termAngular; + } + } + + // impulse matrix + pod::Matrix3f Kinv = uf::matrix::inverse( K ); + // bias + pod::Vector3f bias = positionError * (uf::physics::settings.baumgarteCorrectionPercent / dt); + pod::Vector3f rhs = -(relativeVelocity + bias); + // solve and apply + 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(), b.getComponent(), 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(), b.getComponent(), joint ); +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/constraints/coneTwist.cpp b/engine/src/utils/math/physics/constraints/coneTwist.cpp new file mode 100644 index 00000000..152f8b33 --- /dev/null +++ b/engine/src/utils/math/physics/constraints/coneTwist.cpp @@ -0,0 +1,201 @@ +#include +#include +#include + +#include + +void impl::solveConeTwistConstraint( pod::Constraint& constraint, float dt ) { + auto& a = *constraint.a; + auto& b = *constraint.b; + auto& joint = constraint.coneTwist; + + impl::solveBallSocketConstraint( constraint, dt ); + + auto tA = impl::getTransform( a ); + auto tB = impl::getTransform( b ); + + pod::Vector3f worldTwistAxisA = uf::quaternion::rotate( tA.orientation, joint.localTwistAxisA ); + pod::Vector3f worldTwistAxisB = uf::quaternion::rotate( tB.orientation, joint.localTwistAxisB ); + + pod::Vector3f worldRefA = uf::quaternion::rotate( tA.orientation, joint.localReferenceAxisA ); + pod::Vector3f worldRefB = uf::quaternion::rotate( tB.orientation, joint.localReferenceAxisB ); + + auto invIa = impl::computeWorldInverseInertia(a); + auto invIb = impl::computeWorldInverseInertia(b); + auto relAngularVel = b.angularVelocity - a.angularVelocity; + + // swing + pod::Vector3f swingAxis = uf::vector::cross( worldTwistAxisA, worldTwistAxisB ); + float swingLength2 = uf::vector::magnitude( 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( worldTwistAxisA, worldTwistAxisB ); + swingAngle = std::atan2( swingLength2, dot ); + + if ( swingAngle > joint.swingLimit ) { + swingError = swingAngle - joint.swingLimit; + swingActive = true; + } else { + joint.accumulatedAngularImpulse.x = 0.0f; + } + } + + // twist + pod::Vector3f twistAxis = worldTwistAxisA; + float proj = uf::vector::dot( worldRefB, twistAxis ); + pod::Vector3f refB_projected = worldRefB - (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( worldRefA, refB_projected ); + float sinTheta = uf::vector::dot( crossRef, twistAxis ); + float cosTheta = uf::vector::dot( worldRefA, 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 ) { + float kVal = 0.0f; + if ( !a.isStatic ) kVal += uf::vector::dot(axes[i], uf::matrix::multiply(invIa, axes[j])); + if ( !b.isStatic ) kVal += uf::vector::dot(axes[i], uf::matrix::multiply(invIb, axes[j])); + K(i, j) = kVal; + } + } + + 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; + + pod::Vector3f impulseVector = (swingAxis * swingDelta) + (twistAxis * twistDelta); + if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply(invIa, impulseVector); + if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply(invIb, impulseVector); + + return; + } + } + + // swing fallback: solve 1D + if ( swingActive ) { + float invMassN = 0.0f; + if ( !a.isStatic ) invMassN += uf::vector::dot(swingAxis, uf::matrix::multiply(invIa, swingAxis)); + if ( !b.isStatic ) invMassN += uf::vector::dot(swingAxis, uf::matrix::multiply(invIb, swingAxis)); + + if ( invMassN > EPS ) { + float bias = (uf::physics::settings.baumgarteCorrectionPercent / dt) * swingError; + float j = -(uf::vector::dot(relAngularVel, swingAxis) + bias) / invMassN; + + float jOld = joint.accumulatedAngularImpulse.x; + float jNew = std::min(0.0f, jOld + j); + joint.accumulatedAngularImpulse.x = jNew; + float jDelta = jNew - jOld; + + pod::Vector3f impulseVector = swingAxis * jDelta; + if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply(invIa, impulseVector); + if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply(invIb, impulseVector); + } + } + + // twist fallback: solve 1D + if ( twistActive ) { + float invMassN = 0.0f; + if ( !a.isStatic ) invMassN += uf::vector::dot(twistAxis, uf::matrix::multiply(invIa, twistAxis)); + if ( !b.isStatic ) invMassN += uf::vector::dot(twistAxis, uf::matrix::multiply(invIb, twistAxis)); + + if ( invMassN > EPS ) { + float bias = (uf::physics::settings.baumgarteCorrectionPercent / dt) * twistError; + float j = -(uf::vector::dot(relAngularVel, twistAxis) + bias) / invMassN; + + float jOld = joint.accumulatedAngularImpulse.y; + float jNew = jOld + j; + if ( twistError > 0.0f ) jNew = std::min( 0.0f, jNew ); + else jNew = std::max( 0.0f, jNew ); + + joint.accumulatedAngularImpulse.y = jNew; + float jDelta = jNew - jOld; + + pod::Vector3f impulseVector = twistAxis * jDelta; + if ( !a.isStatic ) a.angularVelocity -= uf::matrix::multiply(invIa, impulseVector); + if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply(invIb, impulseVector); + } + } +} + +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(), b.getComponent(), 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(), b.getComponent(), joint, axis, swingLimit, twistLimit ); +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/constraints/contact.cpp b/engine/src/utils/math/physics/constraints/contact.cpp new file mode 100644 index 00000000..bacfba96 --- /dev/null +++ b/engine/src/utils/math/physics/constraints/contact.cpp @@ -0,0 +1,354 @@ +#include +#include +#include +#include +#include + +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, 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, 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 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& cache, const uf::stl::vector& islands, const uf::stl::vector& 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& manifolds, uf::stl::unordered_map& 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& 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& 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 ); +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/constraints/hinge.cpp b/engine/src/utils/math/physics/constraints/hinge.cpp new file mode 100644 index 00000000..e78d1b81 --- /dev/null +++ b/engine/src/utils/math/physics/constraints/hinge.cpp @@ -0,0 +1,87 @@ +#include +#include +#include + +#include + +void impl::solveHingeConstraint( pod::Constraint& constraint, float dt ) { + auto& a = *constraint.a; + auto& b = *constraint.b; + auto& joint = constraint.hinge; + + auto tA = impl::getTransform( a ); + auto tB = impl::getTransform( b ); + + // solve linearly first + impl::solveBallSocketConstraint( constraint, dt ); + + auto worldAxisA = uf::quaternion::rotate( tA.orientation, joint.localAxisA ); + auto worldAxisB = uf::quaternion::rotate( tB.orientation, joint.localAxisB ); + + auto angularError = uf::vector::cross( worldAxisA, worldAxisB ); + + pod::Vector3f tangents[2]; + tangents[0] = impl::computeTangent( worldAxisA ); + tangents[1] = uf::vector::cross( worldAxisA, tangents[0] ); + + auto invIa = impl::computeWorldInverseInertia(a); + auto invIb = impl::computeWorldInverseInertia(b); + + auto relAngularVel = b.angularVelocity - a.angularVelocity; + + // effective mass matrix + pod::Matrix2f K = {}; + for ( auto i = 0; i < 2; ++i ) { + for ( auto j = 0; j < 2; ++j ) { + float kVal = 0.0f; + if ( !a.isStatic ) kVal += uf::vector::dot(tangents[i], uf::matrix::multiply(invIa, tangents[j])); + if ( !b.isStatic ) kVal += uf::vector::dot(tangents[i], uf::matrix::multiply(invIb, tangents[j])); + + K(i, j) = kVal; + } + } + + // impulse matrix + pod::Matrix2f Kinv = uf::matrix::inverse(K); + // bias + 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])); + }); + // solve and apply + 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( invIa, j ); + if ( !b.isStatic ) b.angularVelocity += uf::matrix::multiply( invIb, j ); + } +} + +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(), b.getComponent(), 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(), b.getComponent(), joint, axis ); +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/impl.cpp b/engine/src/utils/math/physics/impl.cpp index e7866131..ad81ed01 100644 --- a/engine/src/utils/math/physics/impl.cpp +++ b/engine/src/utils/math/physics/impl.cpp @@ -1,11 +1,13 @@ #include -#include #include #include #include #include #include +#include + +#include #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, 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(); +} // 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(); - auto& scene = uf::scene::getCurrentScene(); - auto& world = scene.getComponent(); - 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(); - 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(); - 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(); - 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(); - 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(); - 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(); - return create( world, object, mesh, mass, offset, convex ); -} void uf::physics::destroy( uf::Object& object ) { if ( !object.hasComponent() ) 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(), b.getComponent() ); +} +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(), b.getComponent() ); } -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, 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() ); } #if UF_PHYSICS_TEST diff --git a/engine/src/utils/math/physics/integration.cpp b/engine/src/utils/math/physics/integration.cpp index 5078152a..5aeb2c17 100644 --- a/engine/src/utils/math/physics/integration.cpp +++ b/engine/src/utils/math/physics/integration.cpp @@ -52,6 +52,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 +132,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, 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, 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 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& cache, const uf::stl::vector& islands, const uf::stl::vector& 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& manifolds, uf::stl::unordered_map& 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& 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; diff --git a/engine/src/utils/math/physics/narrowphase/aabb.cpp b/engine/src/utils/math/physics/narrowphase/aabb.cpp index d8edf2ea..ff18c8d0 100644 --- a/engine/src/utils/math/physics/narrowphase/aabb.cpp +++ b/engine/src/utils/math/physics/narrowphase/aabb.cpp @@ -96,4 +96,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 ); } \ No newline at end of file diff --git a/engine/src/utils/math/physics/narrowphase/capsule.cpp b/engine/src/utils/math/physics/narrowphase/capsule.cpp index e9cf88e5..fa61aabf 100644 --- a/engine/src/utils/math/physics/narrowphase/capsule.cpp +++ b/engine/src/utils/math/physics/narrowphase/capsule.cpp @@ -126,4 +126,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 ); } \ No newline at end of file diff --git a/engine/src/utils/math/physics/narrowphase/mesh.cpp b/engine/src/utils/math/physics/narrowphase/mesh.cpp index bbc8d852..ca16559b 100644 --- a/engine/src/utils/math/physics/narrowphase/mesh.cpp +++ b/engine/src/utils/math/physics/narrowphase/mesh.cpp @@ -201,4 +201,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 ); } \ No newline at end of file diff --git a/engine/src/utils/math/physics/narrowphase/obb.cpp b/engine/src/utils/math/physics/narrowphase/obb.cpp index 9fb3f290..9cd9af24 100644 --- a/engine/src/utils/math/physics/narrowphase/obb.cpp +++ b/engine/src/utils/math/physics/narrowphase/obb.cpp @@ -256,15 +256,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 +320,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 ); } \ No newline at end of file diff --git a/engine/src/utils/math/physics/narrowphase/plane.cpp b/engine/src/utils/math/physics/narrowphase/plane.cpp index 978777b9..7da05edd 100644 --- a/engine/src/utils/math/physics/narrowphase/plane.cpp +++ b/engine/src/utils/math/physics/narrowphase/plane.cpp @@ -84,4 +84,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 ); } \ No newline at end of file diff --git a/engine/src/utils/math/physics/narrowphase/ray.cpp b/engine/src/utils/math/physics/narrowphase/ray.cpp index 28f9eec3..6d68a4ef 100644 --- a/engine/src/utils/math/physics/narrowphase/ray.cpp +++ b/engine/src/utils/math/physics/narrowphase/ray.cpp @@ -304,4 +304,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, 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; } \ No newline at end of file diff --git a/engine/src/utils/math/physics/narrowphase/sphere.cpp b/engine/src/utils/math/physics/narrowphase/sphere.cpp index 2d6c0974..728719e8 100644 --- a/engine/src/utils/math/physics/narrowphase/sphere.cpp +++ b/engine/src/utils/math/physics/narrowphase/sphere.cpp @@ -97,4 +97,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 ); } \ No newline at end of file diff --git a/engine/src/utils/math/physics/narrowphase/triangle.cpp b/engine/src/utils/math/physics/narrowphase/triangle.cpp index 7a346ccf..fab59713 100644 --- a/engine/src/utils/math/physics/narrowphase/triangle.cpp +++ b/engine/src/utils/math/physics/narrowphase/triangle.cpp @@ -94,7 +94,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 +152,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 +365,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 ); } \ No newline at end of file diff --git a/engine/src/utils/math/physics/solvers/block.cpp b/engine/src/utils/math/physics/solvers/block.cpp index 5a11982f..c873a1cf 100644 --- a/engine/src/utils/math/physics/solvers/block.cpp +++ b/engine/src/utils/math/physics/solvers/block.cpp @@ -113,10 +113,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 +126,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 +138,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 +159,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 ); } } diff --git a/engine/src/utils/math/physics/solvers/iterativeImpulse.cpp b/engine/src/utils/math/physics/solvers/iterativeImpulse.cpp index ae0cfd91..c16ba7c4 100644 --- a/engine/src/utils/math/physics/solvers/iterativeImpulse.cpp +++ b/engine/src/utils/math/physics/solvers/iterativeImpulse.cpp @@ -20,15 +20,8 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod if ( velAlongNormal < -1.0f ) 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 +32,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 +43,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 ); } } \ No newline at end of file diff --git a/engine/src/utils/math/physics/solvers.cpp b/engine/src/utils/math/physics/solvers/ngs.cpp similarity index 74% rename from engine/src/utils/math/physics/solvers.cpp rename to engine/src/utils/math/physics/solvers/ngs.cpp index 09fe7468..b6cdb9de 100644 --- a/engine/src/utils/math/physics/solvers.cpp +++ b/engine/src/utils/math/physics/solvers/ngs.cpp @@ -2,17 +2,6 @@ #include #include -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& 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& manifolds, float dt, uint32_t iterations ) { if ( !uf::physics::settings.ngsPositionSolver ) return; if ( uf::physics::settings.baumgarteCorrectionPercent <= 0 ) return; @@ -52,7 +41,7 @@ void impl::solvePositions( uf::stl::vector& manifolds, float dt, 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); @@ -69,7 +58,7 @@ void impl::solvePositions( uf::stl::vector& manifolds, float dt, 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); diff --git a/engine/src/utils/math/physics/tests.inl b/engine/src/utils/math/physics/tests.inl index 41ea9258..7863ef76 100644 --- a/engine/src/utils/math/physics/tests.inl +++ b/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,197 @@ 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 ); + } }) \ No newline at end of file