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

Applied a bugfix on constraint solver and removed some frozen code

Browse Source
This commit is contained in:
Irlan 2019-04-18 18:48:10 -03:00
parent 923a069408
commit 2f9abcbe92
8 changed files with 10 additions and 490 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
include/bounce/cloth/cloth.h
View File

@ -153,9 +153,6 @@ private:
// Update body contacts.
void UpdateBodyContacts();
// Update particle contacts.
void UpdateParticleContacts();
// Update contacts
void UpdateContacts();
@ -183,17 +180,11 @@ private:
// Pool of particles
b3BlockPool m_particleBlocks;
// Pool of particle contacts
b3BlockPool m_particleContactBlocks;
// List of particles
b3List2<b3Particle> m_particleList;
// List of forces
b3List2<b3Force> m_forceList;
// List of particle contacts
b3List2<b3ParticleContact> m_particleContactList;
};
inline void b3Cloth::SetGravity(const b3Vec3& gravity)

50
include/bounce/cloth/cloth_contact_solver.h
View File

@ -28,7 +28,6 @@ class b3Particle;
class b3Body;
class b3BodyContact;
class b3ParticleContact;
struct b3DenseVec3;
@ -76,44 +75,11 @@ struct b3ClothSolverBodyContactPositionConstraint
b3Vec3 rA;
b3Vec3 rB;
b3Vec3 normalA;
b3Vec3 localPointA;
b3Vec3 localPointB;
};
struct b3ClothSolverParticleContactVelocityConstraint
{
u32 indexA;
float32 invMassA;
u32 indexB;
float32 invMassB;
float32 friction;
b3Vec3 point;
b3Vec3 normal;
float32 normalMass;
float32 normalImpulse;
float32 velocityBias;
b3Vec3 tangent1;
b3Vec3 tangent2;
b3Mat22 tangentMass;
b3Vec2 tangentImpulse;
};
struct b3ClothSolverParticleContactPositionConstraint
{
u32 indexA;
float32 invMassA;
float32 radiusA;
u32 indexB;
float32 invMassB;
float32 radiusB;
};
struct b3ClothContactSolverDef
{
b3StackAllocator* allocator;
@ -123,9 +89,6 @@ struct b3ClothContactSolverDef
u32 bodyContactCount;
b3BodyContact** bodyContacts;
u32 particleContactCount;
b3ParticleContact** particleContacts;
};
inline float32 b3MixFriction(float32 u1, float32 u2)
@ -141,19 +104,13 @@ public:
void InitializeBodyContactConstraints();
void InitializeParticleContactConstraints();
void WarmStart();
void SolveBodyContactVelocityConstraints();
void SolveParticleContactVelocityConstraints();
void StoreImpulses();
bool SolveBodyContactPositionConstraints();
bool SolveParticleContactPositionConstraints();
protected:
b3StackAllocator* m_allocator;
@ -164,11 +121,6 @@ protected:
b3BodyContact** m_bodyContacts;
b3ClothSolverBodyContactVelocityConstraint* m_bodyVelocityConstraints;
b3ClothSolverBodyContactPositionConstraint* m_bodyPositionConstraints;
u32 m_particleContactCount;
b3ParticleContact** m_particleContacts;
b3ClothSolverParticleContactVelocityConstraint* m_particleVelocityConstraints;
b3ClothSolverParticleContactPositionConstraint* m_particlePositionConstraints;
};
#endif

1
include/bounce/cloth/cloth_solver.h
View File

@ -41,7 +41,6 @@ struct b3ClothSolverDef
u32 particleCapacity;
u32 forceCapacity;
u32 bodyContactCapacity;
u32 particleContactCapacity;
};
struct b3ClothSolverData

30
include/bounce/cloth/particle.h
View File

@ -103,36 +103,6 @@ struct b3BodyContactWorldPoint
float32 separation;
};
// A contact between two particles
class b3ParticleContact
{
public:
b3ParticleContact() { }
~b3ParticleContact() { }
b3Particle* p1;
b3Particle* p2;
// Contact constraint
float32 normalImpulse;
// Friction constraint
b3Vec3 t1, t2;
b3Vec2 tangentImpulse;
b3ParticleContact* m_prev;
b3ParticleContact* m_next;
};
struct b3ParticleContactWorldPoint
{
void Initialize(const b3ParticleContact* c);
b3Vec3 point;
b3Vec3 normal;
float32 separation;
};
// A cloth particle.
class b3Particle
{

69
src/bounce/cloth/cloth.cpp
View File

@ -147,8 +147,7 @@ static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m)
}
b3Cloth::b3Cloth(const b3ClothDef& def) :
m_particleBlocks(sizeof(b3Particle)),
m_particleContactBlocks(sizeof(b3ParticleContact))
m_particleBlocks(sizeof(b3Particle))
{
B3_ASSERT(def.mesh);
B3_ASSERT(def.density > 0.0f);
@ -652,66 +651,6 @@ void b3Cloth::UpdateBodyContacts()
}
}
void b3Cloth::UpdateParticleContacts()
{
B3_PROFILE("Cloth Update Particle Contacts");
// Clear buffer
b3ParticleContact* c = m_particleContactList.m_head;
while (c)
{
b3ParticleContact* c0 = c;
c = c->m_next;
m_particleContactList.Remove(c0);
c0->~b3ParticleContact();
m_particleContactBlocks.Free(c0);
}
// Create particle contacts
for (b3Particle* p1 = m_particleList.m_head; p1; p1 = p1->m_next)
{
for (b3Particle* p2 = p1->m_next; p2; p2 = p2->m_next)
{
if (p1->m_type != e_dynamicParticle && p2->m_type != e_dynamicBody)
{
// At least one particle should be kinematic or dynamic.
continue;
}
b3Vec3 c1 = p1->m_position;
float32 r1 = p1->m_radius;
b3Vec3 c2 = p2->m_position;
float32 r2 = p2->m_radius;
b3Vec3 d = c2 - c1;
float32 dd = b3Dot(d, d);
float32 totalRadius = r1 + r2;
if (dd > totalRadius * totalRadius)
{
continue;
}
b3Vec3 n(0.0f, 1.0f, 0.0f);
if (dd > B3_EPSILON * B3_EPSILON)
{
float32 distance = b3Sqrt(dd);
n = d / distance;
}
b3ParticleContact* c = (b3ParticleContact*)m_particleContactBlocks.Allocate();
c->p1 = p1;
c->p2 = p2;
c->normalImpulse = 0.0f;
c->t1 = b3Perp(n);
c->t2 = b3Cross(c->t1, n);
c->tangentImpulse.SetZero();
m_particleContactList.PushFront(c);
}
}
}
void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
{
B3_PROFILE("Cloth Solve");
@ -722,7 +661,6 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
solverDef.particleCapacity = m_particleList.m_count;
solverDef.forceCapacity = m_forceList.m_count;
solverDef.bodyContactCapacity = m_particleList.m_count;
solverDef.particleContactCapacity = m_particleContactList.m_count;
b3ClothSolver solver(solverDef);
@ -744,11 +682,6 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
}
}
for (b3ParticleContact* c = m_particleContactList.m_head; c; c = c->m_next)
{
solver.Add(c);
}
// Solve
solver.Solve(dt, gravity);
}

293
src/bounce/cloth/cloth_contact_solver.cpp
View File

@ -34,18 +34,10 @@ b3ClothContactSolver::b3ClothContactSolver(const b3ClothContactSolverDef& def)
m_bodyContacts = def.bodyContacts;
m_bodyVelocityConstraints = (b3ClothSolverBodyContactVelocityConstraint*)m_allocator->Allocate(m_bodyContactCount * sizeof(b3ClothSolverBodyContactVelocityConstraint));
m_bodyPositionConstraints = (b3ClothSolverBodyContactPositionConstraint*)m_allocator->Allocate(m_bodyContactCount * sizeof(b3ClothSolverBodyContactPositionConstraint));
m_particleContactCount = def.particleContactCount;
m_particleContacts = def.particleContacts;
m_particleVelocityConstraints = (b3ClothSolverParticleContactVelocityConstraint*)m_allocator->Allocate(m_particleContactCount * sizeof(b3ClothSolverParticleContactVelocityConstraint));
m_particlePositionConstraints = (b3ClothSolverParticleContactPositionConstraint*)m_allocator->Allocate(m_particleContactCount * sizeof(b3ClothSolverParticleContactPositionConstraint));
}
b3ClothContactSolver::~b3ClothContactSolver()
{
m_allocator->Free(m_particlePositionConstraints);
m_allocator->Free(m_particleVelocityConstraints);
m_allocator->Free(m_bodyPositionConstraints);
m_allocator->Free(m_bodyVelocityConstraints);
}
@ -87,6 +79,7 @@ void b3ClothContactSolver::InitializeBodyContactConstraints()
pc->localCenterA.SetZero();
pc->localCenterB = pc->bodyB->m_sweep.localCenter;
pc->normalA = c->n;
pc->localPointA = c->localPoint1;
pc->localPointB = c->localPoint2;
}
@ -178,90 +171,6 @@ void b3ClothContactSolver::InitializeBodyContactConstraints()
}
}
void b3ClothContactSolver::InitializeParticleContactConstraints()
{
b3DenseVec3& x = *m_positions;
b3DenseVec3& v = *m_velocities;
for (u32 i = 0; i < m_particleContactCount; ++i)
{
b3ParticleContact* c = m_particleContacts[i];
b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i;
b3ClothSolverParticleContactPositionConstraint* pc = m_particlePositionConstraints + i;
vc->indexA = c->p1->m_solverId;
vc->indexB = c->p2->m_solverId;
vc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass;
vc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass;
vc->friction = b3MixFriction(c->p1->m_friction, c->p2->m_friction);
pc->indexA = c->p1->m_solverId;
pc->indexB = c->p2->m_solverId;
pc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass;
pc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass;
pc->radiusA = c->p1->m_radius;
pc->radiusB = c->p2->m_radius;
}
for (u32 i = 0; i < m_particleContactCount; ++i)
{
b3ParticleContact* c = m_particleContacts[i];
b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i;
b3ClothSolverParticleContactPositionConstraint* pc = m_particlePositionConstraints + i;
u32 indexA = vc->indexA;
u32 indexB = vc->indexB;
float32 mA = vc->invMassA;
float32 mB = vc->invMassB;
b3Vec3 xA = x[indexA];
b3Vec3 xB = x[indexB];
b3ParticleContactWorldPoint wp;
wp.Initialize(c);
vc->normal = wp.normal;
vc->tangent1 = c->t1;
vc->tangent2 = c->t2;
vc->point = wp.point;
b3Vec3 point = vc->point;
vc->normalImpulse = c->normalImpulse;
vc->tangentImpulse = c->tangentImpulse;
{
b3Vec3 n = vc->normal;
float32 K = mA + mB;
vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f;
vc->velocityBias = 0.0f;
}
{
b3Vec3 t1 = vc->tangent1;
b3Vec3 t2 = vc->tangent2;
float32 k11 = mA + mB;
float32 k12 = 0.0f;
float32 k22 = mA + mB;
b3Mat22 K;
K.x.Set(k11, k12);
K.y.Set(k12, k22);
vc->tangentMass = b3Inverse(K);
}
}
}
void b3ClothContactSolver::WarmStart()
{
b3DenseVec3& v = *m_velocities;
@ -307,34 +216,6 @@ void b3ClothContactSolver::WarmStart()
bodyB->SetLinearVelocity(vB);
bodyB->SetAngularVelocity(wB);
}
for (u32 i = 0; i < m_particleContactCount; ++i)
{
b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i;
u32 indexA = vc->indexA;
u32 indexB = vc->indexB;
b3Vec3 vA = v[indexA];
b3Vec3 vB = v[indexB];
float32 mA = vc->invMassA;
float32 mB = vc->invMassB;
b3Vec3 P = vc->normalImpulse * vc->normal;
vA -= mA * P;
vB += mB * P;
b3Vec3 P1 = vc->tangentImpulse.x * vc->tangent1;
b3Vec3 P2 = vc->tangentImpulse.y * vc->tangent2;
vA -= mA * (P1 + P2);
vB += mB * (P1 + P2);
v[indexA] = vA;
v[indexB] = vB;
}
}
void b3ClothContactSolver::SolveBodyContactVelocityConstraints()
@ -425,77 +306,6 @@ void b3ClothContactSolver::SolveBodyContactVelocityConstraints()
}
}
void b3ClothContactSolver::SolveParticleContactVelocityConstraints()
{
b3DenseVec3& v = *m_velocities;
for (u32 i = 0; i < m_particleContactCount; ++i)
{
b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i;
u32 indexA = vc->indexA;
u32 indexB = vc->indexB;
b3Vec3 vA = v[indexA];
b3Vec3 vB = v[indexB];
float32 mA = vc->invMassA;
float32 mB = vc->invMassB;
b3Vec3 normal = vc->normal;
b3Vec3 point = vc->point;
// Solve normal constraint.
{
b3Vec3 dv = vB - vA;
float32 Cdot = b3Dot(normal, dv);
float32 impulse = vc->normalMass * (-Cdot + vc->velocityBias);
float32 oldImpulse = vc->normalImpulse;
vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f);
impulse = vc->normalImpulse - oldImpulse;
b3Vec3 P = impulse * normal;
vA -= mA * P;
vB += mB * P;
}
// Solve tangent constraints.
{
b3Vec3 dv = vB - vA;
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;
vB += mB * P;
}
v[indexA] = vA;
v[indexB] = vB;
}
}
void b3ClothContactSolver::StoreImpulses()
{
for (u32 i = 0; i < m_bodyContactCount; ++i)
@ -506,15 +316,6 @@ void b3ClothContactSolver::StoreImpulses()
c->normalImpulse = vc->normalImpulse;
c->tangentImpulse = vc->tangentImpulse;
}
for (u32 i = 0; i < m_particleContactCount; ++i)
{
b3ParticleContact* c = m_particleContacts[i];
b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i;
c->normalImpulse = vc->normalImpulse;
c->tangentImpulse = vc->tangentImpulse;
}
}
struct b3ClothSolverBodyContactSolverPoint
@ -526,22 +327,15 @@ struct b3ClothSolverBodyContactSolverPoint
float32 rA = pc->radiusA;
float32 rB = pc->radiusB;
b3Vec3 d = cB - cA;
float32 distance = b3Length(d);
b3Vec3 nA(0.0f, 1.0f, 0.0f);
if (distance > B3_EPSILON)
{
nA = d / distance;
}
b3Vec3 nA = pc->normalA;
b3Vec3 pA = cA + rA * nA;
b3Vec3 pB = cB - rB * nA;
point = 0.5f * (pA + pB);
point = cB;
normal = nA;
separation = distance - rA - rB;
separation = b3Dot(cB - cA, nA) - rA - rB;
}
b3Vec3 normal;
@ -623,82 +417,5 @@ bool b3ClothContactSolver::SolveBodyContactPositionConstraints()
bodyB->m_sweep.orientation = qB;
}
return minSeparation >= -3.0f * B3_LINEAR_SLOP;
}
struct b3ClothSolverParticleContactSolverPoint
{
void Initialize(const b3Vec3& cA, float32 rA, const b3Vec3& cB, float32 rB)
{
b3Vec3 d = cB - cA;
float32 distance = b3Length(d);
b3Vec3 nA(0.0f, 1.0f, 0.0f);
if (distance > B3_EPSILON)
{
nA = d / distance;
}
b3Vec3 pA = cA + rA * nA;
b3Vec3 pB = cB - rB * nA;
point = 0.5f * (pA + pB);
normal = nA;
separation = distance - rA - rB;
}
b3Vec3 point;
b3Vec3 normal;
float32 separation;
};
bool b3ClothContactSolver::SolveParticleContactPositionConstraints()
{
b3DenseVec3& x = *m_positions;
float32 minSeparation = 0.0f;
for (u32 i = 0; i < m_particleContactCount; ++i)
{
b3ClothSolverParticleContactPositionConstraint* pc = m_particlePositionConstraints + i;
u32 indexA = pc->indexA;
float32 mA = pc->invMassA;
float32 rA = pc->radiusA;
u32 indexB = pc->indexB;
float32 mB = pc->invMassB;
float32 rB = pc->radiusB;
b3Vec3 xA = x[indexA];
b3Vec3 xB = x[indexB];
b3ClothSolverParticleContactSolverPoint cpcp;
cpcp.Initialize(xA, rA, xB, rB);
b3Vec3 normal = cpcp.normal;
b3Vec3 point = cpcp.point;
float32 separation = cpcp.separation;
// Update max constraint error.
minSeparation = b3Min(minSeparation, separation);
// Allow some slop and prevent large corrections.
float32 C = b3Clamp(B3_BAUMGARTE * (separation + B3_LINEAR_SLOP), -B3_MAX_LINEAR_CORRECTION, 0.0f);
// Compute effective mass.
float32 K = mA + mB;
// Compute normal impulse.
float32 impulse = K > 0.0f ? -C / K : 0.0f;
b3Vec3 P = impulse * normal;
xA -= mA * P;
xB += mB * P;
x[indexA] = xA;
x[indexB] = xB;
}
return minSeparation >= -3.0f * B3_LINEAR_SLOP;
}

12
src/bounce/cloth/cloth_solver.cpp
View File

@ -57,15 +57,10 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
m_bodyContactCapacity = def.bodyContactCapacity;
m_bodyContactCount = 0;
m_bodyContacts = (b3BodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3BodyContact*));;
m_particleContactCapacity = def.particleContactCapacity;
m_particleContactCount = 0;
m_particleContacts = (b3ParticleContact**)m_allocator->Allocate(m_particleContactCapacity * sizeof(b3ParticleContact*));;
}
b3ClothSolver::~b3ClothSolver()
{
m_allocator->Free(m_particleContacts);
m_allocator->Free(m_bodyContacts);
m_allocator->Free(m_constraints);
@ -329,14 +324,11 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
contactSolverDef.velocities = m_solverData.v;
contactSolverDef.bodyContactCount = m_bodyContactCount;
contactSolverDef.bodyContacts = m_bodyContacts;
contactSolverDef.particleContactCount = m_particleContactCount;
contactSolverDef.particleContacts = m_particleContacts;
b3ClothContactSolver contactSolver(contactSolverDef);
{
contactSolver.InitializeBodyContactConstraints();
contactSolver.InitializeParticleContactConstraints();
}
{
@ -350,7 +342,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
for (u32 i = 0; i < kVelocityIterations; ++i)
{
contactSolver.SolveBodyContactVelocityConstraints();
contactSolver.SolveParticleContactVelocityConstraints();
}
}
@ -371,9 +362,8 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
for (u32 i = 0; i < kPositionIterations; ++i)
{
bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints();
bool particleContactsSolved = contactSolver.SolveParticleContactPositionConstraints();
if (bodyContactsSolved && particleContactsSolved)
if (bodyContactsSolved)
{
positionSolved = true;
break;

36
src/bounce/cloth/particle.cpp
View File

@ -27,46 +27,14 @@ void b3BodyContactWorldPoint::Initialize(const b3BodyContact* c, float32 rA, con
b3Vec3 cA = b3Mul(xfA, c->localPoint1);
b3Vec3 cB = b3Mul(xfB, c->localPoint2);
b3Vec3 d = cB - cA;
float32 distance = b3Length(d);
b3Vec3 nA(0.0f, 1.0f, 0.0f);
if (distance > B3_EPSILON)
{
nA = d / distance;
}
b3Vec3 pA = cA + rA * nA;
b3Vec3 pB = cB - rB * nA;
point = 0.5f * (pA + pB);
normal = nA;
separation = distance - rA - rB;
}
void b3ParticleContactWorldPoint::Initialize(const b3ParticleContact* c)
{
b3Vec3 cA = c->p1->GetPosition();
float32 rA = c->p1->GetRadius();
b3Vec3 cB = c->p2->GetPosition();
float32 rB = c->p2->GetRadius();
b3Vec3 d = cB - cA;
float32 distance = b3Length(d);
b3Vec3 nA(0.0f, 1.0f, 0.0f);
if (distance > B3_EPSILON)
{
nA = d / distance;
}
b3Vec3 nA = c->n;
b3Vec3 pA = cA + rA * nA;
b3Vec3 pB = cB - rB * nA;
point = 0.5f * (pA + pB);
normal = nA;
separation = distance - rA - rB;
separation = b3Dot(cB - cA, nA) - rA - rB;
}
b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)