solve forces then solve an lcp, decouple contact constraints
This commit is contained in:
Irlan
parent
927b35a45d
commit
65e5ff217e
@ -197,9 +197,15 @@ public:
|
||||
// Get the mass of the body. Typically in kg/m^3.
|
||||
float32 GetMass() const;
|
||||
|
||||
// Get the rotational inertia of the body about the center of mass. Typically in kg/m^3.
|
||||
// Get the inverse mass of the body. Typically in kg/m^3.
|
||||
float32 GetInverseMass() const;
|
||||
|
||||
// Get the rotational inertia of the body about the local center of mass. Typically in kg/m^3.
|
||||
const b3Mat33& GetInertia() const;
|
||||
|
||||
// Get the inverse of the rotational inertia of the body about the world center of mass. Typically in kg/m^3.
|
||||
const b3Mat33& GetWorldInverseInertia() const;
|
||||
|
||||
// Get this body mass data.
|
||||
// However, the mass data returned by this function contains the mass of the body,
|
||||
// the body local center of mass, and the rotational inertia about the body local center of mass.
|
||||
@ -609,6 +615,16 @@ inline float32 b3Body::GetMass() const
|
||||
return m_mass;
|
||||
}
|
||||
|
||||
inline float32 b3Body::GetInverseMass() const
|
||||
{
|
||||
return m_invMass;
|
||||
}
|
||||
|
||||
inline const b3Mat33& b3Body::GetWorldInverseInertia() const
|
||||
{
|
||||
return m_worldInvI;
|
||||
}
|
||||
|
||||
inline const b3Mat33& b3Body::GetInertia() const
|
||||
{
|
||||
return m_I;
|
||||
|
||||
@ -19,12 +19,13 @@
|
||||
#ifndef B3_CLOTH_SOLVER_H
|
||||
#define B3_CLOTH_SOLVER_H
|
||||
|
||||
#include <bounce/common/math/vec3.h>
|
||||
#include <bounce/common/math/mat22.h>
|
||||
#include <bounce/common/math/mat33.h>
|
||||
|
||||
class b3StackAllocator;
|
||||
|
||||
class b3Particle;
|
||||
class b3Body;
|
||||
class b3Force;
|
||||
class b3BodyContact;
|
||||
|
||||
@ -63,6 +64,36 @@ struct b3AccelerationConstraint
|
||||
void Apply(const b3ClothSolverData* data);
|
||||
};
|
||||
|
||||
struct b3ClothContactVelocityConstraint
|
||||
{
|
||||
u32 indexA;
|
||||
float32 invMassA;
|
||||
b3Mat33 invIA;
|
||||
|
||||
b3Body* bodyB;
|
||||
float32 invMassB;
|
||||
b3Mat33 invIB;
|
||||
|
||||
float32 friction;
|
||||
|
||||
b3Vec3 point;
|
||||
b3Vec3 rA;
|
||||
b3Vec3 rB;
|
||||
|
||||
b3Vec3 normal;
|
||||
float32 normalMass;
|
||||
float32 normalImpulse;
|
||||
float32 velocityBias;
|
||||
|
||||
b3Vec3 tangent1;
|
||||
b3Vec3 tangent2;
|
||||
b3Mat22 tangentMass;
|
||||
b3Vec2 tangentImpulse;
|
||||
|
||||
float32 motorMass;
|
||||
float32 motorImpulse;
|
||||
};
|
||||
|
||||
class b3ClothSolver
|
||||
{
|
||||
public:
|
||||
@ -84,6 +115,18 @@ private:
|
||||
// Solve Ax = b.
|
||||
void Solve(b3DenseVec3& x, u32& iterations, const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const;
|
||||
|
||||
//
|
||||
void InitializeVelocityConstraints();
|
||||
|
||||
//
|
||||
void WarmStart();
|
||||
|
||||
//
|
||||
void SolveVelocityConstraints();
|
||||
|
||||
//
|
||||
void StoreImpulses();
|
||||
|
||||
b3StackAllocator* m_allocator;
|
||||
|
||||
u32 m_particleCapacity;
|
||||
@ -102,6 +145,8 @@ private:
|
||||
u32 m_constraintCount;
|
||||
b3AccelerationConstraint* m_constraints;
|
||||
|
||||
b3ClothContactVelocityConstraint* m_velocityConstraints;
|
||||
|
||||
b3ClothSolverData m_solverData;
|
||||
};
|
||||
|
||||
|
||||
@ -22,6 +22,7 @@
|
||||
#include <bounce/common/math/transform.h>
|
||||
#include <bounce/common/template/list.h>
|
||||
#include <bounce/dynamics/cloth/force.h>
|
||||
#include <bounce/common/math/vec2.h>
|
||||
|
||||
class b3Shape;
|
||||
class b3Cloth;
|
||||
@ -81,19 +82,19 @@ public:
|
||||
b3Shape* s2;
|
||||
|
||||
// Contact constraint
|
||||
bool n_active;
|
||||
b3Vec3 p;
|
||||
b3Vec3 n;
|
||||
float32 Fn;
|
||||
float32 normalImpulse;
|
||||
|
||||
// Friction constraint
|
||||
bool t1_active, t2_active;
|
||||
b3Vec3 t1, t2;
|
||||
float32 Ft1, Ft2;
|
||||
b3Vec2 tangentImpulse;
|
||||
|
||||
// Motor constraint
|
||||
float32 motorImpulse;
|
||||
|
||||
// Friction force
|
||||
bool f_active;
|
||||
b3FrictionForce f;
|
||||
//
|
||||
bool active;
|
||||
};
|
||||
|
||||
// A cloth particle.
|
||||
|
||||
@ -157,7 +157,7 @@ b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) : m_particleBlocks(sizeo
|
||||
m_world = world;
|
||||
m_mesh = def.mesh;
|
||||
m_density = def.density;
|
||||
|
||||
|
||||
const b3ClothMesh* m = m_mesh;
|
||||
|
||||
m_vertexParticles = (b3Particle**)b3Alloc(m->vertexCount * sizeof(b3Particle*));
|
||||
@ -172,7 +172,7 @@ b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) : m_particleBlocks(sizeo
|
||||
b3Particle* p = CreateParticle(pd);
|
||||
|
||||
p->m_vertex = i;
|
||||
|
||||
|
||||
m_vertexParticles[i] = p;
|
||||
}
|
||||
|
||||
@ -213,7 +213,7 @@ b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) : m_particleBlocks(sizeo
|
||||
b3Particle* p2 = m_vertexParticles[e->v2];
|
||||
b3Particle* p3 = m_vertexParticles[e->nsv1];
|
||||
b3Particle* p4 = m_vertexParticles[e->nsv2];
|
||||
|
||||
|
||||
b3BendForceDef fd;
|
||||
fd.Initialize(p1, p2, p3, p4, def.bending, def.damping);
|
||||
|
||||
@ -350,7 +350,7 @@ void b3Cloth::ComputeMass()
|
||||
}
|
||||
}
|
||||
|
||||
void b3Cloth::RayCast(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2)
|
||||
void b3Cloth::RayCast(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2)
|
||||
{
|
||||
b3RayCastInput input;
|
||||
input.p1 = p1;
|
||||
@ -407,7 +407,7 @@ bool b3Cloth::RayCastSingle(b3ClothRayCastSingleOutput* output, const b3Vec3& p1
|
||||
output->triangle = triangle0;
|
||||
output->fraction = output0.fraction;
|
||||
output->normal = output0.normal;
|
||||
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -511,22 +511,11 @@ void b3Cloth::UpdateContacts()
|
||||
// Create contacts
|
||||
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
|
||||
{
|
||||
if (p->m_type == e_staticParticle)
|
||||
{
|
||||
// TODO
|
||||
continue;
|
||||
}
|
||||
|
||||
b3BodyContact* c = &p->m_contact;
|
||||
|
||||
// Save the old contact
|
||||
b3BodyContact c0 = *c;
|
||||
|
||||
// Create a new contact
|
||||
c->f_active = false;
|
||||
c->n_active = false;
|
||||
c->t1_active = false;
|
||||
c->t2_active = false;
|
||||
c->active = false;
|
||||
|
||||
b3Sphere s1;
|
||||
s1.vertex = p->m_position;
|
||||
@ -561,122 +550,34 @@ void b3Cloth::UpdateContacts()
|
||||
}
|
||||
}
|
||||
|
||||
if (bestShape != nullptr)
|
||||
{
|
||||
b3Shape* shape = bestShape;
|
||||
float32 s = bestSeparation;
|
||||
b3Vec3 n = bestNormal;
|
||||
b3Vec3 cp = p->m_position - s * n;
|
||||
|
||||
// Store the contact manifold
|
||||
// Here the normal points from shape 2 to the particle
|
||||
c->p1 = p;
|
||||
c->s2 = shape;
|
||||
c->n_active = true;
|
||||
c->p = cp;
|
||||
c->n = n;
|
||||
c->t1 = b3Perp(n);
|
||||
c->t2 = b3Cross(c->t1, n);
|
||||
}
|
||||
|
||||
// Update the contact state
|
||||
if (c0.n_active == true && c->n_active == true)
|
||||
{
|
||||
// The contact persists
|
||||
|
||||
// Has the contact constraint been satisfied?
|
||||
if (c0.Fn <= 0.0f)
|
||||
{
|
||||
// Contact force is attractive.
|
||||
|
||||
// Terminate the contact.
|
||||
c->n_active = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (c->n_active == false)
|
||||
if (bestShape == nullptr)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// A friction force requires an associated normal force.
|
||||
if (c0.n_active == false)
|
||||
b3Shape* shape = bestShape;
|
||||
float32 s = bestSeparation;
|
||||
b3Vec3 n = bestNormal;
|
||||
b3Vec3 cp = p->m_position - s * n;
|
||||
|
||||
// Store the contact manifold
|
||||
// Here the normal points from shape 2 to the particle
|
||||
c->active = true;
|
||||
c->p1 = p;
|
||||
c->s2 = shape;
|
||||
c->p = cp;
|
||||
c->n = n;
|
||||
c->t1 = b3Perp(n);
|
||||
c->t2 = b3Cross(c->t1, n);
|
||||
c->normalImpulse = 0.0f;
|
||||
c->tangentImpulse.SetZero();
|
||||
c->motorImpulse = 0.0f;
|
||||
|
||||
if (c0.active == true)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
b3Shape* s = c->s2;
|
||||
b3Body* b = s->GetBody();
|
||||
b3Vec3 n = c->n;
|
||||
float32 u = s->GetFriction();
|
||||
float32 normalForce = c0.Fn;
|
||||
float32 maxFrictionForce = u * normalForce;
|
||||
|
||||
// Relative velocity at contact point
|
||||
b3Vec3 v1 = b->GetPointVelocity(c->p);
|
||||
b3Vec3 v2 = p->m_velocity;
|
||||
b3Vec3 dv = v2 - v1;
|
||||
|
||||
b3Vec3 t1 = dv - b3Dot(dv, n) * n;
|
||||
if (b3Dot(t1, t1) > B3_EPSILON * B3_EPSILON)
|
||||
{
|
||||
// Create a dynamic basis
|
||||
t1.Normalize();
|
||||
|
||||
b3Vec3 t2 = b3Cross(t1, n);
|
||||
t2.Normalize();
|
||||
|
||||
c->t1 = t1;
|
||||
c->t2 = t2;
|
||||
}
|
||||
|
||||
b3Vec3 ts[2];
|
||||
ts[0] = c->t1;
|
||||
ts[1] = c->t2;
|
||||
|
||||
bool t_active[2];
|
||||
t_active[0] = c->t1_active;
|
||||
t_active[1] = c->t2_active;
|
||||
|
||||
bool t_active0[2];
|
||||
t_active0[0] = c0.t1_active;
|
||||
t_active0[1] = c0.t2_active;
|
||||
|
||||
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);
|
||||
|
||||
if (dvt * dvt <= B3_EPSILON * B3_EPSILON)
|
||||
{
|
||||
// Lock particle on surface
|
||||
t_active[k] = true;
|
||||
}
|
||||
}
|
||||
|
||||
c->t1_active = t_active[0];
|
||||
c->t2_active = t_active[1];
|
||||
|
||||
if (c0.t1_active == true)
|
||||
{
|
||||
if (c0.Ft1 > maxFrictionForce)
|
||||
{
|
||||
c->t1_active = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (c0.t2_active == true)
|
||||
{
|
||||
if (c0.Ft2 > maxFrictionForce)
|
||||
{
|
||||
c->t2_active = false;
|
||||
}
|
||||
c->normalImpulse = c->normalImpulse;
|
||||
c->tangentImpulse = c->tangentImpulse;
|
||||
c->motorImpulse = c->motorImpulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -706,12 +607,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
|
||||
|
||||
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
|
||||
{
|
||||
if (p->m_contact.f_active)
|
||||
{
|
||||
solver.Add(&p->m_contact.f);
|
||||
}
|
||||
|
||||
if (p->m_contact.n_active)
|
||||
if (p->m_contact.active)
|
||||
{
|
||||
solver.Add(&p->m_contact);
|
||||
}
|
||||
@ -762,8 +658,8 @@ void b3Cloth::Draw() const
|
||||
}
|
||||
|
||||
b3BodyContact* c = &p->m_contact;
|
||||
|
||||
if (c->n_active)
|
||||
|
||||
if (c->active)
|
||||
{
|
||||
b3Draw_draw->DrawSegment(c->p, c->p + c->n, b3Color_yellow);
|
||||
}
|
||||
|
||||
@ -55,10 +55,13 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
|
||||
m_constraintCapacity = def.particleCapacity;
|
||||
m_constraintCount = 0;
|
||||
m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_constraintCapacity * sizeof(b3AccelerationConstraint));
|
||||
|
||||
m_velocityConstraints = (b3ClothContactVelocityConstraint*)m_allocator->Allocate(m_contactCount * sizeof(b3ClothContactVelocityConstraint));
|
||||
}
|
||||
|
||||
b3ClothSolver::~b3ClothSolver()
|
||||
{
|
||||
m_allocator->Free(m_velocityConstraints);
|
||||
m_allocator->Free(m_constraints);
|
||||
m_allocator->Free(m_contacts);
|
||||
m_allocator->Free(m_forces);
|
||||
@ -132,7 +135,6 @@ void b3ClothSolver::ApplyConstraints()
|
||||
{
|
||||
b3DiagMat33& S = *m_solverData.S;
|
||||
b3DenseVec3& z = *m_solverData.z;
|
||||
float32 h = m_solverData.dt;
|
||||
|
||||
S.SetIdentity();
|
||||
z.SetZero();
|
||||
@ -150,43 +152,6 @@ void b3ClothSolver::ApplyConstraints()
|
||||
}
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_contactCount; ++i)
|
||||
{
|
||||
b3BodyContact* pc = m_contacts[i];
|
||||
b3Particle* p = pc->p1;
|
||||
|
||||
if (p->m_type != e_dynamicParticle)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
b3AccelerationConstraint* ac = m_constraints + m_constraintCount;
|
||||
++m_constraintCount;
|
||||
ac->i1 = p->m_solverId;
|
||||
ac->ndof = 2;
|
||||
ac->p = pc->n;
|
||||
ac->z.SetZero();
|
||||
|
||||
if (pc->t1_active && pc->t2_active)
|
||||
{
|
||||
ac->ndof = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (pc->t1_active)
|
||||
{
|
||||
ac->ndof = 1;
|
||||
ac->q = pc->t1;
|
||||
}
|
||||
|
||||
if (pc->t2_active)
|
||||
{
|
||||
ac->ndof = 1;
|
||||
ac->q = pc->t2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_constraintCount; ++i)
|
||||
{
|
||||
m_constraints[i].Apply(&m_solverData);
|
||||
@ -242,11 +207,15 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
|
||||
b3BodyContact* c = m_contacts[i];
|
||||
b3Particle* p = c->p1;
|
||||
|
||||
b3Vec3 dx = c->p - p->m_position;
|
||||
|
||||
sy[p->m_solverId] += dx;
|
||||
if (p->m_type == e_dynamicParticle)
|
||||
{
|
||||
b3Vec3 dx = c->p - p->m_position;
|
||||
sy[p->m_solverId] += dx;
|
||||
}
|
||||
}
|
||||
|
||||
// Integrate velocities
|
||||
|
||||
// Apply internal forces
|
||||
ApplyForces();
|
||||
|
||||
@ -275,8 +244,22 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
|
||||
Solve(x, iterations, A, b, S, z, sx0);
|
||||
b3_clothSolverIterations = iterations;
|
||||
|
||||
// Compute the new state
|
||||
sv = sv + x;
|
||||
|
||||
// Initialize velocity constraints
|
||||
InitializeVelocityConstraints();
|
||||
|
||||
// Warm start velocity constraints
|
||||
WarmStart();
|
||||
|
||||
// Solve velocity constraints
|
||||
const u32 kVelocityIterations = 10;
|
||||
for (u32 i = 0; i < kVelocityIterations; ++i)
|
||||
{
|
||||
SolveVelocityConstraints();
|
||||
}
|
||||
|
||||
// Integrate positions
|
||||
sx = sx + h * sv + sy;
|
||||
|
||||
// Copy state buffers back to the particles
|
||||
@ -290,40 +273,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
|
||||
// Cache x to improve convergence
|
||||
p->m_x = x[i];
|
||||
}
|
||||
|
||||
// f = A * x - b
|
||||
b3DenseVec3 f = A * x - b;
|
||||
|
||||
for (u32 i = 0; i < m_contactCount; ++i)
|
||||
{
|
||||
b3BodyContact* c = m_contacts[i];
|
||||
|
||||
b3Particle* p1 = c->p1;
|
||||
b3Body* b2 = c->s2->GetBody();
|
||||
|
||||
b3Vec3 f1 = f[p1->m_solverId];
|
||||
b3Vec3 f2 = -f1;
|
||||
|
||||
// Apply constraint reaction force at the contact point on the body
|
||||
b2->ApplyForce(f2, c->p, true);
|
||||
|
||||
// Store constraint force acted on the particle
|
||||
|
||||
// Signed normal force magnitude
|
||||
c->Fn = b3Dot(f1, c->n);
|
||||
|
||||
if (c->t1_active)
|
||||
{
|
||||
// Signed tangent force magnitude
|
||||
c->Ft1 = b3Dot(f1, c->t1);
|
||||
}
|
||||
|
||||
if (c->t2_active)
|
||||
{
|
||||
// Signed tangent force magnitude
|
||||
c->Ft2 = b3Dot(f1, c->t2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
|
||||
@ -432,4 +381,273 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
|
||||
}
|
||||
|
||||
iterations = iteration;
|
||||
}
|
||||
|
||||
void b3ClothSolver::InitializeVelocityConstraints()
|
||||
{
|
||||
b3DenseVec3& x = *m_solverData.x;
|
||||
b3DenseVec3& v = *m_solverData.v;
|
||||
|
||||
for (u32 i = 0; i < m_contactCount; ++i)
|
||||
{
|
||||
b3BodyContact* c = m_contacts[i];
|
||||
b3ClothContactVelocityConstraint* vc = m_velocityConstraints + i;
|
||||
|
||||
vc->indexA = c->p1->m_solverId;
|
||||
vc->bodyB = c->s2->GetBody();
|
||||
|
||||
vc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass;
|
||||
vc->invMassB = vc->bodyB->GetInverseMass();
|
||||
|
||||
vc->invIA.SetZero();
|
||||
vc->invIB = vc->bodyB->GetWorldInverseInertia();
|
||||
|
||||
vc->friction = c->s2->GetFriction();
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_contactCount; ++i)
|
||||
{
|
||||
b3BodyContact* c = m_contacts[i];
|
||||
b3ClothContactVelocityConstraint* vc = m_velocityConstraints + i;
|
||||
|
||||
u32 indexA = vc->indexA;
|
||||
b3Body* bodyB = vc->bodyB;
|
||||
|
||||
float32 mA = vc->invMassA;
|
||||
float32 mB = vc->invMassB;
|
||||
|
||||
b3Mat33 iA = vc->invIA;
|
||||
b3Mat33 iB = vc->invIB;
|
||||
|
||||
b3Vec3 xA = x[indexA];
|
||||
b3Vec3 xB = bodyB->GetPosition();
|
||||
|
||||
// Ensure the normal points from shape 1 to the shape 2
|
||||
vc->normal = -c->n;
|
||||
vc->tangent1 = c->t1;
|
||||
vc->tangent2 = c->t2;
|
||||
vc->point = c->p;
|
||||
|
||||
b3Vec3 point = c->p;
|
||||
|
||||
b3Vec3 rA = point - xA;
|
||||
b3Vec3 rB = point - xB;
|
||||
|
||||
vc->rA = rA;
|
||||
vc->rB = rB;
|
||||
|
||||
vc->normalImpulse = c->normalImpulse;
|
||||
vc->tangentImpulse = c->tangentImpulse;
|
||||
vc->motorImpulse = c->motorImpulse;
|
||||
|
||||
{
|
||||
b3Vec3 n = vc->normal;
|
||||
|
||||
b3Vec3 rnA = b3Cross(rA, n);
|
||||
b3Vec3 rnB = b3Cross(rB, n);
|
||||
|
||||
float32 K = mA + mB + b3Dot(iA * rnA, rnA) + b3Dot(iB * rnB, rnB);
|
||||
|
||||
vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f;
|
||||
}
|
||||
|
||||
{
|
||||
b3Vec3 t1 = vc->tangent1;
|
||||
b3Vec3 t2 = vc->tangent2;
|
||||
|
||||
b3Vec3 rn1A = b3Cross(rA, t1);
|
||||
b3Vec3 rn1B = b3Cross(rB, t1);
|
||||
b3Vec3 rn2A = b3Cross(rA, t2);
|
||||
b3Vec3 rn2B = b3Cross(rB, t2);
|
||||
|
||||
// dot(t1, t2) = 0
|
||||
// J1_l1 * M1 * J2_l1 = J1_l2 * M2 * J2_l2 = 0
|
||||
float32 k11 = mA + mB + b3Dot(iA * rn1A, rn1A) + b3Dot(iB * rn1B, rn1B);
|
||||
float32 k12 = b3Dot(iA * rn1A, rn2A) + b3Dot(iB * rn1B, rn2B);
|
||||
float32 k22 = mA + mB + b3Dot(iA * rn2A, rn2A) + b3Dot(iB * rn2B, rn2B);
|
||||
|
||||
b3Mat22 K;
|
||||
K.x.Set(k11, k12);
|
||||
K.y.Set(k12, k22);
|
||||
|
||||
vc->tangentMass = b3Inverse(K);
|
||||
}
|
||||
|
||||
{
|
||||
float32 mass = b3Dot(vc->normal, (iA + iB) * vc->normal);
|
||||
vc->motorMass = mass > 0.0f ? 1.0f / mass : 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothSolver::WarmStart()
|
||||
{
|
||||
b3DenseVec3& v = *m_solverData.v;
|
||||
|
||||
for (u32 i = 0; i < m_contactCount; ++i)
|
||||
{
|
||||
b3ClothContactVelocityConstraint* vc = m_velocityConstraints + i;
|
||||
|
||||
u32 indexA = vc->indexA;
|
||||
b3Body* bodyB = vc->bodyB;
|
||||
|
||||
b3Vec3 vA = v[indexA];
|
||||
b3Vec3 vB = bodyB->GetLinearVelocity();
|
||||
|
||||
b3Vec3 wA; wA.SetZero();
|
||||
b3Vec3 wB = bodyB->GetAngularVelocity();
|
||||
|
||||
float32 mA = vc->invMassA;
|
||||
float32 mB = vc->invMassB;
|
||||
|
||||
b3Mat33 iA = vc->invIA;
|
||||
b3Mat33 iB = vc->invIB;
|
||||
|
||||
b3Vec3 P = vc->normalImpulse * vc->normal;
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * b3Cross(vc->rA, P);
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * b3Cross(vc->rB, P);
|
||||
|
||||
b3Vec3 P1 = vc->tangentImpulse.x * vc->tangent1;
|
||||
b3Vec3 P2 = vc->tangentImpulse.y * vc->tangent2;
|
||||
b3Vec3 P3 = vc->motorImpulse * vc->normal;
|
||||
|
||||
vA -= mA * (P1 + P2);
|
||||
wA -= iA * (b3Cross(vc->rA, P1 + P2) + P3);
|
||||
|
||||
vB += mB * (P1 + P2);
|
||||
wB += iB * (b3Cross(vc->rB, P1 + P2) + P3);
|
||||
|
||||
v[indexA] = vA;
|
||||
|
||||
bodyB->SetLinearVelocity(vB);
|
||||
bodyB->SetAngularVelocity(wB);
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothSolver::SolveVelocityConstraints()
|
||||
{
|
||||
b3DenseVec3& v = *m_solverData.v;
|
||||
|
||||
for (u32 i = 0; i < m_contactCount; ++i)
|
||||
{
|
||||
b3ClothContactVelocityConstraint* vc = m_velocityConstraints + i;
|
||||
|
||||
u32 indexA = vc->indexA;
|
||||
b3Body* bodyB = vc->bodyB;
|
||||
|
||||
b3Vec3 vA = v[indexA];
|
||||
b3Vec3 vB = bodyB->GetLinearVelocity();
|
||||
|
||||
b3Vec3 wA; wA.SetZero();
|
||||
b3Vec3 wB = bodyB->GetAngularVelocity();
|
||||
|
||||
float32 mA = vc->invMassA;
|
||||
float32 mB = vc->invMassB;
|
||||
|
||||
b3Mat33 iA = vc->invIA;
|
||||
b3Mat33 iB = vc->invIB;
|
||||
|
||||
// Ensure the normal points from shape 1 to the shape 2
|
||||
b3Vec3 normal = vc->normal;
|
||||
b3Vec3 point = vc->point;
|
||||
|
||||
b3Vec3 rA = vc->rA;
|
||||
b3Vec3 rB = vc->rB;
|
||||
|
||||
// Solve normal constraint.
|
||||
{
|
||||
b3Vec3 rnA = b3Cross(rA, normal);
|
||||
b3Vec3 rnB = b3Cross(rB, normal);
|
||||
|
||||
float32 K = mA + mB + b3Dot(iA * rnA, rnA) + b3Dot(iB * rnB, rnB);
|
||||
|
||||
float32 invK = K > 0.0f ? 1.0f / K : 0.0f;
|
||||
|
||||
b3Vec3 dv = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA);
|
||||
float32 Cdot = b3Dot(normal, dv);
|
||||
|
||||
float32 impulse = -invK * Cdot;
|
||||
|
||||
float32 oldImpulse = vc->normalImpulse;
|
||||
vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f);
|
||||
impulse = vc->normalImpulse - oldImpulse;
|
||||
|
||||
b3Vec3 P = impulse * normal;
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * b3Cross(rA, P);
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * b3Cross(rB, P);
|
||||
}
|
||||
|
||||
// Solve tangent constraints.
|
||||
{
|
||||
b3Vec3 dv = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA);
|
||||
|
||||
b3Vec2 Cdot;
|
||||
Cdot.x = b3Dot(dv, vc->tangent1);
|
||||
Cdot.y = b3Dot(dv, vc->tangent2);
|
||||
|
||||
b3Vec2 impulse = vc->tangentMass * -Cdot;
|
||||
b3Vec2 oldImpulse = vc->tangentImpulse;
|
||||
vc->tangentImpulse += impulse;
|
||||
|
||||
float32 maxImpulse = vc->friction * vc->normalImpulse;
|
||||
if (b3Dot(vc->tangentImpulse, vc->tangentImpulse) > maxImpulse * maxImpulse)
|
||||
{
|
||||
vc->tangentImpulse.Normalize();
|
||||
vc->tangentImpulse *= maxImpulse;
|
||||
}
|
||||
|
||||
impulse = vc->tangentImpulse - oldImpulse;
|
||||
|
||||
b3Vec3 P1 = impulse.x * vc->tangent1;
|
||||
b3Vec3 P2 = impulse.y * vc->tangent2;
|
||||
b3Vec3 P = P1 + P2;
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * b3Cross(rA, P);
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * b3Cross(rB, P);
|
||||
}
|
||||
|
||||
// Solve motor constraint.
|
||||
{
|
||||
float32 Cdot = b3Dot(vc->normal, wB - wA);
|
||||
float32 impulse = -vc->motorMass * Cdot;
|
||||
float32 oldImpulse = vc->motorImpulse;
|
||||
float32 maxImpulse = vc->friction * vc->normalImpulse;
|
||||
vc->motorImpulse = b3Clamp(vc->motorImpulse + impulse, -maxImpulse, maxImpulse);
|
||||
impulse = vc->motorImpulse - oldImpulse;
|
||||
|
||||
b3Vec3 P = impulse * vc->normal;
|
||||
|
||||
wA -= iA * P;
|
||||
wB += iB * P;
|
||||
}
|
||||
|
||||
v[indexA] = vA;
|
||||
|
||||
bodyB->SetLinearVelocity(vB);
|
||||
bodyB->SetAngularVelocity(wB);
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothSolver::StoreImpulses()
|
||||
{
|
||||
for (u32 i = 0; i < m_contactCount; ++i)
|
||||
{
|
||||
b3BodyContact* c = m_contacts[i];
|
||||
b3ClothContactVelocityConstraint* vc = m_velocityConstraints + i;
|
||||
|
||||
c->normalImpulse = vc->normalImpulse;
|
||||
c->tangentImpulse = vc->tangentImpulse;
|
||||
c->motorImpulse = vc->motorImpulse;
|
||||
}
|
||||
}
|
||||
@ -43,10 +43,7 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)
|
||||
m_x.SetZero();
|
||||
m_vertex = ~0;
|
||||
|
||||
m_contact.n_active = false;
|
||||
m_contact.t1_active = false;
|
||||
m_contact.t2_active = false;
|
||||
m_contact.f_active = false;
|
||||
m_contact.active = false;
|
||||
}
|
||||
|
||||
b3Particle::~b3Particle()
|
||||
@ -69,9 +66,6 @@ void b3Particle::SetType(b3ParticleType type)
|
||||
m_velocity.SetZero();
|
||||
m_translation.SetZero();
|
||||
|
||||
m_contact.f_active = false;
|
||||
m_contact.n_active = false;
|
||||
m_contact.t1_active = false;
|
||||
m_contact.t2_active = false;
|
||||
m_contact.active = false;
|
||||
}
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user