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

Caching impulses for b3BodyContact has been added. Also added some code for completeness.

Browse Source
This commit is contained in:
Irlan 2019-04-03 11:05:21 -03:00
parent 397a8eeee3
commit b4d3330579
5 changed files with 176 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -183,9 +183,6 @@ private:
// Pool of particles
b3BlockPool m_particleBlocks;
// Pool of body contacts
b3BlockPool m_bodyContactBlocks;
// Pool of particle contacts
b3BlockPool m_particleContactBlocks;
@ -195,9 +192,6 @@ private:
// List of forces
b3List2<b3Force> m_forceList;
// List of particle contacts
b3List2<b3BodyContact> m_bodyContactList;
// List of particle contacts
b3List2<b3ParticleContact> m_particleContactList;
};

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

@ -71,6 +71,16 @@ public:
b3Particle* p1;
b3Shape* s2;
// Contact constraint
float32 s;
b3Vec3 p;
b3Vec3 n;
float32 fn0;
float32 fn;
float32 ft1, ft2;
bool t1Active;
bool t2Active;
// Contact constraint
b3Vec3 localPoint1;
b3Vec3 localPoint2;
@ -80,8 +90,7 @@ public:
b3Vec3 t1, t2;
b3Vec2 tangentImpulse;
b3BodyContact* m_prev;
b3BodyContact* m_next;
bool active;
};
struct b3BodyContactWorldPoint
@ -198,7 +207,7 @@ private:
// Applied external force
b3Vec3 m_force;
// Applied external translation
// Applied translation
b3Vec3 m_translation;
// Mass
@ -230,6 +239,9 @@ private:
//
b3Cloth* m_cloth;
//
b3BodyContact m_bodyContact;
//
b3Particle* m_prev;

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

@ -148,7 +148,6 @@ static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m)
b3Cloth::b3Cloth(const b3ClothDef& def) :
m_particleBlocks(sizeof(b3Particle)),
m_bodyContactBlocks(sizeof(b3BodyContact)),
m_particleContactBlocks(sizeof(b3ParticleContact))
{
B3_ASSERT(def.mesh);
@ -484,17 +483,6 @@ void b3Cloth::UpdateBodyContacts()
B3_PROFILE("Cloth Update Body Contacts");
// Clear buffer
b3BodyContact* c = m_bodyContactList.m_head;
while (c)
{
b3BodyContact* c0 = c;
c = c->m_next;
m_bodyContactList.Remove(c0);
c0->~b3BodyContact();
m_bodyContactBlocks.Free(c0);
}
// Create contacts
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
{
@ -539,6 +527,7 @@ void b3Cloth::UpdateBodyContacts()
if (bestShape == nullptr)
{
p->m_bodyContact.active = false;
continue;
}
@ -549,17 +538,108 @@ void b3Cloth::UpdateBodyContacts()
b3Vec3 point = bestPoint;
b3Vec3 normal = -bestNormal;
b3BodyContact* c = (b3BodyContact*)m_bodyContactBlocks.Allocate();
b3BodyContact* c = &p->m_bodyContact;
b3BodyContact c0 = *c;
c->active = true;
c->p1 = p;
c->s2 = shape;
c->s = separation;
c->p = point;
c->n = normal;
c->fn0 = 0.0f;
c->fn = 0.0f;
c->ft1 = 0.0f;
c->ft2 = 0.0f;
c->t1Active = false;
c->t2Active = false;
c->localPoint1.SetZero();
c->localPoint2 = body->GetLocalPoint(point);
c->t1 = b3Perp(normal);
c->t2 = b3Cross(c->t1, normal);
c->normalImpulse = 0.0f;
c->tangentImpulse.SetZero();
#if 0
// Apply position correction
b3Vec3 dx = separation * normal;
p->m_translation += dx;
#endif
m_bodyContactList.PushFront(c);
// Update contact state
if (c0.active == true)
{
c->fn0 = c0.fn0;
c->fn = c0.fn;
c->ft1 = c0.ft1;
c->ft2 = c0.ft2;
c->normalImpulse = c0.normalImpulse;
c->tangentImpulse = c0.tangentImpulse;
#if 0
const float32 kForceTol = 0.0f;
// Allow the contact to release when the constraint force
// switches from a repulsive force to an attractive one.
// if (c0.fn0 < kForceTol && c0.fn > kForceTol)
if (c0.fn > kForceTol)
{
// Contact force is attractive.
c->active = false;
continue;
}
#endif
}
#if 0
// A friction force requires an associated normal force.
if (c0.active == false)
{
continue;
}
b3Vec3 v1; v1.SetZero();
b3Vec3 v2 = p->m_velocity;
b3Vec3 dv = v2 - v1;
const float32 kVelTol = B3_EPSILON * B3_EPSILON;
// Lock particle on surface
float32 dvt1 = b3Dot(dv, c->t1);
if (dvt1 * dvt1 < kVelTol * kVelTol)
{
c->t1Active = true;
}
float32 dvt2 = b3Dot(dv, c->t2);
if (dvt2 * dvt2 < kVelTol * kVelTol)
{
c->t2Active = true;
}
// Unlock particle off surface
float32 normalForce = c->fn;
float32 friction = shape->GetFriction();
float32 maxFrictionForce = friction * normalForce;
if (c0.t1Active == true)
{
float32 tangentForce1 = c0.ft1;
if (tangentForce1 * tangentForce1 > maxFrictionForce * maxFrictionForce)
{
c->t1Active = false;
}
}
if (c0.t2Active == true)
{
float32 tangentForce2 = c0.ft2;
if (tangentForce2 * tangentForce2 > maxFrictionForce* maxFrictionForce)
{
c->t2Active = false;
}
}
#endif
}
}
@ -632,7 +712,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
solverDef.stack = &m_stackAllocator;
solverDef.particleCapacity = m_particleList.m_count;
solverDef.forceCapacity = m_forceList.m_count;
solverDef.bodyContactCapacity = m_bodyContactList.m_count;
solverDef.bodyContactCapacity = m_particleList.m_count;
solverDef.particleContactCapacity = m_particleContactList.m_count;
b3ClothSolver solver(solverDef);
@ -646,26 +726,22 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
{
solver.Add(f);
}
for (b3BodyContact* c = m_bodyContactList.m_head; c; c = c->m_next)
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
{
solver.Add(c);
if (p->m_bodyContact.active)
{
solver.Add(&p->m_bodyContact);
}
}
for (b3ParticleContact* c = m_particleContactList.m_head; c; c = c->m_next)
{
solver.Add(c);
}
// Solve
solver.Solve(dt, gravity);
// Clear external applied forces and translations
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
{
p->m_force.SetZero();
p->m_translation.SetZero();
}
}
void b3Cloth::UpdateContacts()
@ -691,6 +767,13 @@ void b3Cloth::Step(float32 dt)
{
Solve(dt, m_gravity);
}
// Clear external applied forces and translations
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
{
p->m_force.SetZero();
p->m_translation.SetZero();
}
}
void b3Cloth::Draw() const

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

@ -149,7 +149,7 @@ void b3ClothSolver::ApplyConstraints()
S.SetIdentity();
z.SetZero();
for (u32 i = 0; i < m_particleCount; ++i)
{
b3Particle* p = m_particles[i];
@ -162,33 +162,42 @@ void b3ClothSolver::ApplyConstraints()
ac->z.SetZero();
}
}
#if 0
for (u32 i = 0; i < m_bodyContactCount; ++i)
{
b3BodyContact* bc = m_bodyContacts[i];
b3Particle* p1 = bc->p1;
B3_ASSERT(p1->m_type == e_dynamicParticle);
b3Transform xf1;
xf1.position = x[p1->m_solverId];
xf1.rotation.SetIdentity();
b3Shape* s2 = bc->s2;
b3Body* b2 = s2->GetBody();
b3Transform xf2 = b2->GetTransform();
b3BodyContactWorldPoint bcwp;
bcwp.Initialize(bc, p1->m_radius, xf1, s2->m_radius, xf2);
b3AccelerationConstraint* ac = m_constraints + m_constraintCount;
++m_constraintCount;
ac->i1 = p1->m_solverId;
ac->ndof = 2;
ac->p = bcwp.normal;
ac->p = bc->n;
ac->z.SetZero();
if (bc->t1Active && bc->t2Active)
{
ac->ndof = 0;
}
else
{
if (bc->t1Active)
{
ac->ndof = 1;
ac->q = bc->t1;
}
if (bc->t2Active)
{
ac->ndof = 1;
ac->q = bc->t2;
}
}
}
#endif
for (u32 i = 0; i < m_constraintCount; ++i)
{
@ -210,7 +219,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
m_solverData.x = &sx;
m_solverData.v = &sv;
m_solverData.f = &sf;
for (u32 i = 0; i < m_particleCount; ++i)
{
b3Particle* p = m_particles[i];
@ -249,25 +258,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
sy[i] = p->m_translation;
sx0[i] = p->m_x;
}
for (u32 i = 0; i < m_bodyContactCount; ++i)
{
b3BodyContact* bc = m_bodyContacts[i];
b3Particle* p1 = bc->p1;
b3Transform xf1;
xf1.position = sx[p1->m_solverId];
xf1.rotation.SetIdentity();
b3Shape* s2 = bc->s2;
b3Body* b2 = s2->GetBody();
b3Transform xf2 = b2->GetTransform();
b3BodyContactWorldPoint bcwp;
bcwp.Initialize(bc, p1->m_radius, xf1, s2->m_radius, xf2);
sy[p1->m_solverId] += bcwp.separation * bcwp.normal;
}
// Apply internal forces
ApplyForces();
@ -290,16 +280,33 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
// b
b3DenseVec3 b = h * (sf + h * (dfdx * sv) + dfdx * sy);
// x
b3DenseVec3 x(m_particleCount);
u32 iterations = 0;
Solve(x, iterations, A, b, S, z, sx0);
b3_clothSolverIterations = iterations;
#if 0
// Copy constraint forces to the contacts
b3DenseVec3 f = A * x - b;
for (u32 i = 0; i < m_bodyContactCount; ++i)
{
b3BodyContact* c = m_bodyContacts[i];
b3Vec3 cf = f[i];
c->fn0 = c->fn;
c->fn = b3Dot(cf, c->n);
c->ft1 = b3Dot(cf, c->t1);
c->ft2 = b3Dot(cf, c->t2);
}
#endif
//
sv = sv + x;
sx = sx + sy;
// Copy state buffers back to the particle
for (u32 i = 0; i < m_particleCount; ++i)
{
b3Particle* p = m_particles[i];
@ -309,7 +316,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
p->m_velocity = sv[i];
}
}
#if 1
// Solve constraints
b3ClothContactSolverDef contactSolverDef;
contactSolverDef.allocator = m_allocator;
@ -341,10 +348,11 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
contactSolver.SolveParticleContactVelocityConstraints();
}
}
{
contactSolver.StoreImpulses();
}
#endif
// Integrate positions
sx = sx + h * sv;
@ -358,7 +366,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
{
bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints();
bool particleContactsSolved = contactSolver.SolveParticleContactPositionConstraints();
if (bodyContactsSolved && particleContactsSolved)
{
positionSolved = true;
@ -366,7 +374,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
}
}
}
#if 0
// Synchronize bodies
for (u32 i = 0; i < m_bodyContactCount; ++i)
{
@ -378,7 +386,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
body->SynchronizeShapes();
}
#endif
// Copy state buffers back to the particles
for (u32 i = 0; i < m_particleCount; ++i)
{

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

@ -73,7 +73,6 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)
{
m_cloth = cloth;
m_type = def.type;
m_position = def.position;
m_velocity = def.velocity;
m_force = def.force;
@ -85,6 +84,7 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)
m_userData = nullptr;
m_x.SetZero();
m_vertex = ~0;
m_bodyContact.active = false;
}
b3Particle::~b3Particle()