correctly initiate/terminate contact constraints
This commit is contained in:
Irlan
parent
4804e48f0b
commit
d8826c751e
@ -22,6 +22,8 @@
|
||||
#include <bounce/common/math/vec3.h>
|
||||
#include <bounce/common/math/mat33.h>
|
||||
|
||||
class b3Shape;
|
||||
|
||||
class b3SpringCloth;
|
||||
class b3StackAllocator;
|
||||
|
||||
@ -56,15 +58,21 @@ private:
|
||||
// Compute A and b in Ax = b
|
||||
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).
|
||||
// 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.
|
||||
// Output x and the residual error f.
|
||||
// This method is slower than MCG because we have to compute the preconditioning
|
||||
// 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;
|
||||
float32 m_h;
|
||||
@ -87,6 +95,8 @@ private:
|
||||
|
||||
b3Spring* m_springs;
|
||||
u32 m_springCount;
|
||||
|
||||
b3Shape** m_shapes;
|
||||
};
|
||||
|
||||
inline u32 b3SpringSolver::GetIterations() const
|
||||
|
||||
@ -25,7 +25,7 @@
|
||||
#include <bounce/common/memory/stack_allocator.h>
|
||||
#include <bounce/common/draw.h>
|
||||
|
||||
#define B3_FORCE_THRESHOLD (0.1f)
|
||||
#define B3_FORCE_THRESHOLD 0.0f
|
||||
|
||||
#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;
|
||||
if (b3Dot(t1, t1) > B3_EPSILON * B3_EPSILON)
|
||||
@ -369,9 +369,18 @@ void b3SpringCloth::UpdateContacts()
|
||||
continue;
|
||||
}
|
||||
|
||||
// Relative velocity
|
||||
b3Vec3 dv = m_v[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;
|
||||
s1.vertex = m_x[i];
|
||||
@ -384,13 +393,13 @@ void b3SpringCloth::UpdateContacts()
|
||||
|
||||
for (u32 j = 0; j < m_shapeCount; ++j)
|
||||
{
|
||||
b3Shape* shape = m_shapes[j];
|
||||
b3Shape* s2 = m_shapes[j];
|
||||
|
||||
b3Transform xf2;
|
||||
xf2.SetIdentity();
|
||||
|
||||
b3TestSphereOutput output;
|
||||
if (shape->TestSphere(&output, s1, xf2) == false)
|
||||
if (s2->TestSphere(&output, s1, xf2) == false)
|
||||
{
|
||||
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);
|
||||
|
||||
b3Shape* shape = m_shapes[bestIndex];
|
||||
float32 s = bestSeparation;
|
||||
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
|
||||
m_y[i] -= s * n;
|
||||
}
|
||||
|
||||
B3_ASSERT(bestSeparation <= 0.0f);
|
||||
|
||||
b3Shape* shape = m_shapes[bestIndex];
|
||||
float32 s = bestSeparation;
|
||||
b3Vec3 n = bestNormal;
|
||||
|
||||
// Apply position correction
|
||||
m_y[i] -= s * n;
|
||||
|
||||
// Update contact state
|
||||
if (wasLockedN)
|
||||
if (c0.lockN == true && c->lockN == true)
|
||||
{
|
||||
// Was the contact force attractive?
|
||||
if (c->Fn < B3_FORCE_THRESHOLD)
|
||||
// The contact persists
|
||||
|
||||
// Is the contact constraint still violated?
|
||||
if (c0.Fn <= -B3_FORCE_THRESHOLD)
|
||||
{
|
||||
// Terminate the contact.
|
||||
c->Fn = 0.0f;
|
||||
c->Ft1 = 0.0f;
|
||||
c->Ft2 = 0.0f;
|
||||
c->lockN = false;
|
||||
c->lockT1 = false;
|
||||
c->lockT2 = false;
|
||||
continue;
|
||||
}
|
||||
// Contact force is attractive.
|
||||
|
||||
// Since the contact force was repulsive
|
||||
// maintain the normal acceleration constraint.
|
||||
c->j = bestIndex;
|
||||
c->n = n;
|
||||
// Terminate the contact.
|
||||
c->lockN = false;
|
||||
}
|
||||
}
|
||||
else
|
||||
|
||||
#if 0
|
||||
// Notify the new contact state
|
||||
if (wasLockedN == false && c->lockN == true)
|
||||
{
|
||||
// The contact has began.
|
||||
c->j = bestIndex;
|
||||
c->n = n;
|
||||
c->Fn = 0.0f;
|
||||
c->Ft1 = 0.0f;
|
||||
c->Ft2 = 0.0f;
|
||||
c->lockN = true;
|
||||
c->lockT1 = false;
|
||||
c->lockT2 = false;
|
||||
// The contact has begun
|
||||
}
|
||||
|
||||
if (wasLockedN == true && c->lockN == false)
|
||||
{
|
||||
// The contact has ended
|
||||
}
|
||||
#endif
|
||||
if (c->lockN == false)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
#if B3_CLOTH_FRICTION == 1
|
||||
|
||||
// Apply friction impulses
|
||||
// A friction force requires an associated normal force.
|
||||
if (c0.lockN == false)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// Relative velocity
|
||||
b3Vec3 dv = m_v[i];
|
||||
b3Shape* s = m_shapes[c->j];
|
||||
b3Vec3 n = c->n;
|
||||
float32 friction = s->GetFriction();
|
||||
float32 normalForce = c0.Fn;
|
||||
|
||||
b3MakeTangents(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;
|
||||
// Tangents
|
||||
b3CreateTangents(c->t1, c->t2, dv, n);
|
||||
|
||||
b3Vec3 ts[2];
|
||||
ts[0] = c->t1;
|
||||
ts[1] = c->t2;
|
||||
|
||||
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)
|
||||
{
|
||||
b3Vec3 t = ts[k];
|
||||
|
||||
// Relative tangential velocity
|
||||
float32 dvt = b3Dot(dv, t);
|
||||
float32 tangentImpulse = -m_inv_m[i] * dvt;
|
||||
|
||||
float32 maxStaticImpulse = us * normalImpulse;
|
||||
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
|
||||
if (dvt * dvt <= B3_EPSILON * B3_EPSILON)
|
||||
{
|
||||
// Lock mass on surface
|
||||
lockT[k] = true;
|
||||
}
|
||||
|
||||
// Static friction
|
||||
b3Vec3 P = tangentImpulse * t;
|
||||
|
||||
m_v[i] += m_m[i] * P;
|
||||
if (lockT0[k] == true && lockT[k] == true)
|
||||
{
|
||||
// The contact persists
|
||||
float32 maxForce = friction * normalForce;
|
||||
|
||||
if (Ft0[k] * Ft0[k] > maxForce * maxForce)
|
||||
{
|
||||
// Unlock mass off surface
|
||||
lockT[k] = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
c->lockT1 = lockT[0];
|
||||
c->lockT2 = lockT[1];
|
||||
|
||||
#endif // #if B3_CLOTH_FRICTION
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void b3SpringCloth::Step(float32 dt)
|
||||
@ -532,7 +539,7 @@ void b3SpringCloth::Step(float32 dt)
|
||||
// Update contacts
|
||||
UpdateContacts();
|
||||
|
||||
// Apply gravity forces
|
||||
// Apply weights
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
if (m_types[i] == b3MassType::e_dynamicMass)
|
||||
@ -560,14 +567,19 @@ void b3SpringCloth::Step(float32 dt)
|
||||
// Store constraint forces for physics logic
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
b3Vec3 force = forces[i];
|
||||
|
||||
b3MassContact* c = m_contacts + i;
|
||||
|
||||
if (c->lockN == false)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
b3Vec3 force = forces[i];
|
||||
|
||||
// Signed normal force magnitude
|
||||
c->Fn = b3Dot(force, c->n);
|
||||
|
||||
// Signed tangent forces magnitude
|
||||
// Signed tangent force magnitude
|
||||
c->Ft1 = b3Dot(force, c->t1);
|
||||
c->Ft2 = b3Dot(force, c->t2);
|
||||
}
|
||||
@ -599,21 +611,7 @@ void b3SpringCloth::Draw() const
|
||||
|
||||
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)
|
||||
|
||||
@ -20,6 +20,7 @@
|
||||
#include <bounce/dynamics/cloth/spring_cloth.h>
|
||||
#include <bounce/dynamics/cloth/dense_vec3.h>
|
||||
#include <bounce/dynamics/cloth/sparse_mat33.h>
|
||||
#include <bounce/dynamics/shapes/shape.h>
|
||||
#include <bounce/common/memory/stack_allocator.h>
|
||||
|
||||
// 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_springCount = m_cloth->m_springCount;
|
||||
|
||||
m_shapes = m_cloth->m_shapes;
|
||||
}
|
||||
|
||||
b3SpringSolver::~b3SpringSolver()
|
||||
@ -66,7 +69,7 @@ void b3SpringSolver::Solve(b3DenseVec3& f)
|
||||
m_Jx = (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();
|
||||
|
||||
// Integrate
|
||||
@ -86,20 +89,27 @@ void b3SpringSolver::Solve(b3DenseVec3& f)
|
||||
|
||||
//
|
||||
b3DenseVec3 b(m_massCount);
|
||||
|
||||
//
|
||||
|
||||
// A, b
|
||||
Compute_A_b(A, b);
|
||||
|
||||
// x
|
||||
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)
|
||||
{
|
||||
Solve_MPCG(x, f, m_iterations, A, b);
|
||||
Solve_MPCG(x, f, m_iterations, A, b, S);
|
||||
}
|
||||
else
|
||||
{
|
||||
Solve_MCG(x, f, m_iterations, A, b);
|
||||
Solve_MCG(x, f, m_iterations, A, b, S);
|
||||
}
|
||||
|
||||
// Update state
|
||||
@ -110,6 +120,8 @@ void b3SpringSolver::Solve(b3DenseVec3& f)
|
||||
// dx = h * (v0 + dv) + y = h * v1 + y
|
||||
m_x[i] += m_h * m_v[i] + m_y[i];
|
||||
}
|
||||
|
||||
m_allocator->Free(S);
|
||||
}
|
||||
|
||||
m_allocator->Free(rowPtrs);
|
||||
@ -176,7 +188,7 @@ void b3SpringSolver::ApplySpringForces()
|
||||
b3SetZero_Jacobian(m_Jx, 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)
|
||||
{
|
||||
b3Spring* S = m_springs + i;
|
||||
@ -185,7 +197,7 @@ void b3SpringSolver::ApplySpringForces()
|
||||
u32 i1 = S->i1;
|
||||
u32 i2 = S->i2;
|
||||
float32 L0 = S->L0;
|
||||
B3_ASSERT(L0 > 0.0f);
|
||||
B3_ASSERT(L0 > 0.0f);
|
||||
float32 ks = S->ks;
|
||||
float32 kd = S->kd;
|
||||
|
||||
@ -196,7 +208,7 @@ void b3SpringSolver::ApplySpringForces()
|
||||
b3Vec3 v2 = m_v[i2];
|
||||
|
||||
const b3Mat33 I = b3Mat33_identity;
|
||||
|
||||
|
||||
// Strech
|
||||
b3Vec3 dx = x1 - x2;
|
||||
float32 L = b3Length(dx);
|
||||
@ -213,7 +225,7 @@ void b3SpringSolver::ApplySpringForces()
|
||||
m_f[i2] += sf2;
|
||||
|
||||
b3Mat33 Jx11 = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx)));
|
||||
|
||||
|
||||
m_Jx[i] = Jx11;
|
||||
}
|
||||
|
||||
@ -234,9 +246,9 @@ void b3SpringSolver::ApplySpringForces()
|
||||
|
||||
static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A)
|
||||
{
|
||||
bool isZeroX = b3Dot(A.x, A.x) <= B3_EPSILON * B3_EPSILON;
|
||||
bool isZeroY = b3Dot(A.y, A.y) <= B3_EPSILON * B3_EPSILON;
|
||||
bool isZeroZ = b3Dot(A.z, A.z) <= B3_EPSILON * B3_EPSILON;
|
||||
bool isZeroX = b3Dot(A.x, A.x) == 0.0f;
|
||||
bool isZeroY = b3Dot(A.y, A.y) == 0.0f;
|
||||
bool isZeroZ = b3Dot(A.z, A.z) == 0.0f;
|
||||
|
||||
return isZeroX * isZeroY * isZeroZ;
|
||||
}
|
||||
@ -276,7 +288,7 @@ void b3SpringSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b) const
|
||||
dfdv[B3_INDEX(i2, i1, m_massCount)] += Jv21;
|
||||
dfdv[B3_INDEX(i2, i2, m_massCount)] += Jv22;
|
||||
}
|
||||
|
||||
|
||||
// Compute A
|
||||
// A = M - h * dfdv - h * h * dfdx
|
||||
|
||||
@ -298,7 +310,7 @@ void b3SpringSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b) const
|
||||
A[B3_INDEX(i, j, m_massCount)] += (-m_h * dfdv[B3_INDEX(i, j, m_massCount)]) + (-m_h * m_h * dfdx[B3_INDEX(i, j, m_massCount)]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Assembly sparsity
|
||||
u32 nzCount = 0;
|
||||
|
||||
@ -310,7 +322,7 @@ void b3SpringSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b) const
|
||||
{
|
||||
++nzCount;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
SA.row_ptrs[0] = 0;
|
||||
@ -344,8 +356,8 @@ void b3SpringSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b) const
|
||||
m_allocator->Free(A);
|
||||
|
||||
// Compute b
|
||||
// b = h * (f0 + h * Jx_v + Jx_y )
|
||||
|
||||
// b = h * (f0 + h * Jx_v + Jx_y)
|
||||
|
||||
// Jx_v = dfdx * v
|
||||
b3Vec3* Jx_v = (b3Vec3*)m_allocator->Allocate(m_massCount * sizeof(b3Vec3));
|
||||
b3Mul_Jacobian(Jx_v, m_v, m_massCount, m_Jx, m_springs, m_springCount);
|
||||
@ -366,20 +378,16 @@ void b3SpringSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b) const
|
||||
m_allocator->Free(dfdx);
|
||||
}
|
||||
|
||||
// This outputs the desired acceleration of the masses in the constrained
|
||||
// directions.
|
||||
static void b3Compute_z(b3DenseVec3& out,
|
||||
u32 massCount, const b3MassType* types, const b3MassContact* contacts)
|
||||
void b3SpringSolver::Compute_x0(b3DenseVec3& x0)
|
||||
{
|
||||
out.SetZero();
|
||||
x0.SetZero();
|
||||
}
|
||||
|
||||
//
|
||||
static void b3Compute_S(b3Mat33* out, u32 massCount, const b3MassType* types, const b3MassContact* contacts)
|
||||
void b3SpringSolver::Compute_S(b3Mat33* out)
|
||||
{
|
||||
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:
|
||||
{
|
||||
@ -388,24 +396,31 @@ static void b3Compute_S(b3Mat33* out, u32 massCount, const b3MassType* types, co
|
||||
}
|
||||
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);
|
||||
|
||||
if (contacts[i].lockT1 == true)
|
||||
if (m_contacts[i].lockT1 == true && m_contacts[i].lockT2 == true)
|
||||
{
|
||||
b3Vec3 t1 = contacts[i].t1;
|
||||
|
||||
S -= b3Outer(t1, t1);
|
||||
S.SetZero();
|
||||
}
|
||||
|
||||
if (contacts[i].lockT2 == true)
|
||||
else
|
||||
{
|
||||
b3Vec3 t2 = contacts[i].t2;
|
||||
if (m_contacts[i].lockT1 == true)
|
||||
{
|
||||
b3Vec3 t1 = m_contacts[i].t2;
|
||||
|
||||
S -= b3Outer(t1, t1);
|
||||
}
|
||||
|
||||
S -= b3Outer(t2, t2);
|
||||
if (m_contacts[i].lockT2 == true)
|
||||
{
|
||||
b3Vec3 t2 = m_contacts[i].t2;
|
||||
|
||||
S -= b3Outer(t2, t2);
|
||||
}
|
||||
}
|
||||
|
||||
out[i] = S;
|
||||
@ -425,7 +440,7 @@ static void b3Compute_S(b3Mat33* out, u32 massCount, const b3MassType* types, co
|
||||
}
|
||||
|
||||
// Maintains invariants inside the MCG solver.
|
||||
static void b3Filter(b3DenseVec3& out,
|
||||
static void b3Filter(b3DenseVec3& out,
|
||||
const b3DenseVec3& v, const b3Mat33* S, u32 massCount)
|
||||
{
|
||||
for (u32 i = 0; i < massCount; ++i)
|
||||
@ -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)
|
||||
b3DenseVec3 r = b - A * dv;
|
||||
b3Filter(r, r, S, m_massCount);
|
||||
@ -469,7 +477,7 @@ void b3SpringSolver::Solve_MCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations,
|
||||
// q = filter(A * c)
|
||||
b3DenseVec3 q = A * c;
|
||||
b3Filter(q, q, S, m_massCount);
|
||||
|
||||
|
||||
// alpha = epsNew / dot(c, q)
|
||||
float32 alpha = epsNew / b3Dot(c, q);
|
||||
|
||||
@ -494,8 +502,6 @@ void b3SpringSolver::Solve_MCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations,
|
||||
++iter;
|
||||
}
|
||||
|
||||
m_allocator->Free(S);
|
||||
|
||||
iterations = iter;
|
||||
|
||||
// Residual error
|
||||
@ -510,7 +516,7 @@ static bool b3IsPD(const b3Mat33* diagA, u32 n)
|
||||
for (u32 i = 0; i < n; ++i)
|
||||
{
|
||||
b3Mat33 a = diagA[i];
|
||||
|
||||
|
||||
float32 D = b3Det(a.x, a.y, a.z);
|
||||
|
||||
if (D <= B3_EPSILON)
|
||||
@ -522,15 +528,8 @@ static bool b3IsPD(const b3Mat33* diagA, u32 n)
|
||||
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
|
||||
b3DenseVec3 P(m_massCount);
|
||||
|
||||
@ -539,7 +538,7 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations
|
||||
// diag(A)
|
||||
b3Mat33* diagA = (b3Mat33*)m_allocator->Allocate(m_massCount * sizeof(b3Mat33));
|
||||
A.AssembleDiagonal(diagA);
|
||||
|
||||
|
||||
// Compute P, P^-1
|
||||
bool isPD = true;
|
||||
|
||||
@ -563,7 +562,7 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations
|
||||
B3_ASSERT(isPD == true);
|
||||
|
||||
m_allocator->Free(diagA);
|
||||
|
||||
|
||||
// eps0 = dot( filter(b), P * filter(b) )
|
||||
b3DenseVec3 filtered_b(m_massCount);
|
||||
b3Filter(filtered_b, b, S, m_massCount);
|
||||
@ -577,7 +576,7 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations
|
||||
}
|
||||
|
||||
float32 eps0 = b3Dot(filtered_b, P_filtered_b);
|
||||
|
||||
|
||||
// r = filter(b - Adv)
|
||||
b3DenseVec3 r = b - A * dv;
|
||||
b3Filter(r, r, S, m_massCount);
|
||||
@ -591,7 +590,7 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations
|
||||
c[i][2] = inv_P[i][2] * r[i][2];
|
||||
}
|
||||
b3Filter(c, c, S, m_massCount);
|
||||
|
||||
|
||||
// epsNew = dot(r, c)
|
||||
float32 epsNew = b3Dot(r, c);
|
||||
|
||||
@ -610,8 +609,9 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations
|
||||
b3Filter(q, q, S, m_massCount);
|
||||
|
||||
// alpha = epsNew / dot(c, q)
|
||||
B3_ASSERT(b3IsValid(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
|
||||
float32 epsOld = epsNew;
|
||||
B3_ASSERT(b3IsValid(epsOld));
|
||||
|
||||
// epsNew = dot(r, s)
|
||||
epsNew = b3Dot(r, s);
|
||||
B3_ASSERT(b3IsValid(epsNew));
|
||||
|
||||
// beta = epsNew / epsOld
|
||||
float32 beta = epsNew / epsOld;
|
||||
@ -643,8 +645,6 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations
|
||||
++iter;
|
||||
}
|
||||
|
||||
m_allocator->Free(S);
|
||||
|
||||
iterations = iter;
|
||||
|
||||
// Residual error
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user