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

removed PGS solver since it is the iterative impulse solver, fixed returning problem of boxes sinking, some other things

Browse Source
This commit is contained in:
ecker 2026-05-20 15:02:29 -05:00
parent a8ad6d0fe4
commit 098281ad81
16 changed files with 223 additions and 162 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
bin/data/config.json
View File

@ -338,9 +338,10 @@
"physics": {
"warmup solver": true,
"block solver": true,
"pgs solver": true,
"correction percent": 0.1,
"gjk": true,
"resolve block solver": true,
"correction percent": 0.2,
"correction slop": 0.005,
"gjk": false,
"debug draw": {
"dynamic": true
},

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

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

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" } },

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

@ -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,11 @@ namespace pod {
// to-do: find possibly better values for this
uint32_t solverIterations = 10;
float baumgarteCorrectionPercent = 0.01f;
float baumgarteCorrectionSlop = 0.01f;
float baumgarteCorrectionPercent = 0.2f;
float baumgarteCorrectionSlop = 0.005f;
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;

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

@ -4,7 +4,6 @@
#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 );

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

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

@ -203,7 +203,7 @@ 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.pgsContactSolver = configEnginePhysicsJson["pgs solver"].as(uf::physics::settings.pgsContactSolver);
uf::physics::settings.resolveBlockContact = configEnginePhysicsJson["resolve block solver"].as(uf::physics::settings.resolveBlockContact);
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);
@ -211,6 +211,7 @@ void UF_API uf::load( ext::json::Value& json ) {
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);
uf::physics::settings.baumgarteCorrectionSlop = configEnginePhysicsJson["correction slop"].as(uf::physics::settings.baumgarteCorrectionSlop);
auto& configEnginePhysicsDebugDrawJson = configEnginePhysicsJson["debug draw"];
if ( ext::json::isObject( configEnginePhysicsDebugDrawJson ) ) {

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

@ -475,6 +475,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 +573,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;
}

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

@ -24,7 +24,8 @@ 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
// 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)
if ( result < EPS ) result = 1.0f;
return result;
}
@ -176,6 +177,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 +268,7 @@ void impl::reduceContacts( pod::Manifold& manifold ) {
}
manifold.points = result;
#endif
}
void impl::mergeContacts( pod::Manifold& manifold ) {
@ -298,12 +364,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;
}
@ -411,8 +481,8 @@ void impl::integrate( pod::PhysicsBody& body, float 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::Quaternion<> dq = uf::quaternion::axisAngle( body.pseudoAngularVelocity / pseudoAngularSpeed, pseudoAngularSpeed * dt );
uf::transform::rotate( *body.transform/*.reference*/, dq );
}
// reset

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

@ -85,6 +85,26 @@ void impl::getSupportFace( const pod::PhysicsBody& body, const pod::Vector3f& di
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;

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

@ -76,25 +76,44 @@ 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 dist = uf::vector::dot(poly[i], refNormal) - uf::vector::dot(refNormal, refTri.points[0]);
float penetration = -dist;
#endif
manifold.points.emplace_back(pod::Contact{ poly[i], bestAxis, penetration });
}
@ -147,8 +166,18 @@ bool impl::triangleAabb( const pod::TriangleWithNormal& tri, const pod::PhysicsB
if ( polyCount == 0 ) return false;
pod::Vector3f boxSupport = cB;
boxSupport.x -= std::copysign(eB.x, bestAxis.x);
boxSupport.y -= std::copysign(eB.y, bestAxis.y);
boxSupport.z -= std::copysign(eB.z, bestAxis.z);
float referenceOffset = uf::vector::dot(bestAxis, boxSupport);
for ( auto i = 0; i < polyCount; i++ ) {
manifold.points.emplace_back( pod::Contact{ poly[i], bestAxis, minOverlap } );
float pointProjection = uf::vector::dot(bestAxis, poly[i]);
float penetration = pointProjection - referenceOffset;
manifold.points.emplace_back( pod::Contact{ poly[i], bestAxis, penetration } );
}
return true;
@ -204,8 +233,18 @@ bool impl::triangleObb( const pod::TriangleWithNormal& tri, const pod::PhysicsBo
if ( polyCount == 0 ) return false;
pod::Vector3f boxSupport = cB;
boxSupport.x -= std::copysign(eB.x, bestAxis.x);
boxSupport.y -= std::copysign(eB.y, bestAxis.y);
boxSupport.z -= std::copysign(eB.z, bestAxis.z);
float referenceOffset = uf::vector::dot(bestAxis, boxSupport);
for ( auto i = 0; i < polyCount; i++ ) {
manifold.points.emplace_back( pod::Contact{ poly[i], bestAxis, minOverlap } );
float pointProjection = uf::vector::dot(bestAxis, poly[i]);
float penetration = pointProjection - referenceOffset;
manifold.points.emplace_back( pod::Contact{ poly[i], bestAxis, penetration } );
}
return true;

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

55
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 );
@ -95,7 +95,7 @@ namespace impl {
if ( invalidContactIndex != -1 ) {
bool success = false;
// reduce the manifold
if ( N > 1 ) {
if ( uf::physics::settings.resolveBlockContact && N > 1 ) {
pod::Manifold reducedManifold = manifold;
reducedManifold.points.erase( reducedManifold.points.begin() + invalidContactIndex );
manifold.points[invalidContactIndex].accumulatedNormalImpulse = 0.0f;
@ -133,31 +133,44 @@ namespace impl {
contact.accumulatedPseudoImpulse = newLambdaPos;
impl::applyPseudoImpulseTo( a, b, rA, rB, manifold.points[i].normal * dLambdaPos[i] );
}
// 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);
}
}

48
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);
// real 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,6 +30,7 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod
impl::applyImpulseTo(a, b, rA, rB, contact.normal * jn);
}
// pseudo impulse
{
float penetrationBias = std::max(contact.penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f) * (uf::physics::settings.baumgarteCorrectionPercent / dt);
penetrationBias = std::min(penetrationBias, 2.0f / dt);
@ -48,13 +49,21 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod
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 );
float invMassT = impl::computeEffectiveMass(a, b, rA, rB, tangent);
float vt = uf::vector::dot(rv, tangent);
// 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 );
}
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 +79,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;