organized physics system
This commit is contained in:
parent
60527c4e6b
commit
fe10395ce8
@ -1009,114 +1009,3 @@ namespace {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
namespace {
|
||||
struct UnionFind {
|
||||
uf::stl::vector<pod::BVH::index_t> parent;
|
||||
uf::stl::vector<pod::BVH::index_t> rank;
|
||||
|
||||
UnionFind( pod::BVH::index_t n ) {
|
||||
parent.resize(n);
|
||||
rank.resize(n, 0);
|
||||
|
||||
for ( auto i = 0; i < n; i++ )
|
||||
parent[i] = i;
|
||||
}
|
||||
|
||||
pod::BVH::index_t find( pod::BVH::index_t x ) {
|
||||
if ( parent[x] != x ) parent[x] = find(parent[x]);
|
||||
return parent[x];
|
||||
}
|
||||
|
||||
void unite( pod::BVH::index_t a, pod::BVH::index_t b ) {
|
||||
pod::BVH::index_t rootA = find(a);
|
||||
pod::BVH::index_t rootB = find(b);
|
||||
|
||||
if ( rootA == rootB ) return;
|
||||
|
||||
// union by rank
|
||||
if ( rank[rootA] < rank[rootB] ) parent[rootA] = rootB;
|
||||
else if ( rank[rootA] > rank[rootB] ) parent[rootB] = rootA;
|
||||
else {
|
||||
parent[rootB] = rootA;
|
||||
rank[rootA]++;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// to-do: rewrite this, I'm pretty sure it's faulty
|
||||
void buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector<pod::PhysicsBody*>& bodies, uf::stl::vector<pod::Island>& islands ) {
|
||||
UnionFind unionizer(bodies.size());
|
||||
|
||||
// union all pairs
|
||||
for ( auto& [a, b] : pairs ) {
|
||||
if ( !bodies[a]->isStatic && !bodies[b]->isStatic ) {
|
||||
unionizer.unite(a, b);
|
||||
}
|
||||
}
|
||||
|
||||
// map root to island index
|
||||
static thread_local uf::stl::unordered_map<pod::BVH::index_t, pod::BVH::index_t> rootToIsland;
|
||||
rootToIsland.clear();
|
||||
|
||||
islands.clear();
|
||||
islands.reserve(bodies.size());
|
||||
|
||||
for ( auto i = 0; i < bodies.size(); i++ ) {
|
||||
if ( bodies[i]->isStatic ) continue;
|
||||
|
||||
pod::BVH::index_t root = unionizer.find(i);
|
||||
|
||||
auto [ it, inserted ] = rootToIsland.try_emplace( root, (pod::BVH::index_t) islands.size());
|
||||
if ( inserted ) islands.emplace_back();
|
||||
|
||||
pod::BVH::index_t islandID = rootToIsland[root];
|
||||
islands[islandID].indices.emplace_back( i );
|
||||
}
|
||||
|
||||
// collect pairs per island
|
||||
for ( auto& [a, b] : pairs ) {
|
||||
// do not insert these pairs if they're non-colliding
|
||||
if ( !::shouldCollide( *bodies[a], *bodies[b] ) ) continue;
|
||||
|
||||
// just in case
|
||||
pod::BVH::index_t dynamicIndex = bodies[a]->isStatic ? b : a;
|
||||
if ( bodies[a]->isStatic && bodies[b]->isStatic ) continue;
|
||||
|
||||
pod::BVH::index_t root = unionizer.find(a);
|
||||
if ( rootToIsland.find(root) != rootToIsland.end() ) {
|
||||
pod::BVH::index_t islandID = rootToIsland[root];
|
||||
|
||||
islands[islandID].pairs.emplace_back(a, b);
|
||||
|
||||
if ( bodies[a]->activity.awake || bodies[b]->activity.awake ) {
|
||||
::wakeBody( *bodies[dynamicIndex] );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// update islands
|
||||
for ( auto it = islands.begin(); it != islands.end(); ) {
|
||||
auto& island = *it;
|
||||
island.awake = false;
|
||||
|
||||
// wake island if something is awake in it
|
||||
for ( auto idx : island.indices ) {
|
||||
auto& body = *bodies[idx];
|
||||
if ( !body.activity.awake ) continue;
|
||||
island.awake = true;
|
||||
}
|
||||
|
||||
// update bodies within island
|
||||
for ( auto idx : island.indices )
|
||||
(island.awake ? ::wakeBody : ::sleepBody)( *bodies[idx] );
|
||||
|
||||
// erase sleeping island
|
||||
if ( !island.awake ) {
|
||||
it = islands.erase(it);
|
||||
} else {
|
||||
++it;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
110
engine/src/utils/math/physics/broadphase/island.inl
Normal file
110
engine/src/utils/math/physics/broadphase/island.inl
Normal file
@ -0,0 +1,110 @@
|
||||
namespace {
|
||||
struct UnionFind {
|
||||
uf::stl::vector<pod::BVH::index_t> parent;
|
||||
uf::stl::vector<pod::BVH::index_t> rank;
|
||||
|
||||
UnionFind( pod::BVH::index_t n ) {
|
||||
parent.resize(n);
|
||||
rank.resize(n, 0);
|
||||
|
||||
for ( auto i = 0; i < n; i++ )
|
||||
parent[i] = i;
|
||||
}
|
||||
|
||||
pod::BVH::index_t find( pod::BVH::index_t x ) {
|
||||
if ( parent[x] != x ) parent[x] = find(parent[x]);
|
||||
return parent[x];
|
||||
}
|
||||
|
||||
void unite( pod::BVH::index_t a, pod::BVH::index_t b ) {
|
||||
pod::BVH::index_t rootA = find(a);
|
||||
pod::BVH::index_t rootB = find(b);
|
||||
|
||||
if ( rootA == rootB ) return;
|
||||
|
||||
// union by rank
|
||||
if ( rank[rootA] < rank[rootB] ) parent[rootA] = rootB;
|
||||
else if ( rank[rootA] > rank[rootB] ) parent[rootB] = rootA;
|
||||
else {
|
||||
parent[rootB] = rootA;
|
||||
rank[rootA]++;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// to-do: rewrite this, I'm pretty sure it's faulty
|
||||
void buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector<pod::PhysicsBody*>& bodies, uf::stl::vector<pod::Island>& islands ) {
|
||||
UnionFind unionizer(bodies.size());
|
||||
|
||||
// union all pairs
|
||||
for ( auto& [a, b] : pairs ) {
|
||||
if ( !bodies[a]->isStatic && !bodies[b]->isStatic ) {
|
||||
unionizer.unite(a, b);
|
||||
}
|
||||
}
|
||||
|
||||
// map root to island index
|
||||
static thread_local uf::stl::unordered_map<pod::BVH::index_t, pod::BVH::index_t> rootToIsland;
|
||||
rootToIsland.clear();
|
||||
|
||||
islands.clear();
|
||||
islands.reserve(bodies.size());
|
||||
|
||||
for ( auto i = 0; i < bodies.size(); i++ ) {
|
||||
if ( bodies[i]->isStatic ) continue;
|
||||
|
||||
pod::BVH::index_t root = unionizer.find(i);
|
||||
|
||||
auto [ it, inserted ] = rootToIsland.try_emplace( root, (pod::BVH::index_t) islands.size());
|
||||
if ( inserted ) islands.emplace_back();
|
||||
|
||||
pod::BVH::index_t islandID = rootToIsland[root];
|
||||
islands[islandID].indices.emplace_back( i );
|
||||
}
|
||||
|
||||
// collect pairs per island
|
||||
for ( auto& [a, b] : pairs ) {
|
||||
// do not insert these pairs if they're non-colliding
|
||||
if ( !::shouldCollide( *bodies[a], *bodies[b] ) ) continue;
|
||||
|
||||
// just in case
|
||||
pod::BVH::index_t dynamicIndex = bodies[a]->isStatic ? b : a;
|
||||
if ( bodies[a]->isStatic && bodies[b]->isStatic ) continue;
|
||||
|
||||
pod::BVH::index_t root = unionizer.find(a);
|
||||
if ( rootToIsland.find(root) != rootToIsland.end() ) {
|
||||
pod::BVH::index_t islandID = rootToIsland[root];
|
||||
|
||||
islands[islandID].pairs.emplace_back(a, b);
|
||||
|
||||
if ( bodies[a]->activity.awake || bodies[b]->activity.awake ) {
|
||||
::wakeBody( *bodies[dynamicIndex] );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// update islands
|
||||
for ( auto it = islands.begin(); it != islands.end(); ) {
|
||||
auto& island = *it;
|
||||
island.awake = false;
|
||||
|
||||
// wake island if something is awake in it
|
||||
for ( auto idx : island.indices ) {
|
||||
auto& body = *bodies[idx];
|
||||
if ( !body.activity.awake ) continue;
|
||||
island.awake = true;
|
||||
}
|
||||
|
||||
// update bodies within island
|
||||
for ( auto idx : island.indices )
|
||||
(island.awake ? ::wakeBody : ::sleepBody)( *bodies[idx] );
|
||||
|
||||
// erase sleeping island
|
||||
if ( !island.awake ) {
|
||||
it = islands.erase(it);
|
||||
} else {
|
||||
++it;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,10 +1,11 @@
|
||||
// to-do: clean this mess
|
||||
#define REVERSE_COLLIDER( a, b, fun )\
|
||||
auto start = manifold.points.size();\
|
||||
if ( !::fun( b, a, manifold, eps ) ) return false;\
|
||||
for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal;\
|
||||
return true;
|
||||
|
||||
// forward declare
|
||||
// forward declare (to-do: properly handle this)
|
||||
namespace {
|
||||
bool aabbAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS );
|
||||
bool aabbSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS );
|
||||
|
||||
@ -10,17 +10,23 @@
|
||||
#define UF_PHYSICS_TEST 0
|
||||
|
||||
#include "helpers.inl"
|
||||
#include "aabb.inl"
|
||||
#include "sphere.inl"
|
||||
#include "plane.inl"
|
||||
#include "capsule.inl"
|
||||
#include "triangle.inl"
|
||||
#include "mesh.inl"
|
||||
#include "convexHull.inl"
|
||||
#include "ray.inl"
|
||||
#include "bvh.inl"
|
||||
#include "gjk.inl"
|
||||
#include "epa.inl"
|
||||
|
||||
#include "narrowphase/aabb.inl"
|
||||
#include "narrowphase/sphere.inl"
|
||||
#include "narrowphase/plane.inl"
|
||||
#include "narrowphase/capsule.inl"
|
||||
#include "narrowphase/triangle.inl"
|
||||
#include "narrowphase/mesh.inl"
|
||||
#include "narrowphase/convexHull.inl"
|
||||
#include "narrowphase/ray.inl"
|
||||
|
||||
|
||||
#include "narrowphase/gjk.inl"
|
||||
#include "narrowphase/epa.inl"
|
||||
|
||||
#include "broadphase/bvh.inl"
|
||||
#include "broadphase/island.inl"
|
||||
|
||||
#include "integration.inl"
|
||||
#include "solvers.inl"
|
||||
|
||||
|
||||
@ -1,270 +1,8 @@
|
||||
#include "./solvers/iterativeImpulse.inl"
|
||||
#include "./solvers/block.inl"
|
||||
#include "./solvers/psg.inl"
|
||||
|
||||
namespace {
|
||||
void iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Contact& contact, float dt ) {
|
||||
// relative positions from centers to contact point
|
||||
pod::Vector3f rA = contact.point - ::getPosition( a, true );
|
||||
pod::Vector3f rB = contact.point - ::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);
|
||||
|
||||
// nullify restitution if velocity is small enough
|
||||
if ( fabs(velAlongNormal) < 1.0f) e = 0.0f;
|
||||
|
||||
// effective inverse mass along normal
|
||||
float invMassN = ::computeEffectiveMass(a, b, rA, rB, contact.normal);
|
||||
|
||||
// normal impulse scalar
|
||||
float jn = -(1.0f + e) * velAlongNormal;
|
||||
jn /= invMassN;
|
||||
if ( uf::physics::impl::settings.warmupSolver ) {
|
||||
float jnOld = contact.accumulatedNormalImpulse;
|
||||
float jnNew = std::max(0.0f, jnOld + jn);
|
||||
float jnDelta = jnNew - jnOld;
|
||||
contact.accumulatedNormalImpulse = jnNew;
|
||||
jn = jnDelta;
|
||||
}
|
||||
|
||||
::applyImpulseTo(a, b, rA, rB, contact.normal * jn);
|
||||
|
||||
// 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 = ::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
|
||||
|
||||
if ( uf::physics::impl::settings.warmupSolver ) {
|
||||
float maxFriction = mu_s * contact.accumulatedNormalImpulse;
|
||||
float jtOld = contact.accumulatedTangentImpulse;
|
||||
float jtNew = std::max(-maxFriction, std::min(jtOld + jt, maxFriction));
|
||||
float jtDelta = jtNew - jtOld;
|
||||
contact.accumulatedTangentImpulse = jtNew;
|
||||
contact.tangent = tangent;
|
||||
jt = jtDelta;
|
||||
}
|
||||
|
||||
::applyImpulseTo(a, b, rA, rB, tangent * jt);
|
||||
}
|
||||
}
|
||||
|
||||
template<size_t N, typename T = float>
|
||||
void 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 );
|
||||
|
||||
pod::Matrix3f invIa = computeWorldInverseInertia( a );
|
||||
pod::Matrix3f invIb = computeWorldInverseInertia( b );
|
||||
|
||||
auto pA = ::getPosition( a, true );
|
||||
auto pB = ::getPosition( b, true );
|
||||
|
||||
for ( auto i = 0; i < N; i++ ) {
|
||||
pod::Vector3f rA_i = manifold.points[i].point - pA;
|
||||
pod::Vector3f rB_i = manifold.points[i].point - pB;
|
||||
pod::Vector3f n_i = manifold.points[i].normal;
|
||||
|
||||
for ( auto j = 0; j < N; j++ ) {
|
||||
pod::Vector3f rA_j = manifold.points[j].point - pA;
|
||||
pod::Vector3f rB_j = manifold.points[j].point - pB;
|
||||
pod::Vector3f n_j = manifold.points[j].normal;
|
||||
|
||||
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 );
|
||||
|
||||
pod::Vector3f crossA = uf::vector::cross(Ia_raXnj, rA_i);
|
||||
pod::Vector3f crossB = uf::vector::cross(Ib_rbXnj, rB_i);
|
||||
|
||||
float termAngular = uf::vector::dot(n_i, crossA + crossB);
|
||||
|
||||
K(i,j) = termLinear + termAngular;
|
||||
}
|
||||
|
||||
K(i,i) += 1e-3f;
|
||||
}
|
||||
|
||||
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 penetrationBias = std::max(contact.penetration - uf::physics::impl::settings.baumgarteCorrectionSlop, 0.0f) * (uf::physics::impl::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;
|
||||
|
||||
rhs[i] = -vRel + penetrationBias; // RHS is magnitude of correction needed
|
||||
lambda[i] = contact.accumulatedNormalImpulse;
|
||||
}
|
||||
|
||||
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 );
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
for ( auto i = 0; i < N; i++ ) {
|
||||
pod::Vector3f rA = manifold.points[i].point - pA;
|
||||
pod::Vector3f rB = manifold.points[i].point - pB;
|
||||
|
||||
::applyImpulseTo( a, b, rA, rB, manifold.points[i].normal * dLambda[i] );
|
||||
}
|
||||
}
|
||||
|
||||
void block2x2Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
|
||||
return ::blockNxNSolver<2>( a, b, manifold, dt );
|
||||
}
|
||||
void block3x3Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
|
||||
return ::blockNxNSolver<3>( a, b, manifold, dt );
|
||||
}
|
||||
void block4x4Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
|
||||
return ::blockNxNSolver<4>( a, b, manifold, dt );
|
||||
}
|
||||
|
||||
struct ContactCache {
|
||||
pod::Vector3f normal;
|
||||
pod::Vector3f tangent;
|
||||
pod::Vector3f rA, rB;
|
||||
float bias;
|
||||
float effectiveMassN;
|
||||
float effectiveMassT;
|
||||
float accumulatedNormalImpulse;
|
||||
float accumulatedTangentImpulse;
|
||||
};
|
||||
|
||||
void 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 );
|
||||
|
||||
// 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 = ::computeTangent( c.normal );
|
||||
cc.rA = c.point - ::getPosition( a, true );
|
||||
cc.rB = c.point - ::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::impl::settings.baumgarteCorrectionSlop, 0.0f ) * (uf::physics::impl::settings.baumgarteCorrectionPercent / dt);
|
||||
float restitutionBias = (vn < -1.0f) ? -e * vn : 0.0f;
|
||||
cc.bias = restitutionBias + penetrationBias;
|
||||
|
||||
// effective mass (normal)
|
||||
pod::Vector3f rnA = uf::vector::cross( cc.rA, cc.normal );
|
||||
pod::Vector3f rnB = uf::vector::cross( cc.rB, cc.normal );
|
||||
float Kn = (a.isStatic ? 0.0f : a.inverseMass) + (b.isStatic ? 0.0f : b.inverseMass) +
|
||||
uf::vector::dot( uf::vector::cross( rnA * a.inverseInertiaTensor, cc.rA ) + uf::vector::cross( rnB * b.inverseInertiaTensor, 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 );
|
||||
float Kt = (a.isStatic ? 0.0f : a.inverseMass) + (b.isStatic ? 0.0f : b.inverseMass) +
|
||||
uf::vector::dot( uf::vector::cross( rtA * a.inverseInertiaTensor, cc.rA ) + uf::vector::cross( rtB * b.inverseInertiaTensor, 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;
|
||||
|
||||
::applyImpulseTo(a, b, cc.rA, cc.rB, P);
|
||||
#endif
|
||||
}
|
||||
|
||||
// iterative PGS
|
||||
for ( auto iter = 0; iter < uf::physics::impl::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;
|
||||
|
||||
::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;
|
||||
|
||||
::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;
|
||||
}
|
||||
}
|
||||
|
||||
void resolveManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
|
||||
if ( uf::physics::impl::settings.blockContactSolver ) {
|
||||
if ( manifold.points.size() == 2 ) return ::block2x2Solver( a, b, manifold, dt );
|
||||
|
||||
96
engine/src/utils/math/physics/solvers/block.inl
Normal file
96
engine/src/utils/math/physics/solvers/block.inl
Normal file
@ -0,0 +1,96 @@
|
||||
namespace {
|
||||
template<size_t N, typename T = float>
|
||||
void 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 );
|
||||
|
||||
pod::Matrix3f invIa = computeWorldInverseInertia( a );
|
||||
pod::Matrix3f invIb = computeWorldInverseInertia( b );
|
||||
|
||||
auto pA = ::getPosition( a, true );
|
||||
auto pB = ::getPosition( b, true );
|
||||
|
||||
for ( auto i = 0; i < N; i++ ) {
|
||||
pod::Vector3f rA_i = manifold.points[i].point - pA;
|
||||
pod::Vector3f rB_i = manifold.points[i].point - pB;
|
||||
pod::Vector3f n_i = manifold.points[i].normal;
|
||||
|
||||
for ( auto j = 0; j < N; j++ ) {
|
||||
pod::Vector3f rA_j = manifold.points[j].point - pA;
|
||||
pod::Vector3f rB_j = manifold.points[j].point - pB;
|
||||
pod::Vector3f n_j = manifold.points[j].normal;
|
||||
|
||||
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 );
|
||||
|
||||
pod::Vector3f crossA = uf::vector::cross(Ia_raXnj, rA_i);
|
||||
pod::Vector3f crossB = uf::vector::cross(Ib_rbXnj, rB_i);
|
||||
|
||||
float termAngular = uf::vector::dot(n_i, crossA + crossB);
|
||||
|
||||
K(i,j) = termLinear + termAngular;
|
||||
}
|
||||
|
||||
K(i,i) += 1e-3f;
|
||||
}
|
||||
|
||||
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 penetrationBias = std::max(contact.penetration - uf::physics::impl::settings.baumgarteCorrectionSlop, 0.0f) * (uf::physics::impl::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;
|
||||
|
||||
rhs[i] = -vRel + penetrationBias; // RHS is magnitude of correction needed
|
||||
lambda[i] = contact.accumulatedNormalImpulse;
|
||||
}
|
||||
|
||||
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 );
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
for ( auto i = 0; i < N; i++ ) {
|
||||
pod::Vector3f rA = manifold.points[i].point - pA;
|
||||
pod::Vector3f rB = manifold.points[i].point - pB;
|
||||
|
||||
::applyImpulseTo( a, b, rA, rB, manifold.points[i].normal * dLambda[i] );
|
||||
}
|
||||
}
|
||||
|
||||
void block2x2Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
|
||||
return ::blockNxNSolver<2>( a, b, manifold, dt );
|
||||
}
|
||||
void block3x3Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
|
||||
return ::blockNxNSolver<3>( a, b, manifold, dt );
|
||||
}
|
||||
void block4x4Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
|
||||
return ::blockNxNSolver<4>( a, b, manifold, dt );
|
||||
}
|
||||
}
|
||||
73
engine/src/utils/math/physics/solvers/iterativeImpulse.inl
Normal file
73
engine/src/utils/math/physics/solvers/iterativeImpulse.inl
Normal file
@ -0,0 +1,73 @@
|
||||
namespace {
|
||||
void iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Contact& contact, float dt ) {
|
||||
// relative positions from centers to contact point
|
||||
pod::Vector3f rA = contact.point - ::getPosition( a, true );
|
||||
pod::Vector3f rB = contact.point - ::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);
|
||||
|
||||
// nullify restitution if velocity is small enough
|
||||
if ( fabs(velAlongNormal) < 1.0f) e = 0.0f;
|
||||
|
||||
// effective inverse mass along normal
|
||||
float invMassN = ::computeEffectiveMass(a, b, rA, rB, contact.normal);
|
||||
|
||||
// normal impulse scalar
|
||||
float jn = -(1.0f + e) * velAlongNormal;
|
||||
jn /= invMassN;
|
||||
if ( uf::physics::impl::settings.warmupSolver ) {
|
||||
float jnOld = contact.accumulatedNormalImpulse;
|
||||
float jnNew = std::max(0.0f, jnOld + jn);
|
||||
float jnDelta = jnNew - jnOld;
|
||||
contact.accumulatedNormalImpulse = jnNew;
|
||||
jn = jnDelta;
|
||||
}
|
||||
|
||||
::applyImpulseTo(a, b, rA, rB, contact.normal * jn);
|
||||
|
||||
// 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 = ::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
|
||||
|
||||
if ( uf::physics::impl::settings.warmupSolver ) {
|
||||
float maxFriction = mu_s * contact.accumulatedNormalImpulse;
|
||||
float jtOld = contact.accumulatedTangentImpulse;
|
||||
float jtNew = std::max(-maxFriction, std::min(jtOld + jt, maxFriction));
|
||||
float jtDelta = jtNew - jtOld;
|
||||
contact.accumulatedTangentImpulse = jtNew;
|
||||
contact.tangent = tangent;
|
||||
jt = jtDelta;
|
||||
}
|
||||
|
||||
::applyImpulseTo(a, b, rA, rB, tangent * jt);
|
||||
}
|
||||
}
|
||||
}
|
||||
100
engine/src/utils/math/physics/solvers/psg.inl
Normal file
100
engine/src/utils/math/physics/solvers/psg.inl
Normal file
@ -0,0 +1,100 @@
|
||||
namespace {
|
||||
struct ContactCache {
|
||||
pod::Vector3f normal;
|
||||
pod::Vector3f tangent;
|
||||
pod::Vector3f rA, rB;
|
||||
float bias;
|
||||
float effectiveMassN;
|
||||
float effectiveMassT;
|
||||
float accumulatedNormalImpulse;
|
||||
float accumulatedTangentImpulse;
|
||||
};
|
||||
|
||||
void 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 );
|
||||
|
||||
// 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 = ::computeTangent( c.normal );
|
||||
cc.rA = c.point - ::getPosition( a, true );
|
||||
cc.rB = c.point - ::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::impl::settings.baumgarteCorrectionSlop, 0.0f ) * (uf::physics::impl::settings.baumgarteCorrectionPercent / dt);
|
||||
float restitutionBias = (vn < -1.0f) ? -e * vn : 0.0f;
|
||||
cc.bias = restitutionBias + penetrationBias;
|
||||
|
||||
// effective mass (normal)
|
||||
pod::Vector3f rnA = uf::vector::cross( cc.rA, cc.normal );
|
||||
pod::Vector3f rnB = uf::vector::cross( cc.rB, cc.normal );
|
||||
float Kn = (a.isStatic ? 0.0f : a.inverseMass) + (b.isStatic ? 0.0f : b.inverseMass) +
|
||||
uf::vector::dot( uf::vector::cross( rnA * a.inverseInertiaTensor, cc.rA ) + uf::vector::cross( rnB * b.inverseInertiaTensor, 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 );
|
||||
float Kt = (a.isStatic ? 0.0f : a.inverseMass) + (b.isStatic ? 0.0f : b.inverseMass) +
|
||||
uf::vector::dot( uf::vector::cross( rtA * a.inverseInertiaTensor, cc.rA ) + uf::vector::cross( rtB * b.inverseInertiaTensor, 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;
|
||||
|
||||
::applyImpulseTo(a, b, cc.rA, cc.rB, P);
|
||||
#endif
|
||||
}
|
||||
|
||||
// iterative PGS
|
||||
for ( auto iter = 0; iter < uf::physics::impl::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;
|
||||
|
||||
::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;
|
||||
|
||||
::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;
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user