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

solve forces then solve an lcp, decouple contact constraints

Browse Source
This commit is contained in:
Irlan
2018-07-24 21:45:57 -03:00
parent 927b35a45d
commit 65e5ff217e
6 changed files with 399 additions and 229 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
include/bounce/dynamics/body.h
View File

@ -197,9 +197,15 @@ public:
// Get the mass of the body. Typically in kg/m^3. // Get the mass of the body. Typically in kg/m^3.
float32 GetMass() const; 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; 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. // Get this body mass data.
// However, the mass data returned by this function contains the mass of the body, // 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. // 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; 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 inline const b3Mat33& b3Body::GetInertia() const
{ {
return m_I; return m_I;

47
include/bounce/dynamics/cloth/cloth_solver.h
View File

@ -19,12 +19,13 @@
#ifndef B3_CLOTH_SOLVER_H #ifndef B3_CLOTH_SOLVER_H
#define 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> #include <bounce/common/math/mat33.h>
class b3StackAllocator; class b3StackAllocator;
class b3Particle; class b3Particle;
class b3Body;
class b3Force; class b3Force;
class b3BodyContact; class b3BodyContact;
@ -63,6 +64,36 @@ struct b3AccelerationConstraint
void Apply(const b3ClothSolverData* data); 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 class b3ClothSolver
{ {
public: public:
@ -84,6 +115,18 @@ private:
// Solve Ax = b. // 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 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; b3StackAllocator* m_allocator;
u32 m_particleCapacity; u32 m_particleCapacity;
@ -102,6 +145,8 @@ private:
u32 m_constraintCount; u32 m_constraintCount;
b3AccelerationConstraint* m_constraints; b3AccelerationConstraint* m_constraints;
b3ClothContactVelocityConstraint* m_velocityConstraints;
b3ClothSolverData m_solverData; b3ClothSolverData m_solverData;
}; };

15
include/bounce/dynamics/cloth/particle.h
View File

@ -22,6 +22,7 @@
#include <bounce/common/math/transform.h> #include <bounce/common/math/transform.h>
#include <bounce/common/template/list.h> #include <bounce/common/template/list.h>
#include <bounce/dynamics/cloth/force.h> #include <bounce/dynamics/cloth/force.h>
#include <bounce/common/math/vec2.h>
class b3Shape; class b3Shape;
class b3Cloth; class b3Cloth;
@ -81,19 +82,19 @@ public:
b3Shape* s2; b3Shape* s2;
// Contact constraint // Contact constraint
bool n_active;
b3Vec3 p; b3Vec3 p;
b3Vec3 n; b3Vec3 n;
float32 Fn; float32 normalImpulse;
// Friction constraint // Friction constraint
bool t1_active, t2_active;
b3Vec3 t1, t2; b3Vec3 t1, t2;
float32 Ft1, Ft2; b3Vec2 tangentImpulse;
// Motor constraint
float32 motorImpulse;
// Friction force //
bool f_active; bool active;
b3FrictionForce f;
}; };
// A cloth particle. // A cloth particle.

168
src/bounce/dynamics/cloth/cloth.cpp
View File

@ -157,7 +157,7 @@ b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) : m_particleBlocks(sizeo
m_world = world; m_world = world;
m_mesh = def.mesh; m_mesh = def.mesh;
m_density = def.density; m_density = def.density;
const b3ClothMesh* m = m_mesh; const b3ClothMesh* m = m_mesh;
m_vertexParticles = (b3Particle**)b3Alloc(m->vertexCount * sizeof(b3Particle*)); 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); b3Particle* p = CreateParticle(pd);
p->m_vertex = i; p->m_vertex = i;
m_vertexParticles[i] = p; 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* p2 = m_vertexParticles[e->v2];
b3Particle* p3 = m_vertexParticles[e->nsv1]; b3Particle* p3 = m_vertexParticles[e->nsv1];
b3Particle* p4 = m_vertexParticles[e->nsv2]; b3Particle* p4 = m_vertexParticles[e->nsv2];
b3BendForceDef fd; b3BendForceDef fd;
fd.Initialize(p1, p2, p3, p4, def.bending, def.damping); 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; b3RayCastInput input;
input.p1 = p1; input.p1 = p1;
@ -407,7 +407,7 @@ bool b3Cloth::RayCastSingle(b3ClothRayCastSingleOutput* output, const b3Vec3& p1
output->triangle = triangle0; output->triangle = triangle0;
output->fraction = output0.fraction; output->fraction = output0.fraction;
output->normal = output0.normal; output->normal = output0.normal;
return true; return true;
} }
@ -511,22 +511,11 @@ void b3Cloth::UpdateContacts()
// Create contacts // Create contacts
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) 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; b3BodyContact* c = &p->m_contact;
// Save the old contact
b3BodyContact c0 = *c; b3BodyContact c0 = *c;
// Create a new contact c->active = false;
c->f_active = false;
c->n_active = false;
c->t1_active = false;
c->t2_active = false;
b3Sphere s1; b3Sphere s1;
s1.vertex = p->m_position; s1.vertex = p->m_position;
@ -561,122 +550,34 @@ void b3Cloth::UpdateContacts()
} }
} }
if (bestShape != nullptr) 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)
{ {
continue; continue;
} }
// A friction force requires an associated normal force. b3Shape* shape = bestShape;
if (c0.n_active == false) 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; c->normalImpulse = c->normalImpulse;
} c->tangentImpulse = c->tangentImpulse;
c->motorImpulse = c->motorImpulse;
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;
}
} }
} }
} }
@ -706,12 +607,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
{ {
if (p->m_contact.f_active) if (p->m_contact.active)
{
solver.Add(&p->m_contact.f);
}
if (p->m_contact.n_active)
{ {
solver.Add(&p->m_contact); solver.Add(&p->m_contact);
} }
@ -762,8 +658,8 @@ void b3Cloth::Draw() const
} }
b3BodyContact* c = &p->m_contact; b3BodyContact* c = &p->m_contact;
if (c->n_active) if (c->active)
{ {
b3Draw_draw->DrawSegment(c->p, c->p + c->n, b3Color_yellow); b3Draw_draw->DrawSegment(c->p, c->p + c->n, b3Color_yellow);
} }

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

@ -55,10 +55,13 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
m_constraintCapacity = def.particleCapacity; m_constraintCapacity = def.particleCapacity;
m_constraintCount = 0; m_constraintCount = 0;
m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_constraintCapacity * sizeof(b3AccelerationConstraint)); m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_constraintCapacity * sizeof(b3AccelerationConstraint));
m_velocityConstraints = (b3ClothContactVelocityConstraint*)m_allocator->Allocate(m_contactCount * sizeof(b3ClothContactVelocityConstraint));
} }
b3ClothSolver::~b3ClothSolver() b3ClothSolver::~b3ClothSolver()
{ {
m_allocator->Free(m_velocityConstraints);
m_allocator->Free(m_constraints); m_allocator->Free(m_constraints);
m_allocator->Free(m_contacts); m_allocator->Free(m_contacts);
m_allocator->Free(m_forces); m_allocator->Free(m_forces);
@ -132,7 +135,6 @@ void b3ClothSolver::ApplyConstraints()
{ {
b3DiagMat33& S = *m_solverData.S; b3DiagMat33& S = *m_solverData.S;
b3DenseVec3& z = *m_solverData.z; b3DenseVec3& z = *m_solverData.z;
float32 h = m_solverData.dt;
S.SetIdentity(); S.SetIdentity();
z.SetZero(); 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) for (u32 i = 0; i < m_constraintCount; ++i)
{ {
m_constraints[i].Apply(&m_solverData); m_constraints[i].Apply(&m_solverData);
@ -242,11 +207,15 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
b3BodyContact* c = m_contacts[i]; b3BodyContact* c = m_contacts[i];
b3Particle* p = c->p1; b3Particle* p = c->p1;
b3Vec3 dx = c->p - p->m_position; if (p->m_type == e_dynamicParticle)
{
sy[p->m_solverId] += dx; b3Vec3 dx = c->p - p->m_position;
sy[p->m_solverId] += dx;
}
} }
// Integrate velocities
// Apply internal forces // Apply internal forces
ApplyForces(); ApplyForces();
@ -275,8 +244,22 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
Solve(x, iterations, A, b, S, z, sx0); Solve(x, iterations, A, b, S, z, sx0);
b3_clothSolverIterations = iterations; b3_clothSolverIterations = iterations;
// Compute the new state
sv = sv + x; 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; sx = sx + h * sv + sy;
// Copy state buffers back to the particles // Copy state buffers back to the particles
@ -290,40 +273,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
// Cache x to improve convergence // Cache x to improve convergence
p->m_x = x[i]; 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, void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
@ -432,4 +381,273 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
} }
iterations = iteration; 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;
}
} }

10
src/bounce/dynamics/cloth/particle.cpp
View File

@ -43,10 +43,7 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)
m_x.SetZero(); m_x.SetZero();
m_vertex = ~0; m_vertex = ~0;
m_contact.n_active = false; m_contact.active = false;
m_contact.t1_active = false;
m_contact.t2_active = false;
m_contact.f_active = false;
} }
b3Particle::~b3Particle() b3Particle::~b3Particle()
@ -69,9 +66,6 @@ void b3Particle::SetType(b3ParticleType type)
m_velocity.SetZero(); m_velocity.SetZero();
m_translation.SetZero(); m_translation.SetZero();
m_contact.f_active = false; m_contact.active = false;
m_contact.n_active = false;
m_contact.t1_active = false;
m_contact.t2_active = false;
} }
} }