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

Compare commits

base: ecker:f054874fed1cb10515647d4091553b3305c5592a
Branches Tags
ecker:master
ecker:bundle
...
compare: ecker:a8ad6d0fe497df5a547235f04e5d156065bef01b
Branches Tags
ecker:master
ecker:bundle

2 Commits
f054874fed ... a8ad6d0fe4

Author SHA1 Message Date
ecker
a8ad6d0fe4 typo 2026-05-19 22:25:44 -05:00
ecker
437df59459 more cleanup (physics seem to /almost/ work without issues) 2026-05-19 22:25:22 -05:00
21 changed files with 344 additions and 247 deletions
Show Stats Expand all files Collapse all files

14
bin/data/config.json
View File

@ -336,12 +336,14 @@
"discord": { "enabled": false }
},
"physics": {
"warmup solver": false,
"block solver": false,
"psg solver": false,
"correction percent": 0.01,
"gjk": false,
"debug draw": true,
"warmup solver": true,
"block solver": true,
"pgs solver": true,
"correction percent": 0.1,
"gjk": true,
"debug draw": {
"dynamic": true
},
"fixed step": true,
"substeps": 4,
"solver iterations": 10

24
bin/data/entities/ball.json Normal file
View File

@ -0,0 +1,24 @@
{
"assets": [],
"behaviors": [
"SoundEmitterBehavior"
],
"transform": {
"position": [ 0, 3, 0 ],
"rotation": {
"axis": [ 0, 1, 0 ],
"angle": 0
},
"scale": [ 1, 1, 1 ]
},
"metadata": {
"holdable": true,
"physics": {
"mass": 10,
"type": "sphere",
"radius": 1,
"min": [ -0.5, -0.5, -0.5 ],
"max": [ 0.5, 0.5, 0.5 ]
}
}
}

4
bin/data/entities/prop.json
View File

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

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": "/burger.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" } },

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

@ -203,9 +203,11 @@ namespace pod {
/*alignas(16)*/ pod::Vector3f offset = {};
/*alignas(16)*/ pod::Vector3f velocity = {};
/*alignas(16)*/ pod::Vector3f pseudoVelocity = {};
/*alignas(16)*/ pod::Vector3f forceAccumulator = {};
/*alignas(16)*/ pod::Vector3f angularVelocity = {};
/*alignas(16)*/ pod::Vector3f pseudoAngularVelocity = {};
/*alignas(16)*/ pod::Vector3f torqueAccumulator = {};
/*alignas(16)*/ pod::Vector3f inertiaTensor = { 1, 1, 1 };
@ -232,6 +234,7 @@ namespace pod {
pod::Vector3f tangent = {};
float accumulatedNormalImpulse = 0.0f;
float accumulatedTangentImpulse = 0.0f;
float accumulatedPseudoImpulse = 0.0f;
};
struct Manifold {
@ -259,14 +262,16 @@ 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 psgContactSolver = true; // use PSG contact solver
bool pgsContactSolver = true; // use PGS contact solver
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
bool debugDraw = false; // draws wireframe of collision bodies
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
// increasing these make things lag for reasons I can imagine why
uint32_t broadphaseBvhCapacity = 1; // number of bodies per leaf node
uint32_t meshBvhCapacity = 1; // number of triangles per leaf node
@ -317,13 +322,6 @@ namespace uf {
typedef pod::Math::num_t num_t;
namespace time = uf::time; // to-do: have separate values from the physics system
extern UF_API float timescale;
extern UF_API bool async;
extern UF_API bool interpolate;
extern UF_API bool shared;
extern UF_API bool globalStorage;
extern UF_API pod::World world;
extern UF_API pod::PhysicsSettings settings;

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

@ -5,6 +5,7 @@
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 applyPseudoImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& impulse );
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 );

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

@ -4,7 +4,7 @@
#include "solvers/block.h"
#include "solvers/iterativeImpulse.h"
#include "solvers/psg.h"
#include "solvers/pgs.h"
namespace impl {
void resolveManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );

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

@ -3,7 +3,8 @@
#include "../impl.h"
namespace impl {
/*FORCE_INLINE*/ void block2x2Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
/*FORCE_INLINE*/ void block3x3Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
/*FORCE_INLINE*/ void block4x4Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
/*FORCE_INLINE*/ bool block2x2Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
/*FORCE_INLINE*/ bool block3x3Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
/*FORCE_INLINE*/ bool block4x4Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
/*FORCE_INLINE*/ bool blockSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt );
}

0
engine/inc/uf/utils/math/physics/solvers/psg.h → engine/inc/uf/utils/math/physics/solvers/pgs.h
View File

2
engine/inc/uf/utils/math/transform/transform.inl
View File

@ -50,7 +50,7 @@ pod::Transform<T>& /*UF_API*/ uf::transform::move( pod::Transform<T>& transform,
template<typename T>
pod::Transform<T>& /*UF_API*/ uf::transform::reorient( pod::Transform<T>& transform ) {
pod::Quaternion<T> q = transform.orientation;
transform.forward = { 2 * (q.x * q.z + q.w * q.y), 2 * (q.y * q.x - q.w * q.z), 1 - 2 * (q.x * q.x + q.y * q.y) };
transform.forward = { 2 * (q.x * q.z + q.w * q.y), 2 * (q.y * q.z - q.w * q.x), 1 - 2 * (q.x * q.x + q.y * q.y) };
transform.up = { 2 * (q.x * q.y - q.w * q.z), 1 - 2 * (q.x * q.x + q.z * q.z), 2 * (q.y * q.z + q.w * q.x)};
transform.right = { 1 - 2 * (q.y * q.y + q.z * q.z), 2 * (q.x * q.y + q.w * q.z), 2 * (q.x * q.z - q.w * q.y)};
return transform;

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

@ -203,13 +203,29 @@ void UF_API uf::load( ext::json::Value& json ) {
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.psgContactSolver = configEnginePhysicsJson["psg solver"].as(uf::physics::settings.psgContactSolver);
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.debugDraw = configEnginePhysicsJson["debug draw"].as(uf::physics::settings.debugDraw);
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& configEnginePhysicsDebugDrawJson = configEnginePhysicsJson["debug draw"];
if ( ext::json::isObject( configEnginePhysicsDebugDrawJson ) ) {
if ( configEnginePhysicsDebugDrawJson["static"].as<bool>() ) uf::physics::settings.debugDraw |= pod::Collider::CATEGORY_STATIC;
if ( configEnginePhysicsDebugDrawJson["dynamic"].as<bool>() ) uf::physics::settings.debugDraw |= pod::Collider::CATEGORY_DYNAMIC;
if ( configEnginePhysicsDebugDrawJson["player"].as<bool>() ) uf::physics::settings.debugDraw |= pod::Collider::CATEGORY_PLAYER;
if ( configEnginePhysicsDebugDrawJson["npc"].as<bool>() ) uf::physics::settings.debugDraw |= pod::Collider::CATEGORY_NPC;
if ( configEnginePhysicsDebugDrawJson["trigger"].as<bool>() ) uf::physics::settings.debugDraw |= pod::Collider::CATEGORY_TRIGGER;
if ( configEnginePhysicsDebugDrawJson["projectile"].as<bool>() ) uf::physics::settings.debugDraw |= pod::Collider::CATEGORY_PROJECTILE;
if ( configEnginePhysicsDebugDrawJson["character"].as<bool>() ) uf::physics::settings.debugDraw |= pod::Collider::CATEGORY_CHARACTER;
if ( configEnginePhysicsDebugDrawJson["all"].as<bool>() ) uf::physics::settings.debugDraw |= pod::Collider::CATEGORY_ALL;
} else if ( configEnginePhysicsDebugDrawJson.is<bool>() && configEnginePhysicsDebugDrawJson.as<bool>() ) {
uf::physics::settings.debugDraw = pod::Collider::CATEGORY_ALL;
}
}
// Audio settings

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

@ -174,11 +174,13 @@ void uf::ObjectBehavior::initialize( uf::Object& self ) {
auto& physicsBody = this->getComponent<pod::PhysicsBody>();
auto gravity = uf::vector::decode( metadataJsonPhysics["gravity"], physicsBody.gravity );
auto category = metadataJsonPhysics["category"].as<uf::stl::string>("ALL");
auto mask = metadataJsonPhysics["mask"].as<uf::stl::string>("ALL");
uf::physics::setColliderCategory( physicsBody, category );
uf::physics::setColliderMask( physicsBody, mask );
if ( metadataJsonPhysics["category"].is<uf::stl::string>() ){
uf::physics::setColliderCategory( physicsBody, metadataJsonPhysics["category"].as<uf::stl::string>() );
}
if ( metadataJsonPhysics["mask"].is<uf::stl::string>() ){
uf::physics::setColliderMask( physicsBody, metadataJsonPhysics["mask"].as<uf::stl::string>() );
}
uf::physics::setGravity( physicsBody, gravity );
physicsBody.velocity = uf::vector::decode( metadataJsonPhysics["velocity"], physicsBody.velocity );

11
engine/src/utils/math/physics/draw.cpp
View File

@ -44,12 +44,13 @@ void impl::addTransientLine( const pod::Vector3f& start, const pod::Vector3f& en
void impl::drawManifold( const pod::Manifold& manifold ) {
for ( auto& contact : manifold.points ) {
auto& start = contact.point;
auto end = contact.point + (contact.normal * MIN(contact.penetration, 0.1f));
auto end = contact.point + (contact.normal * MIN(contact.penetration, 0.1f) * 2);
impl::addTransientLine( start, end, pod::Vector4f{ 1, 0, 0, 1 }, manifold.a, manifold.b );
}
}
void impl::drawBody( const pod::PhysicsBody& body ) {
if ( !(body.collider.category & uf::physics::settings.debugDraw) ) return;
switch( body.collider.type ) {
case pod::ShapeType::AABB:
impl::drawAabb( body );
@ -83,14 +84,14 @@ void impl::draw( const pod::World& world, float dt ) {
::lines.clear();
// for ( auto* body : world.bodies ) impl::drawBody( *body );
for ( auto* body : world.bodies ) impl::drawBody( *body );
for ( auto it = ::transientLines.begin(); it != ::transientLines.end(); ) {
auto& line = it->second;
if ( line.ttl <= 0 ) it = ::transientLines.erase( it );
else {
impl::addLine( line.start, line.end, line.color * pod::Vector4f{ 1, 1, 1, line.ttl } );
line.ttl -= dt;
line.ttl -= dt * 0.25f;
++it;
}
}
@ -108,12 +109,12 @@ void impl::draw( const pod::World& world, float dt ) {
graphic.device = &uf::renderer::device;
graphic.material.device = &uf::renderer::device;
graphic.descriptor.depth.test = false;
//graphic.descriptor.depth.test = false;
graphic.descriptor.depth.write = false;
graphic.descriptor.renderTarget = 1; // "forward";
graphic.descriptor.topology = uf::renderer::enums::PrimitiveTopology::LINE_LIST;
graphic.descriptor.fill = uf::renderer::enums::PolygonMode::LINE;
graphic.descriptor.lineWidth = 4;
graphic.descriptor.lineWidth = 2;
uf::stl::string vertexShaderFilename = uf::io::resolveURI(uf::io::root+"/shaders/base/line/vert.spv");
uf::stl::string fragmentShaderFilename = uf::io::resolveURI(uf::io::root+"/shaders/base/line/frag.spv");

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

@ -11,14 +11,6 @@
pod::PhysicsSettings uf::physics::settings;
float uf::physics::timescale = 1.0f / 60.0f;
bool uf::physics::async = false;
// unused, as these are from reactphysics
bool uf::physics::interpolate = false;
bool uf::physics::shared = false;
bool uf::physics::globalStorage = false;
// Bindings
void uf::physics::initialize() {
uf::physics::initialize( uf::scene::getCurrentScene() );
@ -41,7 +33,7 @@ void uf::physics::tick( uf::Object& scene ) {
uf::physics::time::delta = uf::physics::time::clamp;
}
if ( uf::physics::async ) {
if ( uf::physics::settings.async ) {
uf::thread::queue( "Physics", [&](){ uf::physics::tick( scene, uf::physics::time::delta ); });
} else {
uf::physics::tick( scene, uf::physics::time::delta );
@ -63,13 +55,16 @@ void uf::physics::tick( pod::World& world, float dt ) {
static float accumulator = 0;
accumulator += dt;
while ( accumulator >= uf::physics::timescale ) {
if ( uf::physics::settings.substeps > 0 ) uf::physics::substep( world, uf::physics::timescale, uf::physics::settings.substeps );
else uf::physics::step( world, uf::physics::timescale );
accumulator -= uf::physics::timescale;
float timestep = uf::physics::settings.timestep;
while ( accumulator >= timestep ) {
if ( uf::physics::settings.substeps > 0 ) uf::physics::substep( world, timestep, uf::physics::settings.substeps );
else uf::physics::step( world, timestep );
accumulator -= timestep;
}
if ( uf::physics::settings.debugDraw ) impl::draw( world, dt );
if ( uf::physics::settings.debugDraw != pod::Collider::CATEGORY_NONE ) impl::draw( world, dt );
}
void uf::physics::terminate() {
uf::physics::terminate( uf::scene::getCurrentScene() );
@ -195,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 );
@ -252,7 +247,7 @@ void uf::physics::setColliderMask( pod::PhysicsBody& body, const uf::stl::string
if ( m == "NPC" ) return uf::physics::setColliderMask( body, pod::Collider::MASK_NPC );
if ( m == "TRIGGER" ) return uf::physics::setColliderMask( body, pod::Collider::MASK_TRIGGER );
if ( m == "PROJECTILE" ) return uf::physics::setColliderMask( body, pod::Collider::MASK_PROJECTILE );
if ( m == "CHARACTER" ) return uf::physics::setColliderCategory( body, pod::Collider::MASK_CHARACTER );
if ( m == "CHARACTER" ) return uf::physics::setColliderMask( body, pod::Collider::MASK_CHARACTER );
if ( m == "ALL" ) return uf::physics::setColliderMask( body, pod::Collider::MASK_ALL );
}
void uf::physics::setGravity( pod::PhysicsBody& body, const pod::Vector3f& gravity ) {

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

@ -24,25 +24,36 @@ float impl::computeEffectiveMass( pod::PhysicsBody& a, pod::PhysicsBody& b, cons
}
float result = inverseMass + angularTermA + angularTermB;
if (result < EPS) result = 1.0f; // prevent divide by zero
if ( result < EPS ) result = 1.0f; // prevent divide by zero
return result;
}
void impl::applyImpulseTo( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Vector3f& rA, const pod::Vector3f& rB, const pod::Vector3f& impulse ) {
if ( !a.isStatic ) {
a.velocity -= impulse * a.inverseMass;
//a.angularVelocity -= (uf::vector::cross(rA, impulse)) * a.inverseInertiaTensor;
pod::Matrix3f invIa = impl::computeWorldInverseInertia( a );
a.angularVelocity -= uf::matrix::multiply( invIa, uf::vector::cross(rA, impulse) );
}
if ( !b.isStatic ) {
b.velocity += impulse * b.inverseMass;
//b.angularVelocity += (uf::vector::cross(rB, impulse)) * b.inverseInertiaTensor;
pod::Matrix3f invIb = impl::computeWorldInverseInertia( b );
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;
pod::Matrix3f invIa = impl::computeWorldInverseInertia( a );
a.pseudoAngularVelocity -= uf::matrix::multiply( invIa, uf::vector::cross(rA, impulse) );
}
if ( !b.isStatic ) {
b.pseudoVelocity += impulse * b.inverseMass;
pod::Matrix3f invIb = impl::computeWorldInverseInertia( b );
b.pseudoAngularVelocity += uf::matrix::multiply( invIb, uf::vector::cross(rB, impulse) );
}
}
void impl::applyRollingResistance( pod::PhysicsBody& body, float dt ) {
if ( body.isStatic ) return;
@ -50,7 +61,6 @@ void impl::applyRollingResistance( pod::PhysicsBody& body, float dt ) {
float angularSpeed2 = uf::vector::magnitude( body.angularVelocity );
if ( angularSpeed2 < EPS2 ) return;
//body.angularVelocity += body.angularVelocity * body.mass * -rollingFriction * dt;
body.angularVelocity *= std::max(0.0f, 1.0f - rollingFriction * dt);
}
@ -183,7 +193,6 @@ void impl::reduceContacts( pod::Manifold& manifold ) {
if ( result.size() < 4 ) {
result.emplace_back(c);
} else {
// Replace weakest if this one is stronger
auto weakest = 0;
for ( auto i = 1; i < 4; i++ ) {
if ( result[i].penetration < result[weakest].penetration ) weakest = i;
@ -249,12 +258,14 @@ 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 ) {
if ( impl::similarContact( validContact, c ) ) {
c.accumulatedNormalImpulse = validContact.accumulatedNormalImpulse;
c.accumulatedTangentImpulse = validContact.accumulatedTangentImpulse;
c.accumulatedPseudoImpulse = validContact.accumulatedPseudoImpulse;
c.lifetime = validContact.lifetime;
isDuplicate = true;
break;
@ -316,8 +327,6 @@ void impl::warmupContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::
// tangent basis
pod::Vector3f Pt = c.tangent * c.accumulatedTangentImpulse;
impl::applyImpulseTo( a, b, rA, rB, Pt );
// UF_MSG_DEBUG("Warming, Pn={}, Pt={}, lifetime={}", uf::vector::toString(Pn), uf::vector::toString(Pt), c.lifetime );
}
void impl::warmupManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Manifold& manifold, float dt ) {
for ( auto& contact : manifold.points ) {
@ -377,19 +386,11 @@ void impl::integrate( pod::PhysicsBody& body, float dt ) {
body.velocity += acceleration * dt;
// angular integration
//body.angularVelocity += body.torqueAccumulator * body.inverseInertiaTensor * dt;
{
#if 1
pod::Matrix3f R = uf::quaternion::matrix3(body.transform->orientation);
pod::Vector3f localTorque = uf::matrix::multiply( uf::matrix::transpose(R), body.torqueAccumulator );
pod::Vector3f localAngAccel = localTorque * body.inverseInertiaTensor; // element-wise
body.angularVelocity += uf::matrix::multiply( R, localAngAccel ) * dt;
#else
pod::Matrix3f R = uf::quaternion::matrix3(body.transform->orientation);
pod::Vector3f localTorque = uf::matrix::multiply( R, body.torqueAccumulator );
pod::Vector3f localAngAccel = localTorque * body.inverseInertiaTensor; // element-wise
body.angularVelocity += uf::matrix::multiply( uf::matrix::transpose(R), localAngAccel ) * dt;
#endif
}
// update position
@ -399,10 +400,26 @@ void impl::integrate( pod::PhysicsBody& body, float dt ) {
float angularSpeed2 = uf::vector::magnitude( body.angularVelocity );
if ( angularSpeed2 > EPS2 ) {
float angularSpeed = std::sqrt( angularSpeed2 );
pod::Quaternion<> dq = uf::quaternion::axisAngle( body.angularVelocity / angularSpeed, -angularSpeed * dt);
pod::Quaternion<> dq = uf::quaternion::axisAngle( body.angularVelocity / angularSpeed, angularSpeed * dt);
uf::transform::rotate( *body.transform/*.reference*/, dq );
}
// split impulse updates
{
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 );
}
// reset
body.pseudoAngularVelocity = {};
body.pseudoVelocity = {};
}
// reset accumulators
body.forceAccumulator = {};
body.torqueAccumulator = {};

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

@ -4,11 +4,9 @@
void impl::resolveManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
if ( uf::physics::settings.blockContactSolver ) {
if ( manifold.points.size() == 2 ) return impl::block2x2Solver( a, b, manifold, dt );
if ( manifold.points.size() == 3 ) return impl::block3x3Solver( a, b, manifold, dt );
if ( manifold.points.size() == 4 ) return impl::block4x4Solver( a, b, manifold, dt );
if ( impl::blockSolver( a, b, manifold, dt ) ) return;
}
if ( uf::physics::settings.psgContactSolver ) return impl::blockPGSSolver( a, b, manifold, dt );
if ( uf::physics::settings.pgsContactSolver ) return impl::blockPGSSolver( a, b, manifold, dt );
for ( auto& contact : manifold.points ) impl::iterativeImpulseSolver( a, b, contact, dt );
}
@ -16,7 +14,7 @@ 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 ) {
for ( auto i = 0; i < iterations; ++i ) {
for ( auto& m : manifolds ) {

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

@ -4,13 +4,9 @@
namespace impl {
template<size_t N, typename T = float>
void blockNxNSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
bool blockNxNSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
pod::Matrix<T,N> K = {};
pod::Vector<T,N> rhs = {};
pod::Vector<T,N> lambda = {};
pod::Vector<T,N> residual = {};
// precompute inverse masses
float invMassA = ( a.isStatic ? 0.0f : a.inverseMass );
float invMassB = ( b.isStatic ? 0.0f : b.inverseMass );
@ -32,11 +28,9 @@ namespace impl {
float termLinear = (invMassA + invMassB) * uf::vector::dot(n_i, n_j);
// angular parts
pod::Vector3f raXnj = uf::vector::cross(rA_j, n_j);
pod::Vector3f rbXnj = uf::vector::cross(rB_j, n_j);
pod::Vector3f Ia_raXnj = uf::matrix::multiply( invIa, raXnj );
pod::Vector3f Ib_rbXnj = uf::matrix::multiply( invIb, rbXnj );
@ -51,50 +45,138 @@ 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 = {};
for ( auto i = 0; i < N; i++ ) {
auto& contact = manifold.points[i];
// full relative velocity, linear + angular
pod::Vector3f vA = a.velocity + uf::vector::cross( a.angularVelocity, contact.point - pA );
pod::Vector3f vB = b.velocity + uf::vector::cross( b.angularVelocity, contact.point - pB );
float vRel = uf::vector::dot((vB - vA), contact.normal);
// penetration bias with clamp
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);
penetrationBias = std::min(penetrationBias, 2.0f / dt); // clamp
float maxPenetrationRecovery = 2.0f; // limit to 2 units per second
if ( penetrationBias > maxPenetrationRecovery ) penetrationBias = maxPenetrationRecovery;
rhsPos[i] = -pseudoVRel + penetrationBias;
rhs[i] = -vRel + penetrationBias; // RHS is magnitude of correction needed
lambda[i] = contact.accumulatedNormalImpulse;
K_lambdaVel[i] = contact.accumulatedNormalImpulse;
K_lambdaPos[i] = contact.accumulatedPseudoImpulse;
}
residual = rhs - uf::matrix::multiply( K, lambda );
pod::Matrix<T,N> Kinv = uf::matrix::inverse( K );
pod::Vector<T,N> dLambda = uf::matrix::multiply( Kinv, residual );
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 );
// check if contacts are all valid
int invalidContactIndex = -1;
for ( auto i = 0; i < N; i++ ) {
float newLambda = std::max(lambda[i] + dLambda[i], 0.0f);
dLambda[i] = newLambda - lambda[i];
lambda[i] = newLambda;
manifold.points[i].accumulatedNormalImpulse = newLambda;
if ( K_lambdaVel[i] + dLambdaVel[i] < 0.0f || K_lambdaPos[i] + dLambdaPos[i] < 0.0f ) {
invalidContactIndex = i;
break;
}
}
// invalid contact found
if ( invalidContactIndex != -1 ) {
bool success = false;
// reduce the manifold
if ( N > 1 ) {
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
success = impl::blockSolver( a, b, reducedManifold, dt );
// copy back to original manifold
for ( size_t i = 0, r = 0; i < N; i++ ) {
if ( i != invalidContactIndex ) {
manifold.points[i] = reducedManifold.points[r++];
}
}
}
return success;
}
for ( auto i = 0; i < N; i++ ) {
auto& contact = manifold.points[i];
pod::Vector3f rA = manifold.points[i].point - pA;
pod::Vector3f rB = manifold.points[i].point - pB;
impl::applyImpulseTo( a, b, rA, rB, manifold.points[i].normal * dLambda[i] );
// real 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] );
}
// 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] );
}
// 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));
}
}
}
return true;
}
}
void impl::block2x2Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
bool impl::block2x2Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
return impl::blockNxNSolver<2>( a, b, manifold, dt );
}
void impl::block3x3Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
bool impl::block3x3Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
return impl::blockNxNSolver<3>( a, b, manifold, dt );
}
void impl::block4x4Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
bool impl::block4x4Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
return impl::blockNxNSolver<4>( a, b, manifold, dt );
}
bool impl::blockSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
if ( manifold.points.size() == 2 ) return impl::block2x2Solver( a, b, manifold, dt );
if ( manifold.points.size() == 3 ) return impl::block3x3Solver( a, b, manifold, dt );
if ( manifold.points.size() == 4 ) return impl::block4x4Solver( a, b, manifold, dt );
return false;
}

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

@ -3,72 +3,75 @@
#include <uf/utils/math/physics/solvers/iterativeImpulse.h>
void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Contact& contact, float dt ) {
// relative positions from centers to contact point
pod::Vector3f rA = contact.point - impl::getPosition( a, true );
pod::Vector3f rB = contact.point - impl::getPosition( b, true );
// relative velocity at contact
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;
// normal contact velocity
float velAlongNormal = uf::vector::dot(rv, contact.normal);
float velTolerance = 0; // -1.0e3f;
if ( velAlongNormal > velTolerance ) return; // if separating, no impulse
// compute restitution (bounce)
float e = std::min(a.material.restitution, b.material.restitution);
float restitutionBias = 0.0f;
if ( velAlongNormal < -1.0f ) restitutionBias = -e * velAlongNormal;
// nullify restitution if velocity is small enough
if ( fabs(velAlongNormal) < 1.0f) e = 0.0f;
// effective inverse mass along normal
float targetVelocity = restitutionBias;
float invMassN = impl::computeEffectiveMass(a, b, rA, rB, contact.normal);
// normal impulse scalar
float jn = -(1.0f + e) * velAlongNormal;
jn /= invMassN;
if ( uf::physics::settings.warmupSolver ) {
{
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 penetrationBias = std::max(contact.penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f) * (uf::physics::settings.baumgarteCorrectionPercent / dt);
penetrationBias = std::min(penetrationBias, 2.0f / 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 jPseudo = (penetrationBias - pseudoVelAlongNormal) / invMassN;
float jPseudoOld = contact.accumulatedPseudoImpulse;
float jPseudoNew = std::max(0.0f, jPseudoOld + jPseudo);
contact.accumulatedPseudoImpulse = jPseudoNew;
jPseudo = jPseudoNew - jPseudoOld;
impl::applyPseudoImpulseTo(a, b, rA, rB, contact.normal * jPseudo);
}
// tangent 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 );
// effective mass along tangent
float invMassT = impl::computeEffectiveMass(a, b, rA, rB, tangent);
// tangential relative velocity
float vt = uf::vector::dot(rv, tangent);
// required tangential impulse to cancel tangent velocity
float jt = -vt / invMassT;
// friction coefficients
float mu_s = std::sqrt(a.material.staticFriction * b.material.staticFriction);
float mu_d = std::sqrt(a.material.dynamicFriction * b.material.dynamicFriction);
if ( std::fabs(jt) > jn * mu_s) jt = -jn * mu_d; // dynamic friction: resist sliding proportionally
float normalForce = contact.accumulatedNormalImpulse;
if ( std::fabs(jt) > normalForce * mu_s ) {
jt = -normalForce * mu_d;
}
if ( uf::physics::settings.warmupSolver ) {
float maxFriction = mu_s * contact.accumulatedNormalImpulse;
float maxFriction = mu_s * normalForce;
float jtOld = contact.accumulatedTangentImpulse;
float jtNew = std::max(-maxFriction, std::min(jtOld + jt, maxFriction));
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);
}

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

@ -0,0 +1,69 @@
#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) );
}
}

112
engine/src/utils/math/physics/solvers/psg.cpp
View File

@ -1,112 +0,0 @@
#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/solvers/psg.h>
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 );
struct ContactCache {
pod::Vector3f normal;
pod::Vector3f tangent;
pod::Vector3f rA, rB;
float bias;
float effectiveMassN;
float effectiveMassT;
float accumulatedNormalImpulse;
float accumulatedTangentImpulse;
};
// precompute contact caches
ContactCache cache[4];
for ( auto i = 0; i < count; i++ ) {
auto& c = manifold.points[i];
auto& cc = cache[i];
cc.normal = c.normal;
cc.tangent = impl::computeTangent( c.normal );
cc.rA = c.point - impl::getPosition( a, true );
cc.rB = c.point - impl::getPosition( b, true );
// relative velocity along normal
pod::Vector3f dv = ( b.velocity + uf::vector::cross( b.angularVelocity, cc.rB ) ) - ( a.velocity + uf::vector::cross( a.angularVelocity, cc.rA ) );
float vn = uf::vector::dot( dv, cc.normal );
// restitution bias + baumgarte
float e = std::min( a.material.restitution, b.material.restitution );
float penetrationBias = std::max( c.penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f ) * (uf::physics::settings.baumgarteCorrectionPercent / dt);
float restitutionBias = (vn < -1.0f) ? -e * vn : 0.0f;
cc.bias = restitutionBias + penetrationBias;
// effective mass (normal)
pod::Matrix3f invIa = impl::computeWorldInverseInertia( a );
pod::Matrix3f invIb = impl::computeWorldInverseInertia( b );
pod::Vector3f rnA = uf::vector::cross( cc.rA, cc.normal );
pod::Vector3f rnB = uf::vector::cross( cc.rB, cc.normal );
pod::Vector3f I_rnA = uf::matrix::multiply( invIa, rnA );
pod::Vector3f I_rnB = uf::matrix::multiply( invIb, rnB );
float Kn = (a.isStatic ? 0.0f : a.inverseMass) + (b.isStatic ? 0.0f : b.inverseMass) +
uf::vector::dot( uf::vector::cross( I_rnA, cc.rA ) + uf::vector::cross( I_rnB, cc.rB ), cc.normal );
cc.effectiveMassN = (Kn > 0.0f) ? 1.0f / Kn : 0.0f;
// effective mass (tangent)
pod::Vector3f rtA = uf::vector::cross( cc.rA, cc.tangent );
pod::Vector3f rtB = uf::vector::cross( cc.rB, cc.tangent );
pod::Vector3f I_rtA = uf::matrix::multiply( invIa, rtA );
pod::Vector3f I_rtB = uf::matrix::multiply( invIb, rtB );
float Kt = (a.isStatic ? 0.0f : a.inverseMass) + (b.isStatic ? 0.0f : b.inverseMass) +
uf::vector::dot( uf::vector::cross( I_rtA, cc.rA ) + uf::vector::cross( I_rtB, cc.rB ), cc.tangent );
cc.effectiveMassT = ( Kt > 0.0f ) ? ( 1.0f / Kt ) : 0.0f;
// warm start
#if 1
cc.accumulatedNormalImpulse = c.accumulatedNormalImpulse;
cc.accumulatedTangentImpulse = c.accumulatedTangentImpulse;
// apply warm-start impulses
pod::Vector3f P = cc.normal * cc.accumulatedNormalImpulse + cc.tangent * cc.accumulatedTangentImpulse;
impl::applyImpulseTo(a, b, cc.rA, cc.rB, P);
#endif
}
// iterative PGS
for ( auto iter = 0; iter < uf::physics::settings.solverIterations; iter++ ) {
for ( auto i = 0; i < count; i++ ) {
auto& cc = cache[i];
// relative velocity
pod::Vector3f dv = ( b.velocity + uf::vector::cross( b.angularVelocity, cc.rB ) ) - ( a.velocity + uf::vector::cross( a.angularVelocity, cc.rA ) );
// normal constraint
float vn = uf::vector::dot( dv, cc.normal );
float lambdaN = cc.effectiveMassN * (-vn + cc.bias);
float oldImpulseN = cc.accumulatedNormalImpulse;
cc.accumulatedNormalImpulse = std::max( oldImpulseN + lambdaN, 0.0f );
float dPn = cc.accumulatedNormalImpulse - oldImpulseN;
impl::applyImpulseTo( a, b, cc.rA, cc.rB, cc.normal * dPn );
// friction constraint
dv = ( b.velocity + uf::vector::cross( b.angularVelocity, cc.rB ) ) - ( a.velocity + uf::vector::cross( a.angularVelocity, cc.rA ) );
float vt = uf::vector::dot( dv, cc.tangent );
float lambdaT = cc.effectiveMassT * (-vt);
float maxFriction = ( a.material.dynamicFriction + b.material.dynamicFriction ) * 0.5f * cc.accumulatedNormalImpulse;
float oldImpulseT = cc.accumulatedTangentImpulse;
cc.accumulatedTangentImpulse = std::clamp( oldImpulseT + lambdaT, -maxFriction, maxFriction );
float dPt = cc.accumulatedTangentImpulse - oldImpulseT;
impl::applyImpulseTo( a, b, cc.rA, cc.rB, cc.tangent * dPt );
}
}
// store impulses back into manifold
for ( auto i = 0; i < count; i++ ) {
manifold.points[i].accumulatedNormalImpulse = cache[i].accumulatedNormalImpulse;
manifold.points[i].accumulatedTangentImpulse = cache[i].accumulatedTangentImpulse;
}
}