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

Compare commits

base: ecker:a8ad6d0fe497df5a547235f04e5d156065bef01b
Branches Tags
ecker:master
ecker:bundle
...
compare: ecker:7bd4b00514941532e9a0fba9566cc297518d1e9d
Branches Tags
ecker:master
ecker:bundle

3 Commits
a8ad6d0fe4 ... 7bd4b00514

Author SHA1 Message Date
ecker
7bd4b00514 NGS position solver fixed because of a pesky minus sign 2026-05-20 22:32:34 -05:00
ecker
9097aebc92 lots of physics cleanup and fixes (split-impulse performs better than a NGS position correction somehow?) 2026-05-20 20:59:47 -05:00
ecker
098281ad81 removed PGS solver since it is the iterative impulse solver, fixed returning problem of boxes sinking, some other things 2026-05-20 15:02:29 -05:00
28 changed files with 791 additions and 565 deletions
Show Stats Expand all files Collapse all files

21
bin/data/config.json
View File

@ -336,17 +336,24 @@
"discord": { "enabled": false }
},
"physics": {
"warmup solver": true,
"block solver": true,
"pgs solver": true,
"correction percent": 0.1,
"gjk": true,
"solvers": {
"warmup": true,
"block": true,
"resolve invalid": true,
"iterations": 10,
"gjk": false
},
"correction": {
"ngs": true,
"percent": 0.2,
"slop": 0.01, // 0.005
"max": 0.01 // 0.2
},
"debug draw": {
"dynamic": true
},
"fixed step": true,
"substeps": 4,
"solver iterations": 10
"substeps": 4
},
"audio": {
"mute": false,

2
bin/data/entities/ball.json
View File

@ -4,7 +4,7 @@
"SoundEmitterBehavior"
],
"transform": {
"position": [ 0, 3, 0 ],
"position": [ -0.574743, 2.3547, -5.05161 ],
"rotation": {
"axis": [ 0, 1, 0 ],
"angle": 0

2
bin/data/entities/physics_prop.json
View File

@ -8,7 +8,7 @@
"physics": {
"mass": 100,
// "inertia": false,
"type": "bounding box"
"type": "obb"
// "type": "mesh"
// "type": "hull"
}

4
bin/data/entities/scripts/door.lua
View File

@ -111,9 +111,9 @@ ent:addHook( "entity:Use.%UID%", function( payload )
local delta = transform.position - userTransform.position
local side = normal:dot(delta)
if side > 0 then
polarity = 1
elseif side < 0 then
polarity = -1
elseif side < 0 then
polarity = 1
end
end
elseif state == 2 --[[or state == 1]] then

2
bin/data/entities/scripts/player.lua
View File

@ -25,7 +25,7 @@ local heldObject = {
uid = 0,
distance = 0,
smoothSpeed = 4,
scrollSpeed = 16,
scrollSpeed = 32,
momentum = Vector3f(0,0,0),
rotate = false,
}

4
bin/data/scenes/sourceengine/mds_mcdonalds.json
View File

@ -3,7 +3,7 @@
"assets": [
// { "filename": "./models/mds_mcdonalds.glb" }
{ "filename": "./models/mds_mcdonalds/graph.json" }
// ,{ "filename": "/ball.json", "delay": 1 }
,{ "filename": "/ball.json", "delay": 1 }
],
"metadata": {
"graph": {
@ -15,7 +15,7 @@
"func_door_rotating_5568": { "action": "load", "payload": { "import": "/door.json", "metadata": { "angle":-1.570795, "normal": [1,0,0] } } },
"func_door_rotating_5584": { "action": "load", "payload": { "import": "/door.json", "metadata": { "angle":-1.570795, "normal": [1,0,0] } } },
//"prop_physics_override_5813": { "action": "load", "payload": { "import": "/physics_prop.json" } },
"prop_physics_override_5813": { "action": "load", "payload": { "import": "/physics_prop.json" } },
// regex matches
"/^prop_physics_[^o]/": { "action": "load", "payload": { "import": "/prop.json" } },

11
engine/inc/uf/utils/math/physics/common.h
View File

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

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

@ -200,25 +200,25 @@ namespace pod {
float inverseMass = 1.0f; // for fast division
int32_t viewIndex = -1; // -1 means it's not an aliased view
/*alignas(16)*/ pod::Vector3f offset = {};
pod::Vector3f offset = {};
/*alignas(16)*/ pod::Vector3f velocity = {};
/*alignas(16)*/ pod::Vector3f pseudoVelocity = {};
/*alignas(16)*/ pod::Vector3f forceAccumulator = {};
pod::Vector3f velocity = {};
pod::Vector3f forceAccumulator = {};
/*alignas(16)*/ pod::Vector3f angularVelocity = {};
/*alignas(16)*/ pod::Vector3f pseudoAngularVelocity = {};
/*alignas(16)*/ pod::Vector3f torqueAccumulator = {};
pod::Vector3f angularVelocity = {};
pod::Vector3f torqueAccumulator = {};
/*alignas(16)*/ pod::Vector3f inertiaTensor = { 1, 1, 1 };
/*alignas(16)*/ pod::Vector3f inverseInertiaTensor = { 1, 1, 1 };
pod::Vector3f pseudoVelocity = {};
pod::Vector3f pseudoAngularVelocity = {};
/*alignas(16)*/ pod::Vector3f gravity = { NAN, NAN, NAN }; // an invalid gravity will fallback to world gravity
pod::Vector3f inverseInertiaTensor = { 1, 1, 1 };
/*alignas(16)*/ pod::AABB bounds;
/*alignas(16)*/ pod::Collider collider;
/*alignas(16)*/ pod::PhysicsMaterial material;
/*alignas(16)*/ pod::Activity activity;
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 {
@ -231,7 +231,7 @@ namespace pod {
// warm-start cached values
int lifetime = 0;
pod::Vector3f tangent = {};
pod::Vector3f tangent;
float accumulatedNormalImpulse = 0.0f;
float accumulatedTangentImpulse = 0.0f;
float accumulatedPseudoImpulse = 0.0f;
@ -260,9 +260,9 @@ namespace pod {
};
struct PhysicsSettings {
bool warmupSolver = false; // cache manifold data to warm up the solver
bool blockContactSolver = false; // use BlockNxN solvers (where N = number of contacts for a manifold)
bool pgsContactSolver = true; // use PGS contact solver
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
@ -287,11 +287,13 @@ namespace pod {
// to-do: find possibly better values for this
uint32_t solverIterations = 10;
float baumgarteCorrectionPercent = 0.01f;
float baumgarteCorrectionSlop = 0.01f;
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 = 6; // to-do: find a good value for this
uint32_t manifoldCacheLifetime = 0; // 0 = derive from current settings
uint32_t frameCounter = 0;
@ -371,6 +373,7 @@ namespace uf {
pod::PhysicsBody& UF_API create( uf::Object&, float mass = 0.0f, const pod::Vector3f& = {} );
pod::PhysicsBody& UF_API create( uf::Object& object, const pod::AABB& aabb, float mass = 0.0f, const pod::Vector3f& = {} );
pod::PhysicsBody& UF_API create( uf::Object& object, const pod::OBB& obb, float mass = 0.0f, const pod::Vector3f& = {} );
pod::PhysicsBody& UF_API create( uf::Object& object, const pod::Sphere& sphere, float mass = 0.0f, const pod::Vector3f& = {} );
pod::PhysicsBody& UF_API create( uf::Object& object, const pod::Plane& plane, float mass = 0.0f, const pod::Vector3f& = {} );
pod::PhysicsBody& UF_API create( uf::Object& object, const pod::Capsule& capsule, float mass = 0.0f, const pod::Vector3f& = {} );
@ -378,6 +381,7 @@ namespace uf {
pod::PhysicsBody& UF_API create( pod::World&, uf::Object&, float mass = 0.0f, const pod::Vector3f& = {} );
pod::PhysicsBody& UF_API create( pod::World&, uf::Object& object, const pod::AABB& aabb, float mass = 0.0f, const pod::Vector3f& = {} );
pod::PhysicsBody& UF_API create( pod::World&, uf::Object& object, const pod::OBB& obb, float mass = 0.0f, const pod::Vector3f& = {} );
pod::PhysicsBody& UF_API create( pod::World&, uf::Object& object, const pod::Sphere& sphere, float mass = 0.0f, const pod::Vector3f& = {} );
pod::PhysicsBody& UF_API create( pod::World&, uf::Object& object, const pod::Plane& plane, float mass = 0.0f, const pod::Vector3f& = {} );
pod::PhysicsBody& UF_API create( pod::World&, uf::Object& object, const pod::Capsule& capsule, float mass = 0.0f, const pod::Vector3f& = {} );

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

@ -4,10 +4,9 @@
#include "solvers/block.h"
#include "solvers/iterativeImpulse.h"
#include "solvers/pgs.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 = 2 );
void solvePositions( uf::stl::vector<pod::Manifold>& manifolds, float dt, uint32_t iterations = 4 );
}

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

@ -1,7 +0,0 @@
#pragma once
#include "../impl.h"
namespace impl {
void blockPGSSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
}

7
engine/inc/uf/utils/math/shapes.h
View File

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

23
engine/src/engine/ext/ext.cpp
View File

@ -201,17 +201,26 @@ void UF_API uf::load( ext::json::Value& json ) {
// Physics settings
{
auto& configEnginePhysicsJson = json["engine"]["physics"];
uf::physics::settings.warmupSolver = configEnginePhysicsJson["warmup solver"].as(uf::physics::settings.warmupSolver);
uf::physics::settings.blockContactSolver = configEnginePhysicsJson["block solver"].as(uf::physics::settings.blockContactSolver);
uf::physics::settings.pgsContactSolver = configEnginePhysicsJson["pgs solver"].as(uf::physics::settings.pgsContactSolver);
uf::physics::settings.useGjk = configEnginePhysicsJson["gjk"].as(uf::physics::settings.useGjk);
uf::physics::settings.async = configEnginePhysicsJson["async"].as(uf::physics::settings.async);
uf::physics::settings.timestep = configEnginePhysicsJson["timestep"].as(uf::physics::settings.timestep);
uf::physics::settings.fixedStep = configEnginePhysicsJson["fixed step"].as(uf::physics::settings.fixedStep);
uf::physics::settings.substeps = configEnginePhysicsJson["substeps"].as(uf::physics::settings.substeps);
uf::physics::settings.solverIterations = configEnginePhysicsJson["solver iterations"].as(uf::physics::settings.solverIterations);
uf::physics::settings.baumgarteCorrectionPercent = configEnginePhysicsJson["correction percent"].as(uf::physics::settings.baumgarteCorrectionPercent);
auto& configEnginePhysicsSolverJson = configEnginePhysicsJson["solvers"];
if ( ext::json::isObject( configEnginePhysicsSolverJson ) ) {
uf::physics::settings.useGjk = configEnginePhysicsSolverJson["gjk"].as(uf::physics::settings.useGjk);
uf::physics::settings.blockContactSolver = configEnginePhysicsSolverJson["block"].as(uf::physics::settings.blockContactSolver);
uf::physics::settings.warmupSolver = configEnginePhysicsSolverJson["warmup"].as(uf::physics::settings.warmupSolver);
uf::physics::settings.resolveBlockContact = configEnginePhysicsSolverJson["resolve invalid"].as(uf::physics::settings.resolveBlockContact);
uf::physics::settings.solverIterations = configEnginePhysicsSolverJson["iterations"].as(uf::physics::settings.solverIterations);
}
auto& configEnginePhysicsCorrectionJson = configEnginePhysicsJson["correction"];
if ( ext::json::isObject( configEnginePhysicsCorrectionJson ) ) {
uf::physics::settings.ngsPositionSolver = configEnginePhysicsCorrectionJson["ngs"].as(uf::physics::settings.ngsPositionSolver);
uf::physics::settings.baumgarteCorrectionPercent = configEnginePhysicsCorrectionJson["percent"].as(uf::physics::settings.baumgarteCorrectionPercent);
uf::physics::settings.baumgarteCorrectionSlop = configEnginePhysicsCorrectionJson["slop"].as(uf::physics::settings.baumgarteCorrectionSlop);
uf::physics::settings.maxLinearCorrection = configEnginePhysicsCorrectionJson["max"].as(uf::physics::settings.maxLinearCorrection);
}
auto& configEnginePhysicsDebugDrawJson = configEnginePhysicsJson["debug draw"];
if ( ext::json::isObject( configEnginePhysicsDebugDrawJson ) ) {
if ( configEnginePhysicsDebugDrawJson["static"].as<bool>() ) uf::physics::settings.debugDraw |= pod::Collider::CATEGORY_STATIC;

8
engine/src/engine/graph/graph.cpp
View File

@ -1321,10 +1321,10 @@ void uf::graph::process( pod::Graph& graph, int32_t index, uf::Object& parent )
auto max = bounds.max; // uf::matrix::multiply<float>( model, bounds.max, 1.0f );
pod::Vector3f center = (max + min) * 0.5f;
pod::Vector3f corner = uf::vector::abs(max - min) * 0.5f;
pod::Vector3f extent = uf::vector::abs(max - min) * 0.5f;
if ( ext::json::isNull( metadataJson["physics"]["center"] ) ) metadataJson["physics"]["center"] = uf::vector::encode( center );
if ( ext::json::isNull( metadataJson["physics"]["corner"] ) ) metadataJson["physics"]["corner"] = uf::vector::encode( corner );
if ( ext::json::isNull( metadataJson["physics"]["extent"] ) ) metadataJson["physics"]["extent"] = uf::vector::encode( extent );
if ( ext::json::isNull( metadataJson["physics"]["min"] ) ) metadataJson["physics"]["min"] = uf::vector::encode( min );
if ( ext::json::isNull( metadataJson["physics"]["max"] ) ) metadataJson["physics"]["max"] = uf::vector::encode( max );
}
@ -1335,7 +1335,7 @@ void uf::graph::process( pod::Graph& graph, int32_t index, uf::Object& parent )
physicsBody.material.staticFriction = phyziks["friction"].as(physicsBody.material.staticFriction);
physicsBody.material.restitution = phyziks["restitution"].as(physicsBody.material.restitution);
physicsBody.inertiaTensor = uf::vector::decode( phyziks["inertia"], physicsBody.inertiaTensor );
physicsBody.inverseInertiaTensor = uf::vector::decode( phyziks["inertia"], physicsBody.inverseInertiaTensor );
physicsBody.gravity = uf::vector::decode( phyziks["gravity"], physicsBody.gravity );
auto center = uf::vector::decode( phyziks["center"], pod::Vector3f{} );
@ -2007,7 +2007,7 @@ void uf::graph::reload( pod::Graph& graph, pod::Node& node ) {
physicsBody.material.staticFriction = phyziks["friction"].as(physicsBody.material.staticFriction);
physicsBody.material.restitution = phyziks["restitution"].as(physicsBody.material.restitution);
physicsBody.inertiaTensor = uf::vector::decode( phyziks["inertia"], physicsBody.inertiaTensor );
physicsBody.inverseInertiaTensor = uf::vector::decode( phyziks["inertia"], physicsBody.inverseInertiaTensor );
physicsBody.gravity = uf::vector::decode( phyziks["gravity"], physicsBody.gravity );
auto center = uf::vector::decode( phyziks["center"], pod::Vector3f{} );

16
engine/src/engine/object/behavior.cpp
View File

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

2
engine/src/ext/lua/usertypes/physics.cpp
View File

@ -30,7 +30,7 @@ namespace binds {
}
std::tuple<uf::Object*, float> rayCast( pod::PhysicsBody& self, const pod::Vector3f& center, const pod::Vector3f& direction ) {
pod::RayQuery query = uf::physics::rayCast( pod::Ray{center, direction}, self, uf::vector::norm( direction ) );
pod::RayQuery query = uf::physics::rayCast( pod::Ray{center, uf::vector::normalize( direction )}, self, uf::vector::norm( direction ) );
uf::Object* object = query.hit ? query.body->object : NULL;
float depth = query.hit ? query.contact.penetration : -1;
return std::make_tuple( object, depth );

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

@ -85,6 +85,10 @@ pod::PhysicsBody impl::physicsBodyTriView( const pod::TriangleWithNormal triangl
pod::PhysicsBody view = body;
view.collider.type = pod::ShapeType::TRIANGLE;
view.collider.triangle = triangle;
// calculate normal if needed
if ( uf::vector::magnitude( view.collider.triangle.normal ) < 0.001f ) {
view.collider.triangle.normal = impl::triangleNormal( (const pod::Triangle&) triangle );
}
// assume triangle is already transformed
view.offset = {};
view.transform = NULL;
@ -382,7 +386,7 @@ float impl::segmentTriangleDistanceSq( const pod::Vector3f& p0, const pod::Vecto
}
// Separating Axis Theorem test
bool impl::testSeparatingAxis( const pod::Triangle& triangle, const pod::AABB& box, const pod::Vector3f& axis, const pod::Vector3f axes[3], float& outMinOverlap, pod::Vector3f& outBestAxis ) {
bool impl::testSeparatingAxis( const pod::Triangle& triangle, const pod::OBB& box, const pod::Vector3f& axis, const pod::Vector3f axes[3], float& outMinOverlap, pod::Vector3f& outBestAxis ) {
float mag = uf::vector::magnitude(axis);
if ( mag < EPS2 ) return true;
pod::Vector3f n = axis / mag;
@ -395,12 +399,8 @@ bool impl::testSeparatingAxis( const pod::Triangle& triangle, const pod::AABB& b
float maxT = std::max( { p0, p1, p2 } );
// project box
pod::Vector3f cB = impl::aabbCenter( box );
pod::Vector3f eB = impl::aabbExtent( box );
float pB = uf::vector::dot(cB, n);
float rB = eB.x * std::fabs(uf::vector::dot(axes[0], n)) +
eB.y * std::fabs(uf::vector::dot(axes[1], n)) +
eB.z * std::fabs(uf::vector::dot(axes[2], n));
float pB = uf::vector::dot( box.center, n );
float rB = impl::projectExtents( box, n, axes );
float minB = pB - rB;
float maxB = pB + rB;
@ -416,6 +416,28 @@ bool impl::testSeparatingAxis( const pod::Triangle& triangle, const pod::AABB& b
}
return true;
}
bool impl::testSeparatingAxis( const pod::OBB& boxA, const pod::OBB& boxB, const pod::Vector3f axesA[3], const pod::Vector3f axesB[3], const pod::Vector3f& axis, float& outMinOverlap, pod::Vector3f& outBestAxis ) {
float mag = uf::vector::magnitude(axis);
if ( mag < EPS2 ) return true;
pod::Vector3f n = axis / mag;
float pA = uf::vector::dot( boxA.center, n );
float rA = impl::projectExtents( boxA, n, axesA );
float pB = uf::vector::dot( boxB.center, n );
float rB = impl::projectExtents( boxB, n, axesB );
float dist = std::fabs( pB - pA );
float overlap = ( rA + rB ) - dist;
if ( overlap < 0.0f ) return false;
if ( overlap < outMinOverlap ) {
outMinOverlap = overlap;
outBestAxis = n;
}
return true;
}
// Sutherland-Hodgman polygon clipping
void impl::clipPolygon( pod::Vector3f* poly, int& polyCount, const pod::Plane& plane ) {
@ -475,6 +497,7 @@ pod::Vector3f impl::triangleNormal( const pod::Triangle& tri ) {
return uf::vector::normalize(uf::vector::cross(tri.points[1] - tri.points[0], tri.points[2] - tri.points[0]));
}
pod::Vector3f impl::triangleNormal( const pod::TriangleWithNormal& tri ) {
if ( uf::vector::magnitude( tri.normal ) < 0.001f ) return impl::triangleNormal( (const pod::Triangle&) tri );
return tri.normal;
}
// mesh accessing
@ -572,19 +595,19 @@ pod::TriangleWithNormal impl::fetchTriangle( const uf::Mesh& mesh, size_t triID,
auto transform = impl::getTransform( body );
if ( body.collider.type == pod::ShapeType::MESH ) {
if ( body.collider.type == pod::ShapeType::MESH || body.collider.type == pod::ShapeType::CONVEX_HULL ) {
FOR_EACH(3, {
tri.points[i] = uf::transform::apply( transform, tri.points[i] );
});
tri.normal = uf::quaternion::rotate( transform.orientation, tri.normal );
}
else {
#if REORIENT_NORMALS_ON_FETCH
else {
auto triCenter = impl::triangleCenter( tri );
auto delta = impl::getPosition( body ) - triCenter;
if ( uf::vector::dot(tri.normal, delta) < 0.0f ) tri.normal = -tri.normal;
#endif
}
#endif
return tri;
}
@ -648,29 +671,63 @@ pod::Vector3f impl::aabbCenter( const pod::AABB& aabb ) {
pod::Vector3f impl::aabbExtent( const pod::AABB& aabb ) {
return ( aabb.max - aabb.min ) * 0.5f;
}
// returns the min bound of an OBB
pod::Vector3f impl::obbMin( const pod::OBB& obb ) {
return obb.center - obb.extent;
}
// returns the max bound of an OBB
pod::Vector3f impl::obbMax( const pod::OBB& obb ) {
return obb.center + obb.extent;
}
// converts a min-max AABB to center-extents OBB
pod::OBB impl::aabbToObb( const pod::AABB& aabb ) {
return pod::OBB{
.center = impl::aabbCenter( aabb ),
.extent = impl::aabbExtent( aabb ),
};
}
// converts a center-extents OBB to min-max AABB
pod::AABB impl::obbToAabb( const pod::OBB& obb ) {
return pod::AABB{
.min = impl::obbMin( obb ),
.max = impl::obbMax( obb ),
};
}
// returns AABB axes
void impl::boxAxes( pod::Vector3f axes[3] ) {
axes[0] = {1,0,0};
axes[1] = {0,1,0};
axes[2] = {0,0,1};
}
// returns OBB axes
void impl::boxAxes( pod::Vector3f axes[3], const pod::Transform<>& transform ) {
axes[0] = uf::quaternion::rotate(transform.orientation, pod::Vector3f{1,0,0});
axes[1] = uf::quaternion::rotate(transform.orientation, pod::Vector3f{0,1,0});
axes[2] = uf::quaternion::rotate(transform.orientation, pod::Vector3f{0,0,1});
}
// computes a box's extents from given axes
pod::Vector3f impl::extentFromAxes( const pod::OBB& box, const pod::Vector3f axes[3] ) {
return ( uf::vector::abs(axes[0]) * box.extent.x ) + ( uf::vector::abs(axes[1]) * box.extent.y ) + ( uf::vector::abs(axes[2]) * box.extent.z );
}
//
float impl::projectExtents( const pod::OBB& box, const pod::Vector3f& normal, const pod::Vector3f axes[3] ) {
return uf::vector::dot(box.extent, uf::vector::abs( pod::Vector3f{
uf::vector::dot(axes[0], normal),
uf::vector::dot(axes[1], normal),
uf::vector::dot(axes[2], normal)
} ) );
// return box.extent.x * std::fabs(uf::vector::dot(axes[0], normal)) + box.extent.y * std::fabs(uf::vector::dot(axes[1], normal)) + box.extent.z * std::fabs(uf::vector::dot(axes[2], normal));
}
// transforms an AABB into world-space
pod::AABB impl::transformAabbToWorld( const pod::AABB& aabb, const pod::Transform<>& transform ) {
// to-do: flatten, since transform might not be flattened (even though getTransform does that)
const auto& q = transform.orientation;
const auto& p = transform.position;
auto box = impl::aabbToObb( aabb );
pod::Vector3f axes[3];
impl::boxAxes( axes, transform );
pod::Vector3f cB = impl::aabbCenter( aabb );
pod::Vector3f eB = impl::aabbExtent( aabb );
pod::Vector3f axes[] = {
uf::quaternion::rotate(q, pod::Vector3f{1,0,0}),
uf::quaternion::rotate(q, pod::Vector3f{0,1,0}),
uf::quaternion::rotate(q, pod::Vector3f{0,0,1}),
};
pod::Vector3f center = uf::quaternion::rotate(transform.orientation, box.center) + transform.position;
pod::Vector3f extent = impl::extentFromAxes( box, axes );
pod::Vector3f cW = uf::quaternion::rotate(q, cB) + p;
// to-do: a cleaner way of doing this with uf::vector::abs
pod::Vector3f cE = {
fabs(axes[0].x) * eB.x + fabs(axes[1].x) * eB.y + fabs(axes[2].x) * eB.z,
fabs(axes[0].y) * eB.x + fabs(axes[1].y) * eB.y + fabs(axes[2].y) * eB.z,
fabs(axes[0].z) * eB.x + fabs(axes[1].z) * eB.y + fabs(axes[2].z) * eB.z
};
return { cW - cE, cW + cE };
return { center - extent, center + extent };
}
// returns the line segment of a capsule
std::pair<pod::Vector3f, pod::Vector3f> impl::getCapsuleSegment( const pod::PhysicsBody& body ) {
@ -687,15 +744,11 @@ std::pair<pod::Vector3f, pod::Vector3f> impl::getCapsuleSegment( const pod::Phys
pod::AABB impl::computeAABB( const pod::PhysicsBody& body ) {
const auto transform = impl::getTransform( body );
switch ( body.collider.type ) {
case pod::ShapeType::AABB:
case pod::ShapeType::OBB: {
case pod::ShapeType::AABB: {
return impl::transformAabbToWorld( body.collider.aabb, transform );
/*
return {
transform.position + body.collider.aabb.min,
transform.position + body.collider.aabb.max,
};
*/
} break;
case pod::ShapeType::OBB: {
return impl::transformAabbToWorld( impl::obbToAabb( body.collider.obb ), transform );
} break;
case pod::ShapeType::SPHERE: {
return {

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

@ -190,7 +190,7 @@ void uf::physics::step( pod::World& world, float dt ) {
// pass manifolds to solver
impl::solveContacts( manifolds, dt );
// do position correction
//impl::solvePositions( manifolds, dt );
impl::solvePositions( manifolds, dt );
// cache manifold positions
if ( uf::physics::settings.warmupSolver ) {
impl::updateManifoldCache( manifolds, uf::physics::settings.manifoldsCache );
@ -259,24 +259,30 @@ pod::Vector3f uf::physics::getGravity( pod::PhysicsBody& body ) {
void uf::physics::updateInertia( pod::PhysicsBody& body ) {
if ( body.isStatic || body.mass <= 0 ) {
body.inertiaTensor = { FLT_MAX, FLT_MAX, FLT_MAX };
body.inverseInertiaTensor = { 0.0f, 0.0f, 0.0f };
return;
}
pod::Vector3f inertiaTensor = {};
switch ( body.collider.type ) {
case pod::ShapeType::AABB:
case pod::ShapeType::OBB: {
case pod::ShapeType::AABB: {
pod::Vector3f dims = (body.collider.aabb.max - body.collider.aabb.min);
pod::Vector3f dimsSq = dims * dims;
body.inertiaTensor = pod::Vector3f{ dimsSq.y + dimsSq.z, dimsSq.x + dimsSq.z, dimsSq.x + dimsSq.y } * (body.mass / 12.0f);
body.inertiaTensor = uf::vector::max( body.inertiaTensor, { EPS, EPS, EPS } );
body.inverseInertiaTensor = 1.0f / body.inertiaTensor;
inertiaTensor = pod::Vector3f{ dimsSq.y + dimsSq.z, dimsSq.x + dimsSq.z, dimsSq.x + dimsSq.y } * (body.mass / 12.0f);
inertiaTensor = uf::vector::max( inertiaTensor, { EPS, EPS, EPS } );
body.inverseInertiaTensor = 1.0f / inertiaTensor;
} break;
case pod::ShapeType::OBB: {
pod::Vector3f dims = body.collider.obb.extent * 2.0f;
pod::Vector3f dimsSq = dims * dims;
inertiaTensor = pod::Vector3f{ dimsSq.y + dimsSq.z, dimsSq.x + dimsSq.z, dimsSq.x + dimsSq.y } * (body.mass / 12.0f);
inertiaTensor = uf::vector::max( inertiaTensor, { EPS, EPS, EPS } );
body.inverseInertiaTensor = 1.0f / inertiaTensor;
} break;
case pod::ShapeType::SPHERE: {
float I = 0.4f * body.mass * body.collider.sphere.radius * body.collider.sphere.radius;
float invI = 1.0f / I;
body.inertiaTensor = { I, I, I };
inertiaTensor = { I, I, I };
body.inverseInertiaTensor = { invI, invI, invI };
} break;
case pod::ShapeType::CAPSULE: {
@ -288,7 +294,7 @@ void uf::physics::updateInertia( pod::PhysicsBody& body ) {
float Iyy = 0.5f * m * r * r;
float Izz = Ixx;
body.inertiaTensor = { Ixx, Iyy, Izz };
inertiaTensor = { Ixx, Iyy, Izz };
body.inverseInertiaTensor = { 1.0f/Ixx, 1.0f/Iyy, 1.0f/Izz };
} break;
case pod::ShapeType::MESH:
@ -298,9 +304,9 @@ void uf::physics::updateInertia( pod::PhysicsBody& body ) {
#if 1
pod::Vector3f dims = (body.bounds.max - body.bounds.min);
pod::Vector3f dimsSq = dims * dims;
body.inertiaTensor = pod::Vector3f{ dimsSq.y + dimsSq.z, dimsSq.x + dimsSq.z, dimsSq.x + dimsSq.y } * (body.mass / 12.0f);
body.inertiaTensor = uf::vector::max( body.inertiaTensor, { EPS, EPS, EPS } );
body.inverseInertiaTensor = 1.0f / body.inertiaTensor;
inertiaTensor = pod::Vector3f{ dimsSq.y + dimsSq.z, dimsSq.x + dimsSq.z, dimsSq.x + dimsSq.y } * (body.mass / 12.0f);
inertiaTensor = uf::vector::max( inertiaTensor, { EPS, EPS, EPS } );
body.inverseInertiaTensor = 1.0f / inertiaTensor;
#else
pod::Matrix3f inertia = {};
float totalVolume = 0.0f;
@ -315,7 +321,7 @@ void uf::physics::updateInertia( pod::PhysicsBody& body ) {
}
if ( totalVolume < EPS ) {
body.inertiaTensor = { FLT_MAX, FLT_MAX, FLT_MAX };
inertiaTensor = { FLT_MAX, FLT_MAX, FLT_MAX };
body.inverseInertiaTensor = { 0.0f, 0.0f, 0.0f };
} else {
// accumulate inertia
@ -347,8 +353,8 @@ void uf::physics::updateInertia( pod::PhysicsBody& body ) {
inertia += Ibox + pat;
}
body.inertiaTensor = { inertia(0,0), inertia(1,1), inertia(2,2) };
body.inverseInertiaTensor = 1.0f / body.inertiaTensor;
inertiaTensor = { inertia(0,0), inertia(1,1), inertia(2,2) };
body.inverseInertiaTensor = 1.0f / inertiaTensor;
}
#endif
} break;
@ -464,14 +470,22 @@ pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, fl
}
pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, const pod::AABB& aabb, float mass, const pod::Vector3f& offset ) {
auto& body = uf::physics::create( world, object, mass, offset );
//body.collider.type = pod::ShapeType::AABB;
body.collider.type = pod::ShapeType::OBB;
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;
@ -500,6 +514,9 @@ pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, co
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;
@ -541,6 +558,11 @@ pod::PhysicsBody& uf::physics::create( uf::Object& object, const pod::AABB& aabb
auto& world = scene.getComponent<pod::World>();
return create( world, object, aabb, mass, offset );
}
pod::PhysicsBody& uf::physics::create( uf::Object& object, const pod::OBB& obb, float mass, const pod::Vector3f& offset ) {
auto& scene = uf::scene::getCurrentScene();
auto& world = scene.getComponent<pod::World>();
return create( world, object, obb, mass, offset );
}
pod::PhysicsBody& uf::physics::create( uf::Object& object, const pod::Sphere& sphere, float mass, const pod::Vector3f& offset ) {
auto& scene = uf::scene::getCurrentScene();
auto& world = scene.getComponent<pod::World>();

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

@ -23,9 +23,8 @@ float impl::computeEffectiveMass( pod::PhysicsBody& a, pod::PhysicsBody& b, cons
angularTermB = uf::vector::dot(uf::vector::cross(I_crossB, rB), n);
}
float result = inverseMass + angularTermA + angularTermB;
if ( result < EPS ) result = 1.0f; // prevent divide by zero
return result;
// to-do: assert / handle result == 0 to avoid division by zero (this probably only would happen with two static bodies colliding, which never should happen)
return inverseMass + angularTermA + angularTermB;
}
void impl::applyImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& impulse ) {
@ -40,7 +39,6 @@ void impl::applyImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::
b.angularVelocity += uf::matrix::multiply( invIb, uf::vector::cross(rB, impulse) );
}
}
void impl::applyPseudoImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& impulse ) {
if ( !a.isStatic ) {
a.pseudoVelocity -= impulse * a.inverseMass;
@ -176,6 +174,70 @@ bool impl::similarContact( const pod::Contact& a, const pod::Contact& b, float d
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);
@ -203,6 +265,7 @@ void impl::reduceContacts( pod::Manifold& manifold ) {
}
manifold.points = result;
#endif
}
void impl::mergeContacts( pod::Manifold& manifold ) {
@ -258,14 +321,12 @@ void impl::retrieveContacts( pod::Manifold& current, const pod::Manifold& previo
validContact.accumulatedNormalImpulse *= decay;
validContact.accumulatedTangentImpulse *= decay;
validContact.accumulatedPseudoImpulse = 0.0f;
bool isDuplicate = false;
for ( auto& c : current.points ) {
for ( auto& c : merged ) {
if ( impl::similarContact( validContact, c ) ) {
c.accumulatedNormalImpulse = validContact.accumulatedNormalImpulse;
c.accumulatedTangentImpulse = validContact.accumulatedTangentImpulse;
c.accumulatedPseudoImpulse = validContact.accumulatedPseudoImpulse;
c.lifetime = validContact.lifetime;
isDuplicate = true;
break;
@ -298,12 +359,16 @@ void impl::updateManifoldCache( const uf::stl::vector<pod::Manifold>& manifolds,
}
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 > uf::physics::settings.manifoldCacheLifetime ) it = manifold.points.erase(it);
if ( it->lifetime > cacheLifetime ) it = manifold.points.erase(it);
else ++it;
}
@ -404,15 +469,18 @@ void impl::integrate( pod::PhysicsBody& body, float dt ) {
uf::transform::rotate( *body.transform/*.reference*/, dq );
}
// split impulse updates
{
// pseudo-impulse position correction
if ( !uf::physics::settings.ngsPositionSolver ) {
body.transform->position += body.pseudoVelocity * dt;
float pseudoAngularSpeed2 = uf::vector::magnitude( body.pseudoAngularVelocity );
if ( pseudoAngularSpeed2 > EPS ) {
float pseudoAngularSpeed = std::sqrt( pseudoAngularSpeed2 );
pod::Quaternion<> dq = uf::quaternion::axisAngle( body.pseudoAngularVelocity / pseudoAngularSpeed, pseudoAngularSpeed * dt );
uf::transform::rotate( *body.transform/*.reference*/, dq );
pod::Vector3f axis = body.pseudoAngularVelocity / pseudoAngularSpeed;
float clampedSpeed = std::min(pseudoAngularSpeed, (2.0f * M_PI / 180.0f) / dt);
pod::Quaternion<> dq = uf::quaternion::axisAngle( axis, clampedSpeed * dt );
uf::transform::rotate( *body.transform, dq );
}
// reset

59
engine/src/utils/math/physics/narrowphase/epa.cpp
View File

@ -50,11 +50,10 @@ void impl::getSupportFace( const pod::PhysicsBody& body, const pod::Vector3f& di
outPoly[i] = hasTransform ? uf::transform::apply( transform, body.collider.triangle.points[i] ) : body.collider.triangle.points[i];
});
} break;
case pod::ShapeType::AABB:
case pod::ShapeType::OBB: {
case pod::ShapeType::AABB: {
outCount = 4;
pod::Vector3f n = localDir;
pod::Vector3f absN = { std::fabs(n.x), std::fabs(n.y), std::fabs(n.z) };
pod::Vector3f absN = uf::vector::abs( n );
pod::Vector3f min = body.collider.aabb.min;
pod::Vector3f max = body.collider.aabb.max;
@ -85,6 +84,60 @@ void impl::getSupportFace( const pod::PhysicsBody& body, const pod::Vector3f& di
outPoly[i] = uf::transform::apply(transform, outPoly[i]);
});
} break;
case pod::ShapeType::OBB: {
outCount = 4;
pod::Vector3f n = localDir;
pod::Vector3f absN = uf::vector::abs( n );
pod::Vector3f min = body.collider.obb.center - body.collider.obb.extent;
pod::Vector3f max = body.collider.obb.center + body.collider.obb.extent;
// dominant axis
if ( absN.x > absN.y && absN.x > absN.z ) {
float x = (n.x > 0) ? max.x : min.x;
outPoly[0] = {x, min.y, max.z};
outPoly[1] = {x, min.y, min.z};
outPoly[2] = {x, max.y, min.z};
outPoly[3] = {x, max.y, max.z};
if ( n.x < 0 ) std::swap(outPoly[1], outPoly[3]);
} else if ( absN.y > absN.z ) {
float y = (n.y > 0) ? max.y : min.y;
outPoly[0] = {max.x, y, max.z};
outPoly[1] = {max.x, y, min.z};
outPoly[2] = {min.x, y, min.z};
outPoly[3] = {min.x, y, max.z};
if ( n.y < 0 ) std::swap(outPoly[1], outPoly[3]);
} else {
float z = (n.z > 0) ? max.z : min.z;
outPoly[0] = {min.x, max.y, z};
outPoly[1] = {min.x, min.y, z};
outPoly[2] = {max.x, min.y, z};
outPoly[3] = {max.x, max.y, z};
if (n.z < 0) std::swap(outPoly[1], outPoly[3]);
}
FOR_EACH(4, {
outPoly[i] = uf::transform::apply(transform, outPoly[i]);
});
} break;
case pod::ShapeType::SPHERE: {
outCount = 1;
outPoly[0] = transform.position + uf::vector::normalize( dir ) * body.collider.sphere.radius;
} break;
case pod::ShapeType::CAPSULE: {
auto up = uf::quaternion::rotate( transform.orientation, pod::Vector3f{0,1,0} );
auto p1 = transform.position + up * body.collider.capsule.halfHeight;
auto p2 = transform.position - up * body.collider.capsule.halfHeight;
if ( std::fabs( uf::vector::dot( dir, up ) ) < 0.01f ) {
outCount = 2;
pod::Vector3f offset = uf::vector::normalize( dir ) * body.collider.capsule.radius;
outPoly[0] = p1 + offset;
outPoly[1] = p2 + offset;
} else {
outCount = 1;
auto end = ( uf::vector::dot( dir, p1 ) > uf::vector::dot( dir, p2 ) ) ? p1 : p2;
outPoly[0] = end + uf::vector::normalize( dir ) * body.collider.capsule.radius;
}
} break;
case pod::ShapeType::MESH:
case pod::ShapeType::CONVEX_HULL: {
if ( !body.collider.convexHull.mesh ) return;

7
engine/src/utils/math/physics/narrowphase/gjk.cpp
View File

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

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

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

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

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

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

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

73
engine/src/utils/math/physics/solvers.cpp
View File

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

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

@ -10,8 +10,8 @@ namespace impl {
float invMassA = ( a.isStatic ? 0.0f : a.inverseMass );
float invMassB = ( b.isStatic ? 0.0f : b.inverseMass );
pod::Matrix3f invIa = computeWorldInverseInertia( a );
pod::Matrix3f invIb = computeWorldInverseInertia( b );
pod::Matrix3f invIa = impl::computeWorldInverseInertia( a );
pod::Matrix3f invIb = impl::computeWorldInverseInertia( b );
auto pA = impl::getPosition( a, true );
auto pB = impl::getPosition( b, true );
@ -45,11 +45,8 @@ namespace impl {
K(i,i) += 1e-3f;
}
pod::Vector<T,N> rhsVel = {};
pod::Vector<T,N> K_lambdaVel = {};
pod::Vector<T,N> rhsPos = {};
pod::Vector<T,N> K_lambdaPos = {};
pod::Vector<T,N> rhs = {};
pod::Vector<T,N> K_lambda = {};
for ( auto i = 0; i < N; i++ ) {
auto& contact = manifold.points[i];
@ -61,32 +58,19 @@ namespace impl {
float e = std::min(a.material.restitution, b.material.restitution);
float restitutionBias = (vRel < -1.0f) ? -e * vRel : 0.0f;
rhsVel[i] = -vRel + restitutionBias;
pod::Vector3f pseudoVa = a.pseudoVelocity + uf::vector::cross( a.pseudoAngularVelocity, contact.point - pA );
pod::Vector3f pseudoVb = b.pseudoVelocity + uf::vector::cross( b.pseudoAngularVelocity, contact.point - pB );
float pseudoVRel = uf::vector::dot((pseudoVb - pseudoVa), contact.normal);
float penetrationBias = std::max(contact.penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f) * (uf::physics::settings.baumgarteCorrectionPercent / dt);
rhsPos[i] = -pseudoVRel + penetrationBias;
K_lambdaVel[i] = contact.accumulatedNormalImpulse;
K_lambdaPos[i] = contact.accumulatedPseudoImpulse;
rhs[i] = -vRel + restitutionBias;
K_lambda[i] = contact.accumulatedNormalImpulse;
}
pod::Matrix<T,N> Kinv = uf::matrix::inverse( K );
pod::Vector<T,N> residualVel = rhsVel - uf::matrix::multiply( K, K_lambdaVel );
pod::Vector<T,N> dLambdaVel = uf::matrix::multiply( Kinv, residualVel );
pod::Vector<T,N> residualPos = rhsPos - uf::matrix::multiply( K, K_lambdaPos );
pod::Vector<T,N> dLambdaPos = uf::matrix::multiply( Kinv, residualPos );
pod::Vector<T,N> residual = rhs - uf::matrix::multiply( K, K_lambda );
pod::Vector<T,N> dLambda = uf::matrix::multiply( Kinv, residual );
// check if contacts are all valid
int invalidContactIndex = -1;
for ( auto i = 0; i < N; i++ ) {
if ( K_lambdaVel[i] + dLambdaVel[i] < 0.0f || K_lambdaPos[i] + dLambdaPos[i] < 0.0f ) {
if ( K_lambda[i] + dLambda[i] < 0.0f ) {
invalidContactIndex = i;
break;
}
@ -95,11 +79,18 @@ namespace impl {
if ( invalidContactIndex != -1 ) {
bool success = false;
// reduce the manifold
if ( N > 1 ) {
if ( uf::physics::settings.resolveBlockContact && N > 1 ) {
#if 1
pod::Manifold reducedManifold = manifold;
reducedManifold.points.erase( reducedManifold.points.begin() + invalidContactIndex );
// re-solve
success = impl::blockSolver( a, b, reducedManifold, dt );
// copy back to original manifold
if ( success ) manifold = reducedManifold;
#else
pod::Manifold reducedManifold = manifold;
reducedManifold.points.erase( reducedManifold.points.begin() + invalidContactIndex );
manifold.points[invalidContactIndex].accumulatedNormalImpulse = 0.0f;
manifold.points[invalidContactIndex].accumulatedPseudoImpulse = 0.0f;
manifold.points[invalidContactIndex].accumulatedTangentImpulse = 0.0f;
// re-solve
@ -110,6 +101,7 @@ namespace impl {
manifold.points[i] = reducedManifold.points[r++];
}
}
#endif
}
return success;
}
@ -119,45 +111,70 @@ namespace impl {
pod::Vector3f rA = manifold.points[i].point - pA;
pod::Vector3f rB = manifold.points[i].point - pB;
// real impulse
// normal impulse
{
float newLambdaVel = contact.accumulatedNormalImpulse + dLambdaVel[i];
dLambdaVel[i] = newLambdaVel - contact.accumulatedNormalImpulse;
contact.accumulatedNormalImpulse = newLambdaVel;
impl::applyImpulseTo( a, b, rA, rB, manifold.points[i].normal * dLambdaVel[i] );
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] );
}
// pseudo impulse
{
float newLambdaPos = contact.accumulatedPseudoImpulse + dLambdaPos[i];
dLambdaPos[i] = newLambdaPos - contact.accumulatedPseudoImpulse;
contact.accumulatedPseudoImpulse = newLambdaPos;
impl::applyPseudoImpulseTo( a, b, rA, rB, manifold.points[i].normal * dLambdaPos[i] );
if ( !uf::physics::settings.ngsPositionSolver ) {
float penetrationBias = std::max(contact.penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f) * (uf::physics::settings.baumgarteCorrectionPercent / dt);
penetrationBias = std::min(penetrationBias, uf::physics::settings.maxLinearCorrection / dt);
pod::Vector3f pseudoVa = a.pseudoVelocity + uf::vector::cross(a.pseudoAngularVelocity, rA);
pod::Vector3f pseudoVb = b.pseudoVelocity + uf::vector::cross(b.pseudoAngularVelocity, rB);
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);
}
// friction
// tangent friction
{
pod::Vector3f vA = a.velocity + uf::vector::cross( a.angularVelocity, rA );
pod::Vector3f vB = b.velocity + uf::vector::cross( b.angularVelocity, rB );
pod::Vector3f rv = vB - vA;
pod::Vector3f tangent = rv - contact.normal * uf::vector::dot(rv, contact.normal);
float tangentMag2 = uf::vector::magnitude(tangent);
if ( tangentMag2 > EPS2 ) {
tangent /= std::sqrt( tangentMag2 );
float invMassT = impl::computeEffectiveMass(a, b, rA, rB, tangent);
float vt = uf::vector::dot(rv, tangent);
float jt = -vt / invMassT;
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);
contact.accumulatedTangentImpulse = jtNew;
impl::applyImpulseTo(a, b, rA, rB, tangent * (jtNew - jtOld));
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 = 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 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;
}
float maxFriction = mu_s * normalForce;
/*
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);
}
}

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

@ -6,20 +6,20 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod
pod::Vector3f rA = contact.point - impl::getPosition( a, true );
pod::Vector3f rB = contact.point - impl::getPosition( b, true );
pod::Vector3f vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);
pod::Vector3f vB = b.velocity + uf::vector::cross(b.angularVelocity, rB);
pod::Vector3f rv = vB - vA;
float velAlongNormal = uf::vector::dot(rv, contact.normal);
float e = std::min(a.material.restitution, b.material.restitution);
float restitutionBias = 0.0f;
if ( velAlongNormal < -1.0f ) restitutionBias = -e * velAlongNormal;
float targetVelocity = restitutionBias;
float invMassN = impl::computeEffectiveMass(a, b, rA, rB, contact.normal);
// normal impulse
{
pod::Vector3f vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);
pod::Vector3f vB = b.velocity + uf::vector::cross(b.angularVelocity, rB);
pod::Vector3f rv = vB - vA;
float restitutionBias = 0.0f;
float e = std::min(a.material.restitution, b.material.restitution);
float velAlongNormal = uf::vector::dot(rv, contact.normal);
if ( velAlongNormal < -1.0f ) restitutionBias = -e * velAlongNormal;
float targetVelocity = restitutionBias;
float jn = (targetVelocity - velAlongNormal) / invMassN;
float jnOld = contact.accumulatedNormalImpulse;
@ -30,9 +30,10 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod
impl::applyImpulseTo(a, b, rA, rB, contact.normal * jn);
}
{
// pseudo impulse
if ( !uf::physics::settings.ngsPositionSolver ) {
float penetrationBias = std::max(contact.penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f) * (uf::physics::settings.baumgarteCorrectionPercent / dt);
penetrationBias = std::min(penetrationBias, 2.0f / dt);
penetrationBias = std::min(penetrationBias, uf::physics::settings.maxLinearCorrection / dt);
pod::Vector3f pseudoVa = a.pseudoVelocity + uf::vector::cross(a.pseudoAngularVelocity, rA);
pod::Vector3f pseudoVb = b.pseudoVelocity + uf::vector::cross(b.pseudoAngularVelocity, rB);
@ -47,14 +48,21 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod
impl::applyPseudoImpulseTo(a, b, rA, rB, contact.normal * jPseudo);
}
// tangent friction
{
pod::Vector3f vA = a.velocity + uf::vector::cross(a.angularVelocity, rA);
pod::Vector3f vB = b.velocity + uf::vector::cross(b.angularVelocity, rB);
pod::Vector3f rv = vB - vA;
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 = impl::computeTangent( contact.normal );
}
// tangent direction
pod::Vector3f tangent = rv - contact.normal * uf::vector::dot(rv, contact.normal);
float tangentMag2 = uf::vector::magnitude(tangent);
if ( tangentMag2 > EPS2 ) {
tangent /= std::sqrt( tangentMag2 );
float invMassT = impl::computeEffectiveMass(a, b, rA, rB, tangent);
float vt = uf::vector::dot(rv, tangent);
float invMassT = impl::computeEffectiveMass(a, b, rA, rB, contact.tangent);
float vt = uf::vector::dot(rv, contact.tangent);
float jt = -vt / invMassT;
float mu_s = std::sqrt(a.material.staticFriction * b.material.staticFriction);
@ -70,9 +78,8 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod
float jtNew = std::clamp(jtOld + jt, -maxFriction, maxFriction);
float jtDelta = jtNew - jtOld;
contact.accumulatedTangentImpulse = jtNew;
contact.tangent = tangent;
jt = jtDelta;
impl::applyImpulseTo(a, b, rA, rB, tangent * jt);
impl::applyImpulseTo(a, b, rA, rB, contact.tangent * jt);
}
}

69
engine/src/utils/math/physics/solvers/pgs.cpp
View File

@ -1,69 +0,0 @@
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/solvers/pgs.h>
// Projected Gauss-Seidel solver
void impl::blockPGSSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
const uint32_t count = std::min( (uint32_t) manifold.points.size(), (uint32_t) 4 );
for ( auto i = 0; i < count; i++ ) {
auto& c = manifold.points[i];
pod::Vector3f rA = c.point - impl::getPosition( a, true );
pod::Vector3f rB = c.point - impl::getPosition( b, true );
// normal impulse
pod::Vector3f dv = ( b.velocity + uf::vector::cross( b.angularVelocity, rB ) ) -
( a.velocity + uf::vector::cross( a.angularVelocity, rA ) );
float vn = uf::vector::dot( dv, c.normal );
float e = std::min( a.material.restitution, b.material.restitution );
float restitutionBias = (vn < -1.0f) ? -e * vn : 0.0f;
float effectiveMassN = impl::computeEffectiveMass( a, b, rA, rB, c.normal );
float lambdaN = (-vn + restitutionBias) / effectiveMassN;
float oldImpulseN = c.accumulatedNormalImpulse;
c.accumulatedNormalImpulse = std::max( oldImpulseN + lambdaN, 0.0f );
impl::applyImpulseTo( a, b, rA, rB, c.normal * (c.accumulatedNormalImpulse - oldImpulseN) );
// tangent impulse
dv = ( b.velocity + uf::vector::cross( b.angularVelocity, rB ) ) -
( a.velocity + uf::vector::cross( a.angularVelocity, rA ) );
pod::Vector3f tangent = dv - c.normal * uf::vector::dot(dv, c.normal);
float tMag2 = uf::vector::magnitude(tangent);
if ( tMag2 > EPS2 ) {
tangent /= std::sqrt(tMag2);
c.tangent = tangent;
} else if ( uf::vector::magnitude(c.tangent) < EPS ) {
c.tangent = impl::computeTangent( c.normal );
}
float vt = uf::vector::dot( dv, c.tangent );
float effectiveMassT = impl::computeEffectiveMass( a, b, rA, rB, c.tangent );
float lambdaT = -vt / effectiveMassT;
float mu = std::sqrt(a.material.dynamicFriction * b.material.dynamicFriction);
float maxFriction = mu * c.accumulatedNormalImpulse;
float oldImpulseT = c.accumulatedTangentImpulse;
c.accumulatedTangentImpulse = std::clamp( oldImpulseT + lambdaT, -maxFriction, maxFriction );
impl::applyImpulseTo( a, b, rA, rB, c.tangent * (c.accumulatedTangentImpulse - oldImpulseT) );
// pseudo impulse
pod::Vector3f pseudoDv = ( b.pseudoVelocity + uf::vector::cross( b.pseudoAngularVelocity, rB ) ) -
( a.pseudoVelocity + uf::vector::cross( a.pseudoAngularVelocity, rA ) );
float pseudoVn = uf::vector::dot( pseudoDv, c.normal );
float penetrationBias = std::max( c.penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f ) *
(uf::physics::settings.baumgarteCorrectionPercent / dt);
penetrationBias = std::min( penetrationBias, 2.0f / dt );
float lambdaP = (-pseudoVn + penetrationBias) / effectiveMassN;
float oldImpulseP = c.accumulatedPseudoImpulse;
c.accumulatedPseudoImpulse = std::max( oldImpulseP + lambdaP, 0.0f );
impl::applyPseudoImpulseTo( a, b, rA, rB, c.normal * (c.accumulatedPseudoImpulse - oldImpulseP) );
}
}

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

@ -930,12 +930,10 @@ TEST(TriangleTriangle_Collision_SimpleOverlap, {
pod::TriangleWithNormal triA{
{ { {0,0,0}, {1,0,0}, {0,1,0} } },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::TriangleWithNormal triB{
{ { {0.2f,0.2f,0}, {0.8f,0.2f,0}, {0.2f,0.8f,0} } },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
auto& bodyA = uf::physics::create( world, objA, triA, 0.0f );
@ -962,12 +960,10 @@ TEST(TriangleTriangle_Collision_CoplanarOverlap, {
pod::TriangleWithNormal triA{
{ { {0,0,0}, {2,0,0}, {0,2,0} } },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::TriangleWithNormal triB{
{ { {1,1,0}, {2,1,0}, {1,2,0} } },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
auto& bodyA = uf::physics::create(world, objA, triA, 0.0f);
@ -990,12 +986,10 @@ TEST(TriangleTriangle_Collision_TouchingEdge, {
pod::TriangleWithNormal triA{
{ { {0,0,0}, {1,0,0}, {0.5f,1,0} } },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::TriangleWithNormal triB{
{ { {0.5f,1,0}, {1.5f,0,0}, {1,1,0} } },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
auto& bodyA = uf::physics::create(world, objA, triA, 0.0f);
@ -1022,7 +1016,6 @@ TEST(TriangleAabb_Collision_OverlapCenter, {
pod::TriangleWithNormal tri {
{ { {0,0,0}, {1,0,0}, {0,1,0} } },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::AABB box = {{0.25f, 0.25f, -0.25f}, {0.75f, 0.75f, +0.25f}};
@ -1050,7 +1043,6 @@ TEST(TriangleAabb_Collision_NoOverlap, {
pod::TriangleWithNormal tri {
{ { {0,0,0}, {1,0,0}, {0,1,0} } },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::AABB box = {{2,2,2}, {3,3,3}};
@ -1076,7 +1068,6 @@ TEST(TrianglePlane_Collision_BelowPlane, {
pod::TriangleWithNormal tri{
{ { {0,0,-0.1f}, {1,0,-0.1f}, {0,1,-0.1f} } },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
auto& bodyA = uf::physics::create(world, objA, tri, 0.0f);
@ -1125,7 +1116,6 @@ TEST(TriangleSphere_Collision_Tangent, {
pod::TriangleWithNormal tri{
{ { {0,0,0}, {1,0,0}, {0,1,0} } },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::Sphere sphere = { 0.5f }; // radius
@ -1155,7 +1145,6 @@ TEST(TriangleCapsule_Collision_Overlap, {
pod::TriangleWithNormal tri{
{ { {0,0,0}, {1,0,0}, {0,1,0} } },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
// Capsule aligned along Z axis, radius 0.2
@ -1187,7 +1176,6 @@ TEST(TriangleCapsule_Collision_NoOverlap, {
pod::TriangleWithNormal tri{
{ { {0,0,0}, {1,0,0}, {0,1,0} } },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::Capsule capsule;
@ -1213,7 +1201,6 @@ TEST(TriangleCapsule_Collision_Tangent, {
pod::TriangleWithNormal tri{
{ { {0,0,0}, {1,0,0}, {0,1,0} } },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::Capsule capsule;
@ -1246,7 +1233,6 @@ TEST(TriangleCapsule_Collision_EdgeAlignment, {
pod::TriangleWithNormal tri{
{ { {0,0,0}, {2,0,0}, {0,2,0} } },
{0,0,1},
//{ {0,0,1}, {0,0,1}, {0,0,1} },
};
pod::Capsule capsule;