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

correctly initiate/terminate contact constraints

Browse Source
This commit is contained in:
Irlan 2018-05-16 02:08:45 -03:00
parent 4804e48f0b
commit d8826c751e
3 changed files with 176 additions and 168 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
include/bounce/dynamics/cloth/spring_solver.h
View File

@ -22,6 +22,8 @@
#include <bounce/common/math/vec3.h> #include <bounce/common/math/vec3.h>
#include <bounce/common/math/mat33.h> #include <bounce/common/math/mat33.h>
class b3Shape;
class b3SpringCloth; class b3SpringCloth;
class b3StackAllocator; class b3StackAllocator;
@ -56,15 +58,21 @@ private:
// Compute A and b in Ax = b // Compute A and b in Ax = b
void Compute_A_b(b3SparseMat33& A, b3DenseVec3& b) const; void Compute_A_b(b3SparseMat33& A, b3DenseVec3& b) const;
// Compute the initial guess for the iterative solver.
void Compute_x0(b3DenseVec3& x0);
// Compute the constraint projection matrix S.
void Compute_S(b3Mat33* S);
// Solve Ax = b using the Modified Conjugate Gradient (MCG). // Solve Ax = b using the Modified Conjugate Gradient (MCG).
// Output x and the residual error f. // Output x and the residual error f.
void Solve_MCG(b3DenseVec3& x, b3DenseVec3& f, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b) const; void Solve_MCG(b3DenseVec3& x0, b3DenseVec3& f, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3Mat33* S) const;
// Solve Ax = b using MCG with Jacobi preconditioning. // Solve Ax = b using MCG with Jacobi preconditioning.
// Output x and the residual error f. // Output x and the residual error f.
// This method is slower than MCG because we have to compute the preconditioning // This method is slower than MCG because we have to compute the preconditioning
// matrix P, but it can improve convergence. // matrix P, but it can improve convergence.
void Solve_MPCG(b3DenseVec3& x, b3DenseVec3& f, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b) const; void Solve_MPCG(b3DenseVec3& x0, b3DenseVec3& f, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3Mat33* S) const;
b3SpringCloth * m_cloth; b3SpringCloth * m_cloth;
float32 m_h; float32 m_h;
@ -87,6 +95,8 @@ private:
b3Spring* m_springs; b3Spring* m_springs;
u32 m_springCount; u32 m_springCount;
b3Shape** m_shapes;
}; };
inline u32 b3SpringSolver::GetIterations() const inline u32 b3SpringSolver::GetIterations() const

180
src/bounce/dynamics/cloth/spring_cloth.cpp
View File

@ -25,7 +25,7 @@
#include <bounce/common/memory/stack_allocator.h> #include <bounce/common/memory/stack_allocator.h>
#include <bounce/common/draw.h> #include <bounce/common/draw.h>
#define B3_FORCE_THRESHOLD (0.1f) #define B3_FORCE_THRESHOLD 0.0f
#define B3_CLOTH_BENDING 0 #define B3_CLOTH_BENDING 0
@ -344,7 +344,7 @@ void b3SpringCloth::GetTension(b3Array<b3Vec3>& T) const
} }
} }
static B3_FORCE_INLINE void b3MakeTangents(b3Vec3& t1, b3Vec3& t2, const b3Vec3& dv, const b3Vec3& n) static B3_FORCE_INLINE void b3CreateTangents(b3Vec3& t1, b3Vec3& t2, const b3Vec3& dv, const b3Vec3& n)
{ {
t1 = dv - b3Dot(dv, n) * n; t1 = dv - b3Dot(dv, n) * n;
if (b3Dot(t1, t1) > B3_EPSILON * B3_EPSILON) if (b3Dot(t1, t1) > B3_EPSILON * B3_EPSILON)
@ -369,9 +369,18 @@ void b3SpringCloth::UpdateContacts()
continue; continue;
} }
// Relative velocity
b3Vec3 dv = m_v[i];
b3MassContact* c = m_contacts + i; b3MassContact* c = m_contacts + i;
bool wasLockedN = c->lockN; // Save the old contact
b3MassContact c0 = *c;
// Create a new contact
c->lockN = false;
c->lockT1 = false;
c->lockT2 = false;
b3Sphere s1; b3Sphere s1;
s1.vertex = m_x[i]; s1.vertex = m_x[i];
@ -384,13 +393,13 @@ void b3SpringCloth::UpdateContacts()
for (u32 j = 0; j < m_shapeCount; ++j) for (u32 j = 0; j < m_shapeCount; ++j)
{ {
b3Shape* shape = m_shapes[j]; b3Shape* s2 = m_shapes[j];
b3Transform xf2; b3Transform xf2;
xf2.SetIdentity(); xf2.SetIdentity();
b3TestSphereOutput output; b3TestSphereOutput output;
if (shape->TestSphere(&output, s1, xf2) == false) if (s2->TestSphere(&output, s1, xf2) == false)
{ {
continue; continue;
} }
@ -403,123 +412,121 @@ void b3SpringCloth::UpdateContacts()
} }
} }
if (bestIndex == ~0) if (bestIndex != ~0)
{ {
c->Fn = 0.0f;
c->Ft1 = 0.0f;
c->Ft2 = 0.0f;
c->lockN = false;
c->lockT1 = false;
c->lockT2 = false;
continue;
}
B3_ASSERT(bestSeparation <= 0.0f); B3_ASSERT(bestSeparation <= 0.0f);
b3Shape* shape = m_shapes[bestIndex]; b3Shape* shape = m_shapes[bestIndex];
float32 s = bestSeparation; float32 s = bestSeparation;
b3Vec3 n = bestNormal; b3Vec3 n = bestNormal;
// Update contact manifold
// Here the normal orientation is from the shape 2 (mass) to shape 1
c->j = bestIndex;
c->n = n;
c->lockN = true;
// Apply position correction // Apply position correction
m_y[i] -= s * n; m_y[i] -= s * n;
}
// Update contact state // Update contact state
if (wasLockedN) if (c0.lockN == true && c->lockN == true)
{ {
// Was the contact force attractive? // The contact persists
if (c->Fn < B3_FORCE_THRESHOLD)
// Is the contact constraint still violated?
if (c0.Fn <= -B3_FORCE_THRESHOLD)
{ {
// Contact force is attractive.
// Terminate the contact. // Terminate the contact.
c->Fn = 0.0f;
c->Ft1 = 0.0f;
c->Ft2 = 0.0f;
c->lockN = false; c->lockN = false;
c->lockT1 = false; }
c->lockT2 = false;
continue;
} }
// Since the contact force was repulsive #if 0
// maintain the normal acceleration constraint. // Notify the new contact state
c->j = bestIndex; if (wasLockedN == false && c->lockN == true)
c->n = n;
}
else
{ {
// The contact has began. // The contact has begun
c->j = bestIndex; }
c->n = n;
c->Fn = 0.0f; if (wasLockedN == true && c->lockN == false)
c->Ft1 = 0.0f; {
c->Ft2 = 0.0f; // The contact has ended
c->lockN = true; }
c->lockT1 = false; #endif
c->lockT2 = false; if (c->lockN == false)
{
continue;
} }
#if B3_CLOTH_FRICTION == 1 #if B3_CLOTH_FRICTION == 1
// Apply friction impulses // A friction force requires an associated normal force.
if (c0.lockN == false)
{
continue;
}
// Relative velocity b3Shape* s = m_shapes[c->j];
b3Vec3 dv = m_v[i]; b3Vec3 n = c->n;
float32 friction = s->GetFriction();
float32 normalForce = c0.Fn;
b3MakeTangents(c->t1, c->t2, dv, n); // Tangents
b3CreateTangents(c->t1, c->t2, dv, n);
// Note without a friction force, the tangential acceleration won't be
// removed.
// Coefficients of friction for the solid
const float32 uk = shape->GetFriction();
const float32 us = 2.0f * uk;
float32 dvn = b3Dot(dv, n);
float32 normalImpulse = -m_inv_m[i] * dvn;
b3Vec3 ts[2]; b3Vec3 ts[2];
ts[0] = c->t1; ts[0] = c->t1;
ts[1] = c->t2; ts[1] = c->t2;
bool lockT[2]; bool lockT[2];
lockT[0] = c->lockT1;
lockT[1] = c->lockT2;
bool lockT0[2];
lockT0[0] = c0.lockT1;
lockT0[1] = c0.lockT2;
float32 Ft0[2];
Ft0[0] = c0.Ft1;
Ft0[1] = c0.Ft2;
for (u32 k = 0; k < 2; ++k) for (u32 k = 0; k < 2; ++k)
{ {
b3Vec3 t = ts[k]; b3Vec3 t = ts[k];
// Relative tangential velocity
float32 dvt = b3Dot(dv, t); float32 dvt = b3Dot(dv, t);
float32 tangentImpulse = -m_inv_m[i] * dvt;
float32 maxStaticImpulse = us * normalImpulse; if (dvt * dvt <= B3_EPSILON * B3_EPSILON)
if (tangentImpulse * tangentImpulse > maxStaticImpulse * maxStaticImpulse)
{
lockT[k] = false;
// Dynamic friction
float32 maxDynamicImpulse = uk * normalImpulse;
if (tangentImpulse * tangentImpulse > maxDynamicImpulse * maxDynamicImpulse)
{
b3Vec3 P = tangentImpulse * t;
m_v[i] += m_m[i] * P;
}
}
else
{ {
// Lock mass on surface
lockT[k] = true; lockT[k] = true;
}
// Static friction if (lockT0[k] == true && lockT[k] == true)
b3Vec3 P = tangentImpulse * t; {
// The contact persists
float32 maxForce = friction * normalForce;
m_v[i] += m_m[i] * P; if (Ft0[k] * Ft0[k] > maxForce * maxForce)
{
// Unlock mass off surface
lockT[k] = false;
}
} }
} }
c->lockT1 = lockT[0]; c->lockT1 = lockT[0];
c->lockT2 = lockT[1]; c->lockT2 = lockT[1];
#endif // #if B3_CLOTH_FRICTION #endif
} }
} }
void b3SpringCloth::Step(float32 dt) void b3SpringCloth::Step(float32 dt)
@ -532,7 +539,7 @@ void b3SpringCloth::Step(float32 dt)
// Update contacts // Update contacts
UpdateContacts(); UpdateContacts();
// Apply gravity forces // Apply weights
for (u32 i = 0; i < m_massCount; ++i) for (u32 i = 0; i < m_massCount; ++i)
{ {
if (m_types[i] == b3MassType::e_dynamicMass) if (m_types[i] == b3MassType::e_dynamicMass)
@ -560,14 +567,19 @@ void b3SpringCloth::Step(float32 dt)
// Store constraint forces for physics logic // Store constraint forces for physics logic
for (u32 i = 0; i < m_massCount; ++i) for (u32 i = 0; i < m_massCount; ++i)
{ {
b3Vec3 force = forces[i];
b3MassContact* c = m_contacts + i; b3MassContact* c = m_contacts + i;
if (c->lockN == false)
{
continue;
}
b3Vec3 force = forces[i];
// Signed normal force magnitude // Signed normal force magnitude
c->Fn = b3Dot(force, c->n); c->Fn = b3Dot(force, c->n);
// Signed tangent forces magnitude // Signed tangent force magnitude
c->Ft1 = b3Dot(force, c->t1); c->Ft1 = b3Dot(force, c->t1);
c->Ft2 = b3Dot(force, c->t2); c->Ft2 = b3Dot(force, c->t2);
} }
@ -598,23 +610,9 @@ void b3SpringCloth::Draw() const
const b3Mesh* m = m_mesh; const b3Mesh* m = m_mesh;
for (u32 i = 0; i < m->vertexCount; ++i) for (u32 i = 0; i < m->vertexCount; ++i)
{
if (m_contacts[i].lockN)
{
if (m_contacts[i].Fn < B3_FORCE_THRESHOLD)
{
b3Draw_draw->DrawPoint(m_x[i], 6.0f, b3Color_yellow);
}
else
{
b3Draw_draw->DrawPoint(m_x[i], 6.0f, b3Color_red);
}
}
else
{ {
b3Draw_draw->DrawPoint(m_x[i], 6.0f, b3Color_green); b3Draw_draw->DrawPoint(m_x[i], 6.0f, b3Color_green);
} }
}
for (u32 i = 0; i < m->triangleCount; ++i) for (u32 i = 0; i < m->triangleCount; ++i)
{ {

86
src/bounce/dynamics/cloth/spring_solver.cpp
View File

@ -20,6 +20,7 @@
#include <bounce/dynamics/cloth/spring_cloth.h> #include <bounce/dynamics/cloth/spring_cloth.h>
#include <bounce/dynamics/cloth/dense_vec3.h> #include <bounce/dynamics/cloth/dense_vec3.h>
#include <bounce/dynamics/cloth/sparse_mat33.h> #include <bounce/dynamics/cloth/sparse_mat33.h>
#include <bounce/dynamics/shapes/shape.h>
#include <bounce/common/memory/stack_allocator.h> #include <bounce/common/memory/stack_allocator.h>
// Here, we solve Ax = b using the Modified Conjugate Gradient method. // Here, we solve Ax = b using the Modified Conjugate Gradient method.
@ -53,6 +54,8 @@ b3SpringSolver::b3SpringSolver(const b3SpringSolverDef& def)
m_springs = m_cloth->m_springs; m_springs = m_cloth->m_springs;
m_springCount = m_cloth->m_springCount; m_springCount = m_cloth->m_springCount;
m_shapes = m_cloth->m_shapes;
} }
b3SpringSolver::~b3SpringSolver() b3SpringSolver::~b3SpringSolver()
@ -66,7 +69,7 @@ void b3SpringSolver::Solve(b3DenseVec3& f)
m_Jx = (b3Mat33*)m_allocator->Allocate(m_springCount * sizeof(b3Mat33)); m_Jx = (b3Mat33*)m_allocator->Allocate(m_springCount * sizeof(b3Mat33));
m_Jv = (b3Mat33*)m_allocator->Allocate(m_springCount * sizeof(b3Mat33)); m_Jv = (b3Mat33*)m_allocator->Allocate(m_springCount * sizeof(b3Mat33));
// Apply spring forces. Also, store their unique derivatives. // Apply internal forces. Also, store their unique derivatives.
ApplySpringForces(); ApplySpringForces();
// Integrate // Integrate
@ -87,19 +90,26 @@ void b3SpringSolver::Solve(b3DenseVec3& f)
// //
b3DenseVec3 b(m_massCount); b3DenseVec3 b(m_massCount);
// // A, b
Compute_A_b(A, b); Compute_A_b(A, b);
// x // x
b3DenseVec3 x(m_massCount); b3DenseVec3 x(m_massCount);
// x0
Compute_x0(x);
// S
b3Mat33* S = (b3Mat33*)m_allocator->Allocate(m_massCount * sizeof(b3Mat33));
Compute_S(S);
if (b3_enablePrecontitioning) if (b3_enablePrecontitioning)
{ {
Solve_MPCG(x, f, m_iterations, A, b); Solve_MPCG(x, f, m_iterations, A, b, S);
} }
else else
{ {
Solve_MCG(x, f, m_iterations, A, b); Solve_MCG(x, f, m_iterations, A, b, S);
} }
// Update state // Update state
@ -110,6 +120,8 @@ void b3SpringSolver::Solve(b3DenseVec3& f)
// dx = h * (v0 + dv) + y = h * v1 + y // dx = h * (v0 + dv) + y = h * v1 + y
m_x[i] += m_h * m_v[i] + m_y[i]; m_x[i] += m_h * m_v[i] + m_y[i];
} }
m_allocator->Free(S);
} }
m_allocator->Free(rowPtrs); m_allocator->Free(rowPtrs);
@ -176,7 +188,7 @@ void b3SpringSolver::ApplySpringForces()
b3SetZero_Jacobian(m_Jx, m_springCount); b3SetZero_Jacobian(m_Jx, m_springCount);
b3SetZero_Jacobian(m_Jv, m_springCount); b3SetZero_Jacobian(m_Jv, m_springCount);
// Compute forces and Jacobians // Compute spring forces and Jacobians
for (u32 i = 0; i < m_springCount; ++i) for (u32 i = 0; i < m_springCount; ++i)
{ {
b3Spring* S = m_springs + i; b3Spring* S = m_springs + i;
@ -234,9 +246,9 @@ void b3SpringSolver::ApplySpringForces()
static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A) static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A)
{ {
bool isZeroX = b3Dot(A.x, A.x) <= B3_EPSILON * B3_EPSILON; bool isZeroX = b3Dot(A.x, A.x) == 0.0f;
bool isZeroY = b3Dot(A.y, A.y) <= B3_EPSILON * B3_EPSILON; bool isZeroY = b3Dot(A.y, A.y) == 0.0f;
bool isZeroZ = b3Dot(A.z, A.z) <= B3_EPSILON * B3_EPSILON; bool isZeroZ = b3Dot(A.z, A.z) == 0.0f;
return isZeroX * isZeroY * isZeroZ; return isZeroX * isZeroY * isZeroZ;
} }
@ -366,20 +378,16 @@ void b3SpringSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b) const
m_allocator->Free(dfdx); m_allocator->Free(dfdx);
} }
// This outputs the desired acceleration of the masses in the constrained void b3SpringSolver::Compute_x0(b3DenseVec3& x0)
// directions.
static void b3Compute_z(b3DenseVec3& out,
u32 massCount, const b3MassType* types, const b3MassContact* contacts)
{ {
out.SetZero(); x0.SetZero();
} }
// void b3SpringSolver::Compute_S(b3Mat33* out)
static void b3Compute_S(b3Mat33* out, u32 massCount, const b3MassType* types, const b3MassContact* contacts)
{ {
for (u32 i = 0; i < massCount; ++i) for (u32 i = 0; i < m_massCount; ++i)
{ {
switch (types[i]) switch (m_types[i])
{ {
case b3MassType::e_staticMass: case b3MassType::e_staticMass:
{ {
@ -388,25 +396,32 @@ static void b3Compute_S(b3Mat33* out, u32 massCount, const b3MassType* types, co
} }
case b3MassType::e_dynamicMass: case b3MassType::e_dynamicMass:
{ {
if (contacts[i].lockN == true) if (m_contacts[i].lockN == true)
{ {
b3Vec3 n = contacts[i].n; b3Vec3 n = m_contacts[i].n;
b3Mat33 S = b3Mat33_identity - b3Outer(n, n); b3Mat33 S = b3Mat33_identity - b3Outer(n, n);
if (contacts[i].lockT1 == true) if (m_contacts[i].lockT1 == true && m_contacts[i].lockT2 == true)
{ {
b3Vec3 t1 = contacts[i].t1; S.SetZero();
}
else
{
if (m_contacts[i].lockT1 == true)
{
b3Vec3 t1 = m_contacts[i].t2;
S -= b3Outer(t1, t1); S -= b3Outer(t1, t1);
} }
if (contacts[i].lockT2 == true) if (m_contacts[i].lockT2 == true)
{ {
b3Vec3 t2 = contacts[i].t2; b3Vec3 t2 = m_contacts[i].t2;
S -= b3Outer(t2, t2); S -= b3Outer(t2, t2);
} }
}
out[i] = S; out[i] = S;
break; break;
@ -434,15 +449,8 @@ static void b3Filter(b3DenseVec3& out,
} }
} }
void b3SpringSolver::Solve_MCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b) const void b3SpringSolver::Solve_MCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3Mat33* S) const
{ {
//
b3Mat33* S = (b3Mat33*)m_allocator->Allocate(m_massCount * sizeof(b3Mat33));
b3Compute_S(S, m_massCount, m_types, m_contacts);
// dv = z
b3Compute_z(dv, m_massCount, m_types, m_contacts);
// r = filter(b - Adv) // r = filter(b - Adv)
b3DenseVec3 r = b - A * dv; b3DenseVec3 r = b - A * dv;
b3Filter(r, r, S, m_massCount); b3Filter(r, r, S, m_massCount);
@ -494,8 +502,6 @@ void b3SpringSolver::Solve_MCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations,
++iter; ++iter;
} }
m_allocator->Free(S);
iterations = iter; iterations = iter;
// Residual error // Residual error
@ -522,15 +528,8 @@ static bool b3IsPD(const b3Mat33* diagA, u32 n)
return true; return true;
} }
void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b) const void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3Mat33* S) const
{ {
// S
b3Mat33* S = (b3Mat33*)m_allocator->Allocate(m_massCount * sizeof(b3Mat33));
b3Compute_S(S, m_massCount, m_types, m_contacts);
// dv = z
b3Compute_z(dv, m_massCount, m_types, m_contacts);
// P = diag(A)^-1 // P = diag(A)^-1
b3DenseVec3 P(m_massCount); b3DenseVec3 P(m_massCount);
@ -610,6 +609,7 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations
b3Filter(q, q, S, m_massCount); b3Filter(q, q, S, m_massCount);
// alpha = epsNew / dot(c, q) // alpha = epsNew / dot(c, q)
B3_ASSERT(b3IsValid(b3Dot(c, q)));
float32 alpha = epsNew / b3Dot(c, q); float32 alpha = epsNew / b3Dot(c, q);
// dv = dv + alpha * c // dv = dv + alpha * c
@ -629,9 +629,11 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations
// epsOld = epsNew // epsOld = epsNew
float32 epsOld = epsNew; float32 epsOld = epsNew;
B3_ASSERT(b3IsValid(epsOld));
// epsNew = dot(r, s) // epsNew = dot(r, s)
epsNew = b3Dot(r, s); epsNew = b3Dot(r, s);
B3_ASSERT(b3IsValid(epsNew));
// beta = epsNew / epsOld // beta = epsNew / epsOld
float32 beta = epsNew / epsOld; float32 beta = epsNew / epsOld;
@ -643,8 +645,6 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations
++iter; ++iter;
} }
m_allocator->Free(S);
iterations = iter; iterations = iter;
// Residual error // Residual error