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 )\
|
#define REVERSE_COLLIDER( a, b, fun )\
|
||||||
auto start = manifold.points.size();\
|
auto start = manifold.points.size();\
|
||||||
if ( !::fun( b, a, manifold, eps ) ) return false;\
|
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;\
|
for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal;\
|
||||||
return true;
|
return true;
|
||||||
|
|
||||||
// forward declare
|
// forward declare (to-do: properly handle this)
|
||||||
namespace {
|
namespace {
|
||||||
bool aabbAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS );
|
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 );
|
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
|
#define UF_PHYSICS_TEST 0
|
||||||
|
|
||||||
#include "helpers.inl"
|
#include "helpers.inl"
|
||||||
#include "aabb.inl"
|
|
||||||
#include "sphere.inl"
|
#include "narrowphase/aabb.inl"
|
||||||
#include "plane.inl"
|
#include "narrowphase/sphere.inl"
|
||||||
#include "capsule.inl"
|
#include "narrowphase/plane.inl"
|
||||||
#include "triangle.inl"
|
#include "narrowphase/capsule.inl"
|
||||||
#include "mesh.inl"
|
#include "narrowphase/triangle.inl"
|
||||||
#include "convexHull.inl"
|
#include "narrowphase/mesh.inl"
|
||||||
#include "ray.inl"
|
#include "narrowphase/convexHull.inl"
|
||||||
#include "bvh.inl"
|
#include "narrowphase/ray.inl"
|
||||||
#include "gjk.inl"
|
|
||||||
#include "epa.inl"
|
|
||||||
|
#include "narrowphase/gjk.inl"
|
||||||
|
#include "narrowphase/epa.inl"
|
||||||
|
|
||||||
|
#include "broadphase/bvh.inl"
|
||||||
|
#include "broadphase/island.inl"
|
||||||
|
|
||||||
#include "integration.inl"
|
#include "integration.inl"
|
||||||
#include "solvers.inl"
|
#include "solvers.inl"
|
||||||
|
|
||||||
|
|||||||
@ -1,270 +1,8 @@
|
|||||||
|
#include "./solvers/iterativeImpulse.inl"
|
||||||
|
#include "./solvers/block.inl"
|
||||||
|
#include "./solvers/psg.inl"
|
||||||
|
|
||||||
namespace {
|
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 ) {
|
void resolveManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) {
|
||||||
if ( uf::physics::impl::settings.blockContactSolver ) {
|
if ( uf::physics::impl::settings.blockContactSolver ) {
|
||||||
if ( manifold.points.size() == 2 ) return ::block2x2Solver( a, b, manifold, dt );
|
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