AnotherFoxGuy/bounce
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

remove baumgarte

Browse Source
This commit is contained in:
Irlan 2018-08-04 15:02:53 -03:00
parent 34c4eb8d49
commit 6aa63996c4
1 changed files with 40 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
src/bounce/dynamics/cloth/cloth_solver.cpp
View File

@ -26,6 +26,7 @@
#include <bounce/dynamics/shapes/shape.h>
#include <bounce/dynamics/body.h>
#include <bounce/common/memory/stack_allocator.h>
#include <bounce/common/math/mat.h>
// Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm.
// described in the paper:
@ -214,6 +215,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
m_solverData.invdt = 1.0f / dt;
float32 h = dt;
float32 inv_h = m_solverData.invdt;
for (u32 i = 0; i < m_particleCount; ++i)
{
@ -306,9 +308,10 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
bool positionSolved = false;
for (u32 i = 0; i < kPositionIterations; ++i)
{
bool bodyContactsSolved = SolveBodyContactPositionConstraints();
bool particleContactsSolved = SolveParticleContactPositionConstraints();
bool triangleContactsSolved = SolveTriangleContactPositionConstraints();
if (particleContactsSolved && triangleContactsSolved)
if (bodyContactsSolved && particleContactsSolved && triangleContactsSolved)
{
positionSolved = true;
break;
@ -445,13 +448,17 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
iterations = iteration;
}
//
static B3_FORCE_INLINE float32 b3MixFriction(float32 u1, float32 u2)
{
return b3Sqrt(u1 * u2);
}
void b3ClothSolver::InitializeBodyContactConstraints()
{
b3DenseVec3& x = *m_solverData.x;
b3DenseVec3& v = *m_solverData.v;
float32 inv_dt = m_solverData.invdt;
for (u32 i = 0; i < m_bodyContactCount; ++i)
{
b3BodyContact* c = m_bodyContacts[i];
@ -467,16 +474,16 @@ void b3ClothSolver::InitializeBodyContactConstraints()
vc->invIA.SetZero();
vc->invIB = vc->bodyB->GetWorldInverseInertia();
vc->friction = c->s2->GetFriction();
vc->friction = b3MixFriction(c->p1->m_friction, c->s2->GetFriction());
pc->indexA = c->p1->m_solverId;
pc->bodyB = vc->bodyB;
pc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass;
pc->invMassB = vc->bodyB->GetInverseMass();
pc->invMassB = vc->bodyB->m_invMass;
pc->invIA.SetZero();
pc->invIB = vc->bodyB->GetWorldInverseInertia();
pc->invIB = vc->bodyB->m_worldInvI;
pc->radiusA = c->p1->m_radius;
pc->radiusB = c->s2->m_radius;
@ -549,10 +556,7 @@ void b3ClothSolver::InitializeBodyContactConstraints()
float32 K = mA + mB + b3Dot(iA * rnA, rnA) + b3Dot(iB * rnB, rnB);
vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f;
float32 C = b3Min(0.0f, wp.separation + B3_LINEAR_SLOP);
vc->velocityBias = -inv_dt * B3_BAUMGARTE * C;
vc->velocityBias = 0.0f;
}
{
@ -579,12 +583,6 @@ void b3ClothSolver::InitializeBodyContactConstraints()
}
}
//
static B3_FORCE_INLINE float32 b3MixFriction(float32 u1, float32 u2)
{
return b3Sqrt(u1 * u2);
}
void b3ClothSolver::InitializeParticleContactConstraints()
{
b3DenseVec3& x = *m_solverData.x;
@ -1109,15 +1107,31 @@ void b3ClothSolver::StoreImpulses()
}
}
struct b3ClothSolverContactSolverPoint
struct b3ClothSolverBodyContactSolverPoint
{
void Initialize(const b3ClothSolverContactPositionConstraint* pc, const b3Transform& xfA, const b3Transform& xfB)
{
normal = b3Mul(xfA.rotation, pc->localNormalA);
b3Vec3 c1 = b3Mul(xfA, pc->localPointA);
b3Vec3 c2 = b3Mul(xfB, pc->localPointB);
point = c2;
separation = b3Dot(c2 - c1, normal) - pc->radiusA - pc->radiusB;
b3Vec3 cA = b3Mul(xfA, pc->localPointA);
b3Vec3 cB = b3Mul(xfB, pc->localPointB);
float32 rA = pc->radiusA;
float32 rB = pc->radiusB;
b3Vec3 d = cB - cA;
float32 distance = b3Length(d);
b3Vec3 nA(0.0f, 1.0f, 0.0f);
if (distance > B3_EPSILON)
{
nA = d / distance;
}
b3Vec3 pA = cA + rA * nA;
b3Vec3 pB = cB - rB * nA;
point = 0.5f * (pA + pB);
normal = nA;
separation = distance - rA - rB;
}
b3Vec3 normal;
@ -1160,7 +1174,7 @@ bool b3ClothSolver::SolveBodyContactPositionConstraints()
xfB.rotation = b3QuatMat33(qB);
xfB.position = cB - b3Mul(xfB.rotation, localCenterB);
b3ClothSolverContactSolverPoint cpcp;
b3ClothSolverBodyContactSolverPoint cpcp;
cpcp.Initialize(pc, xfA, xfB);
b3Vec3 normal = cpcp.normal;
@ -1326,9 +1340,6 @@ bool b3ClothSolver::SolveTriangleContactPositionConstraints()
// Allow some slop and prevent large corrections.
float32 C = b3Clamp(B3_BAUMGARTE * (separation + B3_LINEAR_SLOP), -B3_MAX_LINEAR_CORRECTION, 0.0f);
// Compute effective mass.
float32 K = mA + mB + mC + mD;
b3Mat33 I; I.SetIdentity();
b3Mat33 N = I - b3Outer(n, n);
@ -1344,6 +1355,9 @@ bool b3ClothSolver::SolveTriangleContactPositionConstraints()
b3Vec3 JD = b3Cross(xC - xB, N_n);
b3Vec3 JB = b3Cross(xC - xD, N_n) - n;
// Compute effective mass.
float32 K = mA + mB + mC + mD;
// Compute normal impulse.
float32 impulse = K > 0.0f ? -C / K : 0.0f;