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.
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;

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

@@ -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;
};

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

@@ -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;
// Friction force
bool f_active;
b3FrictionForce f;
// Motor constraint
float32 motorImpulse;
//
bool active;
};
// A cloth particle.

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

@@ -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);
}
@@ -763,7 +659,7 @@ 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);
}

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

@@ -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,
@@ -433,3 +382,272 @@ 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;
}
}

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

@@ -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;
}
}