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

bugfix, particle vs. particle collision

Browse Source
This commit is contained in:
Irlan 2018-07-26 21:13:49 -03:00
parent f231948747
commit 863bf7f052
12 changed files with 661 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/testbed/framework/test_entries.cpp
View File

@ -65,6 +65,7 @@
#include <testbed/tests/shirt.h>
#include <testbed/tests/particle_types.h>
#include <testbed/tests/tension_mapping.h>
#include <testbed/tests/self_collision.h>
#include <testbed/tests/single_pendulum.h>
#include <testbed/tests/rope_test.h>
#include <testbed/tests/mass_spring.h>
@ -117,6 +118,7 @@ TestEntry g_tests[] =
{ "Shirt", &Shirt::Create },
{ "Particle Types", &ParticleTypes::Create },
{ "Tension Mapping", &TensionMapping::Create },
{ "Self-Collision", &SelfCollision::Create },
{ "Mass-Spring System", &MassSpring::Create },
{ "Single Pendulum", &SinglePendulum::Create },
{ "Rope", &Rope::Create },

83
examples/testbed/tests/self_collision.h Normal file
View File

@ -0,0 +1,83 @@
/*
* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SELF_COLLISION_H
#define SELF_COLLISION_H
class SelfCollision : public ClothTest
{
public:
SelfCollision() : m_rectangleGarment(5.0f, 5.0f)
{
// Generate 2D mesh
m_rectangleGarmentMesh.Set(&m_rectangleGarment, 1.0f);
// Create 3D mesh
m_rectangleClothMesh.Set(&m_rectangleGarmentMesh);
//
for (u32 i = 0; i < m_rectangleClothMesh.vertexCount; ++i)
{
m_rectangleClothMesh.vertices[i].y += 10.0f;
}
b3ClothDef def;
def.mesh = &m_rectangleClothMesh;
def.density = 1.0f;
def.structural = 100000.0f;
m_cloth = m_world.CreateCloth(def);
for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext())
{
p->SetRadius(0.25f);
}
{
b3BodyDef bd;
bd.type = e_staticBody;
b3Body* b = m_world.CreateBody(bd);
m_boxHull.Set(50.0f, 1.0f, 50.0f);
b3HullShape boxShape;
boxShape.m_hull = &m_boxHull;
boxShape.m_radius = 0.2f;
b3ShapeDef sd;
sd.shape = &boxShape;
sd.friction = 1.0f;
b->CreateShape(sd);
}
}
static Test* Create()
{
return new SelfCollision();
}
b3RectangleGarment m_rectangleGarment;
b3GarmentMesh m_rectangleGarmentMesh;
b3GarmentClothMesh m_rectangleClothMesh;
b3BoxHull m_boxHull;
};
#endif

16
include/bounce/dynamics/cloth/cloth.h
View File

@ -28,6 +28,8 @@ class b3Shape;
class b3Particle;
class b3Force;
class b3BodyContact;
class b3ParticleContact;
struct b3ParticleDef;
struct b3ForceDef;
@ -143,10 +145,12 @@ private:
// Compute mass of each particle.
void ComputeMass();
// Update contacts.
// This is where some contacts might be initiated or terminated.
void UpdateContacts();
// Update body contacts.
void UpdateBodyContacts();
// Update particle contacts.
void UpdateParticleContacts();
// Solve
void Solve(float32 dt, const b3Vec3& gravity);
@ -162,12 +166,18 @@ 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;
// The parent world of this cloth.
b3World* m_world;

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

@ -28,6 +28,7 @@ class b3Particle;
class b3Body;
class b3Force;
class b3BodyContact;
class b3ParticleContact;
struct b3DenseVec3;
struct b3DiagMat33;
@ -38,7 +39,8 @@ struct b3ClothSolverDef
b3StackAllocator* stack;
u32 particleCapacity;
u32 forceCapacity;
u32 contactCapacity;
u32 bodyContactCapacity;
u32 particleContactCapacity;
};
struct b3ClothSolverData
@ -113,6 +115,40 @@ struct b3ClothSolverContactPositionConstraint
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;
};
class b3ClothSolver
{
public:
@ -122,6 +158,7 @@ public:
void Add(b3Particle* p);
void Add(b3Force* f);
void Add(b3BodyContact* c);
void Add(b3ParticleContact* c);
void Solve(float32 dt, const b3Vec3& gravity);
private:
@ -135,19 +172,28 @@ private:
void Solve(b3DenseVec3& x, u32& iterations, const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const;
//
void InitializeConstraints();
void InitializeBodyContactConstraints();
//
void InitializeParticleContactConstraints();
//
void WarmStart();
//
void SolveVelocityConstraints();
void SolveBodyContactVelocityConstraints();
//
void SolveParticleContactVelocityConstraints();
//
void StoreImpulses();
//
bool SolvePositionConstraints();
bool SolveBodyContactPositionConstraints();
//
bool SolveParticleContactPositionConstraints();
b3StackAllocator* m_allocator;
@ -163,12 +209,17 @@ private:
u32 m_constraintCount;
b3AccelerationConstraint* m_constraints;
u32 m_contactCapacity;
u32 m_contactCount;
b3BodyContact** m_contacts;
u32 m_bodyContactCapacity;
u32 m_bodyContactCount;
b3BodyContact** m_bodyContacts;
b3ClothSolverContactVelocityConstraint* m_bodyVelocityConstraints;
b3ClothSolverContactPositionConstraint* m_bodyPositionConstraints;
b3ClothSolverContactVelocityConstraint* m_velocityConstraints;
b3ClothSolverContactPositionConstraint* m_positionConstraints;
u32 m_particleContactCapacity;
u32 m_particleContactCount;
b3ParticleContact** m_particleContacts;
b3ClothSolverParticleContactVelocityConstraint* m_particleVelocityConstraints;
b3ClothSolverParticleContactPositionConstraint* m_particlePositionConstraints;
b3ClothSolverData m_solverData;
};

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

@ -104,6 +104,36 @@ 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
{
@ -134,7 +164,10 @@ public:
// Get the particle mass.
float32 GetMass() const;
// Get the particle radius;
// Set the particle radius.
void SetRadius(float32 radius);
// Get the particle radius.
float32 GetRadius() const;
// Apply a force.
@ -248,6 +281,11 @@ inline float32 b3Particle::GetMass() const
return m_mass;
}
inline void b3Particle::SetRadius(float32 radius)
{
m_radius = radius;
}
inline float32 b3Particle::GetRadius() const
{
return m_radius;

1
include/bounce/dynamics/shapes/shape.h
View File

@ -40,6 +40,7 @@ enum b3ShapeType
struct b3TestSphereOutput
{
b3Vec3 point;
float32 separation;
b3Vec3 normal;
};

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

@ -149,7 +149,9 @@ static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m)
return sharedCount;
}
b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) : m_particleBlocks(sizeof(b3Particle))
b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) :
m_particleBlocks(sizeof(b3Particle)),
m_particleContactBlocks(sizeof(b3ParticleContact))
{
B3_ASSERT(def.mesh);
B3_ASSERT(def.density > 0.0f);
@ -504,9 +506,9 @@ bool b3Cloth::RayCast(b3RayCastOutput* output, const b3RayCastInput* input, u32
return false;
}
void b3Cloth::UpdateContacts()
void b3Cloth::UpdateBodyContacts()
{
B3_PROFILE("Update Contacts");
B3_PROFILE("Update Body Contacts");
// Create contacts
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
@ -523,6 +525,7 @@ void b3Cloth::UpdateContacts()
// Find the deepest penetration
float32 bestSeparation = 0.0f;
b3Vec3 bestPoint(0.0f, 0.0f, 0.0f);
b3Vec3 bestNormal(0.0f, 0.0f, 0.0f);
b3Shape* bestShape = nullptr;
@ -543,6 +546,7 @@ void b3Cloth::UpdateContacts()
if (output.separation < bestSeparation)
{
bestSeparation = output.separation;
bestPoint = output.point;
bestNormal = output.normal;
bestShape = shape;
}
@ -555,21 +559,19 @@ void b3Cloth::UpdateContacts()
continue;
}
// Ensure the the normal points from the particle 1 to shape 2
b3Shape* shape = bestShape;
b3Body* body = shape->GetBody();
float32 s = bestSeparation;
b3Vec3 n = bestNormal;
b3Vec3 cp2r = p->m_position - s * n;
b3Vec3 cp2 = cp2r - shape->m_radius * n;
b3Vec3 cb = bestPoint;
b3Vec3 n = -bestNormal;
// Store the local contact manifold
// Ensure the the normal points from the particle 1 to shape 2
c->active = true;
c->p1 = p;
c->s2 = shape;
c->localNormal1 = -n;
c->localNormal1 = n;
c->localPoint1.SetZero();
c->localPoint2 = body->GetLocalPoint(cp2);
c->localPoint2 = body->GetLocalPoint(cb);
c->t1 = b3Perp(n);
c->t2 = b3Cross(c->t1, n);
c->normalImpulse = 0.0f;
@ -583,6 +585,66 @@ void b3Cloth::UpdateContacts()
}
}
void b3Cloth::UpdateParticleContacts()
{
B3_PROFILE("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("Solve");
@ -592,7 +654,8 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
solverDef.stack = &m_world->m_stackAllocator;
solverDef.particleCapacity = m_particleList.m_count;
solverDef.forceCapacity = m_forceList.m_count;
solverDef.contactCapacity = m_particleList.m_count;
solverDef.bodyContactCapacity = m_particleList.m_count;
solverDef.particleContactCapacity = m_particleContactList.m_count;
b3ClothSolver solver(solverDef);
@ -608,12 +671,19 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
{
if (p->m_contact.active)
b3BodyContact* bc = &p->m_contact;
if (bc->active)
{
solver.Add(&p->m_contact);
solver.Add(bc);
}
}
for (b3ParticleContact* pc = m_particleContactList.m_head; pc; pc = pc->m_next)
{
solver.Add(pc);
}
// Solve
solver.Solve(dt, gravity);
@ -629,9 +699,12 @@ void b3Cloth::Step(float32 dt, const b3Vec3& gravity)
{
B3_PROFILE("Step");
// Update contacts
UpdateContacts();
// Update body contacts
UpdateBodyContacts();
// Update particle contacts
UpdateParticleContacts();
// Solve constraints, integrate state, clear forces and translations.
if (dt > 0.0f)
{

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

@ -52,19 +52,29 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
m_constraintCount = 0;
m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_constraintCapacity * sizeof(b3AccelerationConstraint));
m_contactCapacity = def.contactCapacity;
m_contactCount = 0;
m_contacts = (b3BodyContact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3BodyContact*));
m_bodyContactCapacity = def.bodyContactCapacity;
m_bodyContactCount = 0;
m_bodyContacts = (b3BodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3BodyContact*));
m_bodyVelocityConstraints = (b3ClothSolverContactVelocityConstraint*)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3ClothSolverContactVelocityConstraint));
m_bodyPositionConstraints = (b3ClothSolverContactPositionConstraint*)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3ClothSolverContactPositionConstraint));
m_velocityConstraints = (b3ClothSolverContactVelocityConstraint*)m_allocator->Allocate(m_contactCapacity * sizeof(b3ClothSolverContactVelocityConstraint));
m_positionConstraints = (b3ClothSolverContactPositionConstraint*)m_allocator->Allocate(m_contactCapacity * sizeof(b3ClothSolverContactPositionConstraint));
m_particleContactCapacity = def.particleContactCapacity;
m_particleContactCount = 0;
m_particleContacts = (b3ParticleContact**)m_allocator->Allocate(m_particleContactCapacity * sizeof(b3ParticleContact*));
m_particleVelocityConstraints = (b3ClothSolverParticleContactVelocityConstraint*)m_allocator->Allocate(m_particleContactCapacity * sizeof(b3ClothSolverParticleContactVelocityConstraint));
m_particlePositionConstraints = (b3ClothSolverParticleContactPositionConstraint*)m_allocator->Allocate(m_particleContactCapacity * sizeof(b3ClothSolverParticleContactPositionConstraint));
}
b3ClothSolver::~b3ClothSolver()
{
m_allocator->Free(m_positionConstraints);
m_allocator->Free(m_velocityConstraints);
m_allocator->Free(m_contacts);
m_allocator->Free(m_particlePositionConstraints);
m_allocator->Free(m_particleVelocityConstraints);
m_allocator->Free(m_particleContacts);
m_allocator->Free(m_bodyPositionConstraints);
m_allocator->Free(m_bodyVelocityConstraints);
m_allocator->Free(m_bodyContacts);
m_allocator->Free(m_constraints);
m_allocator->Free(m_forces);
m_allocator->Free(m_particles);
@ -76,16 +86,21 @@ void b3ClothSolver::Add(b3Particle* p)
m_particles[m_particleCount++] = p;
}
void b3ClothSolver::Add(b3BodyContact* c)
{
m_contacts[m_contactCount++] = c;
}
void b3ClothSolver::Add(b3Force* f)
{
m_forces[m_forceCount++] = f;
}
void b3ClothSolver::Add(b3BodyContact* c)
{
m_bodyContacts[m_bodyContactCount++] = c;
}
void b3ClothSolver::Add(b3ParticleContact* c)
{
m_particleContacts[m_particleContactCount++] = c;
}
void b3ClothSolver::ApplyForces()
{
for (u32 i = 0; i < m_forceCount; ++i)
@ -242,17 +257,22 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
m_particles[i]->m_x = x[i];
}
// Initialize constraints
InitializeConstraints();
// Initialize body contact constraints
InitializeBodyContactConstraints();
// Initialize particle contact constraints
InitializeParticleContactConstraints();
// Warm start velocity constraints
WarmStart();
// Solve velocity constraints
const u32 kVelocityIterations = 10;
const u32 kVelocityIterations = 8;
for (u32 i = 0; i < kVelocityIterations; ++i)
{
SolveVelocityConstraints();
SolveBodyContactVelocityConstraints();
SolveParticleContactVelocityConstraints();
}
// Store impulses to improve convergence
@ -261,22 +281,24 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
// Integrate positions
sx = sx + h * sv;
#if 0
// Solve position constraints
const u32 kPositionIterations = 0;
const u32 kPositionIterations = 2;
bool positionSolved = false;
for (u32 i = 0; i < kPositionIterations; ++i)
{
bool contactsSolved = SolvePositionConstraints();
if (contactsSolved)
bool particleContactsSolved = SolveParticleContactPositionConstraints();
if (particleContactsSolved)
{
positionSolved = true;
break;
}
}
// Synchronize bodies
for (u32 i = 0; i < m_contactCount; ++i)
for (u32 i = 0; i < m_bodyContactCount; ++i)
{
b3Body* body = m_contacts[i]->s2->GetBody();
b3Body* body = m_bodyContacts[i]->s2->GetBody();
body->SynchronizeTransform();
@ -284,7 +306,6 @@ 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)
@ -404,18 +425,18 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
iterations = iteration;
}
void b3ClothSolver::InitializeConstraints()
void b3ClothSolver::InitializeBodyContactConstraints()
{
b3DenseVec3& x = *m_solverData.x;
b3DenseVec3& v = *m_solverData.v;
float32 inv_dt = m_solverData.invdt;
for (u32 i = 0; i < m_contactCount; ++i)
for (u32 i = 0; i < m_bodyContactCount; ++i)
{
b3BodyContact* c = m_contacts[i];
b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i;
b3ClothSolverContactPositionConstraint* pc = m_positionConstraints + i;
b3BodyContact* c = m_bodyContacts[i];
b3ClothSolverContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
b3ClothSolverContactPositionConstraint* pc = m_bodyPositionConstraints + i;
vc->indexA = c->p1->m_solverId;
vc->bodyB = c->s2->GetBody();
@ -436,7 +457,7 @@ void b3ClothSolver::InitializeConstraints()
pc->invIA.SetZero();
pc->invIB = vc->bodyB->GetWorldInverseInertia();
pc->radiusA = c->p1->m_radius;
pc->radiusB = c->s2->m_radius;
@ -448,11 +469,11 @@ void b3ClothSolver::InitializeConstraints()
pc->localPointB = c->localPoint2;
}
for (u32 i = 0; i < m_contactCount; ++i)
for (u32 i = 0; i < m_bodyContactCount; ++i)
{
b3BodyContact* c = m_contacts[i];
b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i;
b3ClothSolverContactPositionConstraint* pc = m_positionConstraints + i;
b3BodyContact* c = m_bodyContacts[i];
b3ClothSolverContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
b3ClothSolverContactPositionConstraint* pc = m_bodyPositionConstraints + i;
u32 indexA = vc->indexA;
b3Body* bodyB = vc->bodyB;
@ -468,7 +489,7 @@ void b3ClothSolver::InitializeConstraints()
b3Quat qA; qA.SetIdentity();
b3Quat qB = bodyB->m_sweep.orientation;
b3Vec3 localCenterA = pc->localCenterA;
b3Vec3 localCenterB = pc->localCenterB;
@ -510,7 +531,7 @@ void b3ClothSolver::InitializeConstraints()
vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f;
float32 C = b3Min(0.0f, wp.separation + B3_LINEAR_SLOP);
vc->velocityBias = -inv_dt * B3_BAUMGARTE * C;
}
@ -538,13 +559,99 @@ void b3ClothSolver::InitializeConstraints()
}
}
void b3ClothSolver::InitializeParticleContactConstraints()
{
b3DenseVec3& x = *m_solverData.x;
b3DenseVec3& v = *m_solverData.v;
float32 inv_dt = m_solverData.invdt;
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 = 1.0f;
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 b3ClothSolver::WarmStart()
{
b3DenseVec3& v = *m_solverData.v;
for (u32 i = 0; i < m_contactCount; ++i)
for (u32 i = 0; i < m_bodyContactCount; ++i)
{
b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i;
b3ClothSolverContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
u32 indexA = vc->indexA;
b3Body* bodyB = vc->bodyB;
@ -583,15 +690,43 @@ void b3ClothSolver::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 b3ClothSolver::SolveVelocityConstraints()
void b3ClothSolver::SolveBodyContactVelocityConstraints()
{
b3DenseVec3& v = *m_solverData.v;
for (u32 i = 0; i < m_contactCount; ++i)
for (u32 i = 0; i < m_bodyContactCount; ++i)
{
b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i;
b3ClothSolverContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
u32 indexA = vc->indexA;
b3Body* bodyB = vc->bodyB;
@ -673,12 +808,92 @@ void b3ClothSolver::SolveVelocityConstraints()
}
}
void b3ClothSolver::SolveParticleContactVelocityConstraints()
{
b3DenseVec3& v = *m_solverData.v;
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 b3ClothSolver::StoreImpulses()
{
for (u32 i = 0; i < m_contactCount; ++i)
for (u32 i = 0; i < m_bodyContactCount; ++i)
{
b3BodyContact* c = m_contacts[i];
b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i;
b3BodyContact* c = m_bodyContacts[i];
b3ClothSolverContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
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;
@ -701,15 +916,15 @@ struct b3ClothSolverContactSolverPoint
float32 separation;
};
bool b3ClothSolver::SolvePositionConstraints()
bool b3ClothSolver::SolveBodyContactPositionConstraints()
{
b3DenseVec3& x = *m_solverData.v;
float32 minSeparation = 0.0f;
for (u32 i = 0; i < m_contactCount; ++i)
for (u32 i = 0; i < m_bodyContactCount; ++i)
{
b3ClothSolverContactPositionConstraint* pc = m_positionConstraints + i;
b3ClothSolverContactPositionConstraint* pc = m_bodyPositionConstraints + i;
u32 indexA = pc->indexA;
float32 mA = pc->invMassA;
@ -775,5 +990,86 @@ bool b3ClothSolver::SolvePositionConstraints()
bodyB->m_sweep.orientation = qB;
}
return minSeparation >= -3.0f * B3_LINEAR_SLOP;
}
struct b3ClothSolverParticleContactSolverPoint
{
void Initialize(const b3Particle* p1, const b3Particle* p2)
{
b3Vec3 cA = p1->GetPosition();
float32 rA = p1->GetRadius();
b3Vec3 cB = p2->GetPosition();
float32 rB = 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 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 b3ClothSolver::SolveParticleContactPositionConstraints()
{
b3DenseVec3& x = *m_solverData.v;
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;
u32 indexB = pc->indexB;
float32 mB = pc->invMassB;
b3Vec3 xA = x[indexA];
b3Vec3 xB = x[indexB];
b3ClothSolverParticleContactSolverPoint cpcp;
cpcp.Initialize(m_particles[indexA], m_particles[indexB]);
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;
}

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

@ -36,6 +36,31 @@ void b3BodyContactWorldPoint::Initialize(const b3BodyContact* c, float32 rA, con
separation = b3Dot(cB - cA, nA) - 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 pA = cA + rA * nA;
b3Vec3 pB = cB - rB * nA;
point = 0.5f * (pA + pB);
normal = nA;
separation = distance - rA - rB;
}
b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)
{
m_cloth = cloth;

3
src/bounce/dynamics/shapes/capsule_shape.cpp
View File

@ -215,6 +215,7 @@ bool b3CapsuleShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphe
n = d / len;
}
output->point = A;
output->separation = len - radius;
output->normal = n;
@ -239,6 +240,7 @@ bool b3CapsuleShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphe
n = d / len;
}
output->point = B;
output->separation = len - radius;
output->normal = n;
@ -274,6 +276,7 @@ bool b3CapsuleShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphe
}
n.Normalize();
output->point = P;
output->separation = b3Sqrt(dd) - radius;
output->normal = -n;
return true;

4
src/bounce/dynamics/shapes/hull_shape.cpp
View File

@ -374,8 +374,10 @@ bool b3HullShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere,
if (maxIndex != ~0)
{
b3Plane plane = b3Mul(xf, m_hull->GetPlane(maxIndex));
output->point = b3ClosestPointOnPlane(sphere.vertex, plane);
output->separation = maxSeparation - radius;
output->normal = b3Mul(xf.rotation, m_hull->GetPlane(maxIndex).normal);
output->normal = plane.normal;
return true;
}

1
src/bounce/dynamics/shapes/sphere_shape.cpp
View File

@ -80,6 +80,7 @@ bool b3SphereShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& spher
{
float32 d_len = b3Sqrt(dd);
output->point = center;
output->separation = d_len - radius;
output->normal.Set(0.0f, 1.0, 0.0f);
if (d_len > B3_EPSILON)