ecker/engine
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

more neurotic code cleanup, added joint constraints

Browse Source
This commit is contained in:
ecker 2026-05-21 20:42:54 -05:00
parent 7bd4b00514
commit 7758389c6c
41 changed files with 1903 additions and 959 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
engine/inc/uf/utils/math/physics/broadphase/island.h
View File

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

12
engine/inc/uf/utils/math/physics/constraints.h Normal file
View File

@ -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 );
}

16
engine/inc/uf/utils/math/physics/constraints/ballSocket.h Normal file
View File

@ -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& );
}
}

16
engine/inc/uf/utils/math/physics/constraints/coneTwist.h Normal file
View File

@ -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 );
}
}

22
engine/inc/uf/utils/math/physics/constraints/contact.h Normal file
View File

@ -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<size_t, pod::Manifold>& cache, const uf::stl::vector<pod::Island>& islands, const uf::stl::vector<pod::PhysicsBody*>& bodies );
void updateManifoldCache( const uf::stl::vector<pod::Manifold>& manifolds, uf::stl::unordered_map<size_t, pod::Manifold>& cache );
void pruneManifoldCache( uf::stl::unordered_map<size_t, pod::Manifold>& cache );
void warmupContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& c, float dt );
void warmupManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Manifold& manifold, float dt );
void resolveManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
void solveContacts( uf::stl::vector<pod::Manifold>& manifolds, float dt );
}

16
engine/inc/uf/utils/math/physics/constraints/hinge.h Normal file
View File

@ -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& );
}
}

330
engine/inc/uf/utils/math/physics/impl.h
View File

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

53
engine/inc/uf/utils/math/physics/integration.h
View File

@ -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<float>::max(), float maxLimit = std::numeric_limits<float>::max()
);
void applyImpulseTo(
pod::PhysicsBody& a, pod::PhysicsBody& b,
const pod::Vector3f& rA, const pod::Vector3f& rB,
const pod::Vector3f& impulse,
pod::Vector3f& accumulatedImpulse,
float minLimit = -std::numeric_limits<float>::max(), float maxLimit = std::numeric_limits<float>::max()
);
void applyPseudoImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& impulse );
void applyPseudoImpulseTo(
pod::PhysicsBody& a, pod::PhysicsBody& b,
const pod::Vector3f& rA, const pod::Vector3f& rB,
const pod::Vector3f& direction, float magnitude,
float& accumulatedImpulse,
float minLimit = -std::numeric_limits<float>::max(), float maxLimit = std::numeric_limits<float>::max()
);
void applyPseudoImpulseTo(
pod::PhysicsBody& a, pod::PhysicsBody& b,
const pod::Vector3f& rA, const pod::Vector3f& rB,
const pod::Vector3f& impulse,
pod::Vector3f& accumulatedImpulse,
float minLimit = -std::numeric_limits<float>::max(), float maxLimit = std::numeric_limits<float>::max()
);
float accumulateImpulseTo(
float magnitude, float& accumulatedImpulse,
float minLimit = -std::numeric_limits<float>::max(), float maxLimit = std::numeric_limits<float>::max()
);
pod::Vector3f accumulateImpulseTo(
const pod::Vector3f& impulse, pod::Vector3f& accumulatedImpulse,
float minLimit = -std::numeric_limits<float>::max(), float maxLimit = std::numeric_limits<float>::max()
);
void applyRollingResistance( pod::PhysicsBody& body, float dt );
void bindManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt = 0 );
bool generateContactsGjk( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
bool generateContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
void computeLocalContacts( pod::Manifold& manifold );
bool similarContact( const pod::Contact& a, const pod::Contact& b, float distSqThreshold = 1.0e-2f, float normThreshold = 0.9f );
void reduceContacts( pod::Manifold& manifold );
void mergeContacts( pod::Manifold& manifold );
void retrieveContacts( pod::Manifold& current, const pod::Manifold& previous, float distanceThreshold = 0.1f, float separationThreshold = 0.1f, float decay = 0.85f );
void prepareManifoldCache( uf::stl::unordered_map<size_t, pod::Manifold>& cache, const uf::stl::vector<pod::Island>& islands, const uf::stl::vector<pod::PhysicsBody*>& bodies );
void updateManifoldCache( const uf::stl::vector<pod::Manifold>& manifolds, uf::stl::unordered_map<size_t, pod::Manifold>& cache );
void pruneManifoldCache( uf::stl::unordered_map<size_t, pod::Manifold>& cache );
void warmupContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& c, float dt );
void warmupManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Manifold& manifold, float dt );
void snapVelocity( pod::PhysicsBody& body, float dt, float threshold = 0.01f );
void positionCorrection( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& contact );
void integrate( pod::PhysicsBody& body, float dt );
}

7
engine/inc/uf/utils/math/physics/narrowphase/aabb.h
View File

@ -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& = {} );
}
}

7
engine/inc/uf/utils/math/physics/narrowphase/capsule.h
View File

@ -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& = {} );
}
}

7
engine/inc/uf/utils/math/physics/narrowphase/mesh.h
View File

@ -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 );
}
}

7
engine/inc/uf/utils/math/physics/narrowphase/obb.h
View File

@ -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& = {} );
}
}

7
engine/inc/uf/utils/math/physics/narrowphase/plane.h
View File

@ -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& = {} );
}
}

8
engine/inc/uf/utils/math/physics/narrowphase/ray.h
View File

@ -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 );
}
}

7
engine/inc/uf/utils/math/physics/narrowphase/sphere.h
View File

@ -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& = {} );
}
}

7
engine/inc/uf/utils/math/physics/narrowphase/triangle.h
View File

@ -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& = {} );
}
}

4
engine/inc/uf/utils/math/physics/solvers.h
View File

@ -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<pod::Manifold>& manifolds, float dt );
void solvePositions( uf::stl::vector<pod::Manifold>& manifolds, float dt, uint32_t iterations = 4 );
}

376
engine/inc/uf/utils/math/physics/structs.h Normal file
View File

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

3
engine/inc/uf/utils/math/vector.h
View File

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

25
engine/inc/uf/utils/math/vector/pod.inl
View File

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

36
engine/src/utils/math/physics/broadphase/island.cpp
View File

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

30
engine/src/utils/math/physics/common.cpp
View File

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

17
engine/src/utils/math/physics/constraints.cpp Normal file
View File

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

86
engine/src/utils/math/physics/constraints/ballSocket.cpp Normal file
View File

@ -0,0 +1,86 @@
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/constraints.h>
void impl::solveBallSocketConstraint( pod::Constraint& constraint, float dt ) {
auto& 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<pod::PhysicsBody>(), b.getComponent<pod::PhysicsBody>(), joint );
}
pod::Constraint& uf::physics::constrain( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& joint ) {
return constrain( uf::physics::getWorld(), a, b, joint );
}
pod::Constraint& uf::physics::constrain( uf::Object& a, uf::Object& b, const pod::Vector3f& joint ) {
return constrain( uf::physics::getWorld(), a.getComponent<pod::PhysicsBody>(), b.getComponent<pod::PhysicsBody>(), joint );
}

201
engine/src/utils/math/physics/constraints/coneTwist.cpp Normal file
View File

@ -0,0 +1,201 @@
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/constraints.h>
#include <uf/engine/scene/scene.h>
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<pod::PhysicsBody>(), b.getComponent<pod::PhysicsBody>(), joint, axis, swingLimit, twistLimit );
}
pod::Constraint& uf::physics::constrain( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& joint, const pod::Vector3f& axis, float swingLimit, float twistLimit ) {
return constrain( uf::physics::getWorld(), a, b, joint, axis, swingLimit, twistLimit );
}
pod::Constraint& uf::physics::constrain( uf::Object& a, uf::Object& b, const pod::Vector3f& joint, const pod::Vector3f& axis, float swingLimit, float twistLimit ) {
return constrain( uf::physics::getWorld(), a.getComponent<pod::PhysicsBody>(), b.getComponent<pod::PhysicsBody>(), joint, axis, swingLimit, twistLimit );
}

354
engine/src/utils/math/physics/constraints/contact.cpp Normal file
View File

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

87
engine/src/utils/math/physics/constraints/hinge.cpp Normal file
View File

@ -0,0 +1,87 @@
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/constraints.h>
#include <uf/engine/scene/scene.h>
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<pod::PhysicsBody>(), b.getComponent<pod::PhysicsBody>(), joint, axis );
}
pod::Constraint& uf::physics::constrain( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& joint, const pod::Vector3f& axis ) {
return constrain( uf::physics::getWorld(), a, b, joint, axis );
}
pod::Constraint& uf::physics::constrain( uf::Object& a, uf::Object& b, const pod::Vector3f& joint, const pod::Vector3f& axis ) {
return constrain( uf::physics::getWorld(), a.getComponent<pod::PhysicsBody>(), b.getComponent<pod::PhysicsBody>(), joint, axis );
}

204
engine/src/utils/math/physics/impl.cpp
View File

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

442
engine/src/utils/math/physics/integration.cpp
View File

@ -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<pod::Contact>, result);
result.reserve(4);
for ( auto& c : manifold.points ) {
if ( !uf::vector::isValid(c.point) ) continue;
bool merged = false;
for ( auto& r : result ) {
if ( !impl::similarContact(c, r) ) continue;
if ( c.penetration > r.penetration ) r = c;
merged = true;
break;
}
if ( !merged ) {
if ( result.size() < 4 ) {
result.emplace_back(c);
} else {
auto weakest = 0;
for ( auto i = 1; i < 4; i++ ) {
if ( result[i].penetration < result[weakest].penetration ) weakest = i;
}
if ( c.penetration > result[weakest].penetration ) result[weakest] = c;
}
}
}
manifold.points = result;
#endif
}
void impl::mergeContacts( pod::Manifold& manifold ) {
STATIC_THREAD_LOCAL(uf::stl::vector<pod::Contact>, result);
result.reserve(4);
for ( auto& c : manifold.points ) {
bool merged = false;
for ( auto& r : result ) {
if ( !impl::similarContact( c, r ) ) continue;
// merge: average position + normal, keep max penetration
r.point = ( r.point + c.point ) * 0.5f;
r.normal = uf::vector::normalize( r.normal + c.normal );
r.penetration = std::max( r.penetration, c.penetration );
merged = true;
break;
}
if ( !merged ) result.emplace_back( c );
}
manifold.points = result;
}
void impl::retrieveContacts( pod::Manifold& current, const pod::Manifold& previous, float distanceThreshold, float separationThreshold, float decay ) {
auto& a = *current.a;
auto& b = *current.b;
auto tA = impl::getTransform( a );
auto tB = impl::getTransform( b );
uf::stl::vector<pod::Contact> merged = current.points;
float distSqThresh = distanceThreshold * distanceThreshold;
for ( const auto& oldContact : previous.points ) {
// reproject point according to current transform
auto worldA = uf::transform::apply( tA, oldContact.localA );
auto worldB = uf::transform::apply( tB, oldContact.localB );
auto delta = worldB - worldA;
auto normal = current.points.empty() ? oldContact.normal : current.points[0].normal;
float penetration = -uf::vector::dot( delta, normal );
if ( penetration < -separationThreshold ) continue;
pod::Vector3f projectedDelta = delta + normal * penetration;
float tangentialDriftSq = uf::vector::dot( projectedDelta, projectedDelta );
if ( tangentialDriftSq > distSqThresh ) continue;
pod::Contact validContact = oldContact;
validContact.point = (worldA + worldB) * 0.5f;
validContact.normal = normal;
validContact.penetration = penetration;
++validContact.lifetime;
validContact.accumulatedNormalImpulse *= decay;
validContact.accumulatedTangentImpulse *= decay;
bool isDuplicate = false;
for ( auto& c : merged ) {
if ( impl::similarContact( validContact, c ) ) {
c.accumulatedNormalImpulse = validContact.accumulatedNormalImpulse;
c.accumulatedTangentImpulse = validContact.accumulatedTangentImpulse;
c.lifetime = validContact.lifetime;
isDuplicate = true;
break;
}
}
if ( !isDuplicate ) merged.emplace_back( validContact );
}
current.points = merged;
}
void impl::prepareManifoldCache( uf::stl::unordered_map<size_t, pod::Manifold>& cache, const uf::stl::vector<pod::Island>& islands, const uf::stl::vector<pod::PhysicsBody*>& bodies ) {
for ( const auto& island : islands ) {
for ( const auto& pair : island.pairs ) {
auto& a = *bodies[pair.first];
auto& b = *bodies[pair.second];
cache[ impl::makePairKey( a, b ) ];
}
}
}
void impl::updateManifoldCache( const uf::stl::vector<pod::Manifold>& manifolds, uf::stl::unordered_map<size_t, pod::Manifold>& cache ) {
for ( const auto& m : manifolds ) {
auto it = cache.find( impl::makePairKey( *m.a, *m.b ) );
if ( it == cache.end() ) continue; // assert
it->second = m;
}
}
void impl::pruneManifoldCache( uf::stl::unordered_map<size_t, pod::Manifold>& cache ) {
auto cacheLifetime = uf::physics::settings.manifoldCacheLifetime;
if ( !cacheLifetime ) {
cacheLifetime = MAX(1, uf::physics::settings.substeps) * 2;
}
for ( auto itCache = cache.begin(); itCache != cache.end(); ) {
auto& manifold = itCache->second;
// prune points that are too old
for ( auto it = manifold.points.begin(); it != manifold.points.end(); ) {
if ( it->lifetime > cacheLifetime ) it = manifold.points.erase(it);
else ++it;
}
// empty manifold, kill it
if ( manifold.points.empty() ) itCache = cache.erase(itCache);
else ++itCache;
}
}
void impl::warmupContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& c, float dt ) {
if ( !c.lifetime ) return; // too new
// build relative offsets
pod::Vector3f rA = c.point - impl::getPosition( a );
pod::Vector3f rB = c.point - impl::getPosition( b );
// normal impulse
pod::Vector3f Pn = c.normal * c.accumulatedNormalImpulse;
impl::applyImpulseTo( a, b, rA, rB, Pn );
// tangent basis
pod::Vector3f Pt = c.tangent * c.accumulatedTangentImpulse;
impl::applyImpulseTo( a, b, rA, rB, Pt );
}
void impl::warmupManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Manifold& manifold, float dt ) {
for ( auto& contact : manifold.points ) {
impl::warmupContacts( a, b, contact, dt );
}
}
// snap velocity for grounded bodies
void impl::snapVelocity( pod::PhysicsBody& body, float dt, float threshold ) {
if ( !body.activity.grounded || !body.activity.awake ) return;
float thresholdSq = threshold * threshold;
float threshold2 = threshold * threshold;
// snap velocity if body is grounded and nearly still
float linSpeedSq = uf::vector::magnitude( body.velocity );
float angSpeedSq = uf::vector::magnitude( body.angularVelocity );
float linSpeed2 = uf::vector::magnitude( body.velocity );
float angSpeed2 = uf::vector::magnitude( body.angularVelocity );
// cancel out vertical component
if ( fabs(body.velocity.y) < thresholdSq ) body.velocity.y = 0.0f;
if ( fabs(body.velocity.y) < threshold2 ) body.velocity.y = 0.0f;
// cancel out velocity entirely
if ( linSpeedSq < thresholdSq ) body.velocity = {};
if ( linSpeed2 < threshold2 ) body.velocity = {};
// cancel out rotational velocity entirely
if ( angSpeedSq < thresholdSq ) body.angularVelocity = {};
if ( angSpeed2 < threshold2 ) body.angularVelocity = {};
}
// baumgarte position correction
void impl::positionCorrection( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& contact ) {
if ( uf::physics::settings.baumgarteCorrectionPercent <= 0 ) return;
if ( a.isStatic && b.isStatic ) return;
// penetration depth beyond slop
float penetration = std::max( contact.penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f );
if ( penetration <= 0.0f ) return;
// compute correction magnitude
float invMassA = ( a.isStatic ? 0.0f : a.inverseMass );
float invMassB = ( b.isStatic ? 0.0f : b.inverseMass );
float totalInvMass = invMassA + invMassB;
if ( totalInvMass <= EPS ) return;
// apply correction vector
pod::Vector3f correction = contact.normal * (penetration / totalInvMass) * uf::physics::settings.baumgarteCorrectionPercent;
if ( !a.isStatic ) a.transform->position -= correction * invMassA;
if ( !b.isStatic ) b.transform->position += correction * invMassB;
}
void impl::integrate( pod::PhysicsBody& body, float dt ) {
// only integrate awake and dynamic bodies
if ( !body.activity.awake || body.isStatic || body.mass == 0 ) return;

13
engine/src/utils/math/physics/narrowphase/aabb.cpp
View File

@ -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 );
}

13
engine/src/utils/math/physics/narrowphase/capsule.cpp
View File

@ -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 );
}

26
engine/src/utils/math/physics/narrowphase/mesh.cpp
View File

@ -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 );
}

25
engine/src/utils/math/physics/narrowphase/obb.cpp
View File

@ -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 );
}

13
engine/src/utils/math/physics/narrowphase/plane.cpp
View File

@ -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 );
}

40
engine/src/utils/math/physics/narrowphase/ray.cpp
View File

@ -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<pod::BVH::index_t>, candidates);
impl::queryBVH( dynamicBvh, ray, candidates );
if ( uf::physics::settings.useSplitBvhs ) impl::queryBVH( staticBvh, ray, candidates );
for ( auto i : candidates ) {
auto* b = bodies[i];
if ( body == b ) continue;
switch ( b->collider.type ) {
case pod::ShapeType::AABB: impl::rayAabb( ray, *b, rayHit ); break;
case pod::ShapeType::OBB: impl::rayObb( ray, *b, rayHit ); break;
case pod::ShapeType::SPHERE: impl::raySphere( ray, *b, rayHit ); break;
case pod::ShapeType::PLANE: impl::rayPlane( ray, *b, rayHit ); break;
case pod::ShapeType::CAPSULE: impl::rayCapsule( ray, *b, rayHit ); break;
case pod::ShapeType::MESH: impl::rayMesh( ray, *b, rayHit ); break;
case pod::ShapeType::CONVEX_HULL: impl::rayHull( ray, *b, rayHit ); break;
}
}
if ( uf::physics::settings.debugDraw ) impl::drawRay( ray, rayHit );
return rayHit;
}

13
engine/src/utils/math/physics/narrowphase/sphere.cpp
View File

@ -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 );
}

23
engine/src/utils/math/physics/narrowphase/triangle.cpp
View File

@ -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 );
}

29
engine/src/utils/math/physics/solvers/block.cpp
View File

@ -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 );
}
}

40
engine/src/utils/math/physics/solvers/iterativeImpulse.cpp
View File

@ -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 );
}
}

15
engine/src/utils/math/physics/solvers.cpp → engine/src/utils/math/physics/solvers/ngs.cpp
View File

@ -2,17 +2,6 @@
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/solvers.h>
void impl::resolveManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
if ( uf::physics::settings.blockContactSolver ) {
if ( impl::blockSolver( a, b, manifold, dt ) ) return;
}
for ( auto& contact : manifold.points ) impl::iterativeImpulseSolver( a, b, contact, dt );
}
void impl::solveContacts( uf::stl::vector<pod::Manifold>& manifolds, float dt ) {
if ( uf::physics::settings.warmupSolver ) for ( auto& manifold : manifolds ) impl::warmupManifold( *manifold.a, *manifold.b, manifold, dt );
for ( auto i = 0; i < uf::physics::settings.solverIterations; ++i ) for ( auto& manifold : manifolds ) impl::resolveManifold( *manifold.a, *manifold.b, manifold, dt );
}
void impl::solvePositions( uf::stl::vector<pod::Manifold>& manifolds, float dt, uint32_t iterations ) {
if ( !uf::physics::settings.ngsPositionSolver ) return;
if ( uf::physics::settings.baumgarteCorrectionPercent <= 0 ) return;
@ -52,7 +41,7 @@ void impl::solvePositions( uf::stl::vector<pod::Manifold>& 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<pod::Manifold>& 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);

223
engine/src/utils/math/physics/tests.inl
View File

@ -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 );
}
})