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

maintain the upper triangle of A, external particle/force creation/destruction, particle force abstraction, testbed update

Browse Source
This commit is contained in:
Irlan
2018-05-30 11:34:41 -03:00
parent dba5ffbe06
commit caef3fede8
21 changed files with 1657 additions and 1088 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -17,62 +17,24 @@
*/
#include <bounce/dynamics/cloth/cloth.h>
#include <bounce/dynamics/cloth/cloth_solver.h>
#include <bounce/dynamics/cloth/dense_vec3.h>
#include <bounce/dynamics/cloth/sparse_mat33.h>
#include <bounce/dynamics/cloth/cloth_mesh.h>
#include <bounce/dynamics/shapes/shape.h>
#include <bounce/dynamics/body.h>
#include <bounce/dynamics/cloth/particle.h>
#include <bounce/dynamics/cloth/force.h>
#include <bounce/dynamics/cloth/spring_force.h>
#include <bounce/dynamics/cloth/cloth_solver.h>
#include <bounce/dynamics/world.h>
#include <bounce/dynamics/body.h>
#include <bounce/dynamics/shapes/shape.h>
#include <bounce/collision/collision.h>
#include <bounce/common/memory/stack_allocator.h>
#include <bounce/common/draw.h>
#define B3_FORCE_THRESHOLD 0.005f
#define B3_CLOTH_BENDING 0
#define B3_CLOTH_BENDING 1
#define B3_CLOTH_FRICTION 1
// b3Spring
void b3Spring::InitializeForces(const b3ClothSolverData* data)
{
u32 i1 = p1->solverId;
u32 i2 = p2->solverId;
b3Vec3 x1 = data->x[i1];
b3Vec3 v1 = data->v[i1];
b3Vec3 x2 = data->x[i2];
b3Vec3 v2 = data->v[i2];
b3Mat33 I; I.SetIdentity();
b3Vec3 dx = x1 - x2;
if (b3Dot(dx, dx) >= L0 * L0)
{
float32 L = b3Length(dx);
b3Vec3 n = dx / L;
// Tension
f = -ks * (L - L0) * n;
// Jacobian
Jx = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx)));
}
else
{
f.SetZero();
Jx.SetZero();
}
// Damping
b3Vec3 dv = v1 - v2;
f += -kd * dv;
Jv = -kd * I;
}
static B3_FORCE_INLINE u32 b3NextIndex(u32 i)
{
return i + 1 < 3 ? i + 1 : 0;
@@ -191,92 +153,56 @@ static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m)
return sharedCount;
}
b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world)
b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) : m_particleBlocks(sizeof(b3Particle))
{
B3_ASSERT(def.mesh);
B3_ASSERT(def.density > 0.0f);
m_world = world;
m_allocator = &m_world->m_stackAllocator;
m_mesh = def.mesh;
m_density = def.density;
b3ClothMesh* m = m_mesh;
// Create particles
m_particleCount = m->vertexCount;
m_particles = (b3Particle*)b3Alloc(m_particleCount * sizeof(b3Particle));
m_contacts = (b3BodyContact*)b3Alloc(m_particleCount * sizeof(b3BodyContact));
for (u32 i = 0; i < m_particleCount; ++i)
for (u32 i = 0; i < m->vertexCount; ++i)
{
b3Particle* p = m_particles + i;
p->type = e_dynamicParticle;
p->position = m->vertices[i];
p->velocity.SetZero();
p->force.SetZero();
p->mass = 0.0f;
p->invMass = 0.0f;
p->radius = def.r;
p->userData = nullptr;
b3ParticleDef pd;
pd.type = e_dynamicParticle;
pd.position = m->vertices[i];
p->translation.SetZero();
p->x.SetZero();
b3Particle* p = CreateParticle(pd);
b3BodyContact* c = m_contacts + i;
c->n_active = false;
c->t1_active = false;
c->t2_active = false;
c->Fn = 0.0f;
c->Ft1 = 0.0f;
c->Ft2 = 0.0f;
p->m_vertex = i;
m->particles[i] = p;
}
// Compute mass
ResetMass();
ComputeMass();
// Create springs
m_springCount = 0;
// Create forces
b3StackAllocator* allocator = &m_world->m_stackAllocator;
// Worst-case edge memory
u32 edgeCount = 3 * m->triangleCount;
b3UniqueEdge* uniqueEdges = (b3UniqueEdge*)m_allocator->Allocate(edgeCount * sizeof(b3UniqueEdge));
b3UniqueEdge* uniqueEdges = (b3UniqueEdge*)allocator->Allocate(edgeCount * sizeof(b3UniqueEdge));
u32 uniqueCount = b3FindUniqueEdges(uniqueEdges, m);
u32 springCapacity = uniqueCount;
#if B3_CLOTH_BENDING
b3SharedEdge* sharedEdges = (b3SharedEdge*)m_allocator->Allocate(edgeCount * sizeof(b3SharedEdge));
b3SharedEdge* sharedEdges = (b3SharedEdge*)allocator->Allocate(edgeCount * sizeof(b3SharedEdge));
u32 sharedCount = b3FindSharedEdges(sharedEdges, m);
springCapacity += sharedCount;
#endif
springCapacity += m->sewingLineCount;
m_springs = (b3Spring*)b3Alloc(springCapacity * sizeof(b3Spring));
// Tension
for (u32 i = 0; i < uniqueCount; ++i)
{
b3UniqueEdge* e = uniqueEdges + i;
b3Vec3 v1 = m->vertices[e->v1];
b3Vec3 v2 = m->vertices[e->v2];
b3Particle* p1 = m->particles[e->v1];
b3Particle* p2 = m->particles[e->v2];
b3Particle* p1 = m_particles + e->v1;
b3Particle* p2 = m_particles + e->v2;
b3SpringForceDef fd;
fd.Initialize(p1, p2, def.structural, def.damping);
b3Spring* s = m_springs + m_springCount++;
s->type = e_strechSpring;
s->p1 = p1;
s->p2 = p2;
s->L0 = b3Distance(p1->position, p2->position);
s->ks = def.ks;
s->kd = def.kd;
s->f.SetZero();
CreateForce(fd);
}
#if B3_CLOTH_BENDING
@@ -286,112 +212,265 @@ b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world)
{
b3SharedEdge* e = sharedEdges + i;
b3Vec3 v1 = m->vertices[e->nsv1];
b3Vec3 v2 = m->vertices[e->nsv2];
b3Particle* p1 = m->particles[e->nsv1];
b3Particle* p2 = m->particles[e->nsv2];
b3Particle* p1 = m_particles + e->nsv1;
b3Particle* p2 = m_particles + e->nsv2;
b3Spring* s = m_springs + m_springCount++;
s->type = e_bendSpring;
s->p1 = p1;
s->p2 = p2;
s->L0 = b3Distance(p1->position, p2->position);
s->ks = def.kb;
s->kd = def.kd;
s->tension.SetZero();
b3SpringForceDef fd;
fd.Initialize(p1, p2, def.bending, def.damping);
CreateForce(fd);
}
m_allocator->Free(sharedEdges);
#endif
m_allocator->Free(uniqueEdges);
allocator->Free(sharedEdges);
allocator->Free(uniqueEdges);
// Sewing
for (u32 i = 0; i < m->sewingLineCount; ++i)
{
b3ClothMeshSewingLine* line = m->sewingLines + i;
b3Particle* p1 = m_particles + line->v1;
b3Particle* p2 = m_particles + line->v2;
b3Particle* p1 = m->particles[line->v1];
b3Particle* p2 = m->particles[line->v2];
b3Spring* s = m_springs + m_springCount++;
s->type = e_strechSpring;
s->p1 = p1;
s->p2 = p2;
s->L0 = 0.0f;
s->ks = def.ks;
s->kd = def.kd;
s->f.SetZero();
b3SpringForceDef fd;
fd.Initialize(p1, p2, def.structural, def.damping);
CreateForce(fd);
}
B3_ASSERT(m_springCount <= springCapacity);
}
b3Cloth::~b3Cloth()
{
b3Free(m_particles);
b3Free(m_springs);
b3Free(m_contacts);
b3Particle* p = m_particleList.m_head;
while (p)
{
b3Particle* p0 = p;
p = p->m_next;
p0->~b3Particle();
}
b3Force* f = m_forceList.m_head;
while (f)
{
b3Force* f0 = f;
f = f->m_next;
f0->~b3Force();
b3Free(f0);
}
}
void b3Cloth::ResetMass()
b3Particle* b3Cloth::CreateParticle(const b3ParticleDef& def)
{
for (u32 i = 0; i < m_particleCount; ++i)
void* mem = m_particleBlocks.Allocate();
b3Particle* p = new(mem) b3Particle(def, this);
m_particleList.PushFront(p);
return p;
}
void b3Cloth::DestroyParticle(b3Particle* particle)
{
m_particleList.Remove(particle);
particle->~b3Particle();
m_particleBlocks.Free(particle);
}
b3Force* b3Cloth::CreateForce(const b3ForceDef& def)
{
b3Force* f = b3Force::Create(&def);
m_forceList.PushFront(f);
return f;
}
void b3Cloth::DestroyForce(b3Force* force)
{
m_forceList.Remove(force);
b3Force::Destroy(force);
}
float32 b3Cloth::GetEnergy() const
{
float32 E = 0.0f;
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
{
m_particles[i].mass = 0.0f;
m_particles[i].invMass = 0.0f;
E += p->m_mass * b3Dot(p->m_velocity, p->m_velocity);
}
return 0.5f * E;
}
void b3Cloth::ComputeMass()
{
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
{
p->m_mass = 0.0f;
p->m_invMass = 0.0f;
}
const float32 inv3 = 1.0f / 3.0f;
const float32 rho = m_density;
// Accumulate the mass about the mesh origin of all triangles.
for (u32 i = 0; i < m_mesh->triangleCount; ++i)
{
b3ClothMeshTriangle* triangle = m_mesh->triangles + i;
u32 v1 = triangle->v1;
u32 v2 = triangle->v2;
u32 v3 = triangle->v3;
b3Vec3 p1 = m_mesh->vertices[v1];
b3Vec3 p2 = m_mesh->vertices[v2];
b3Vec3 p3 = m_mesh->vertices[v3];
b3Vec3 v1 = m_mesh->vertices[triangle->v1];
b3Vec3 v2 = m_mesh->vertices[triangle->v2];
b3Vec3 v3 = m_mesh->vertices[triangle->v3];
float32 area = b3Area(p1, p2, p3);
float32 area = b3Area(v1, v2, v3);
B3_ASSERT(area > B3_EPSILON);
float32 mass = rho * area;
m_particles[v1].mass += inv3 * mass;
m_particles[v2].mass += inv3 * mass;
m_particles[v3].mass += inv3 * mass;
b3Particle* p1 = m_mesh->particles[triangle->v1];
b3Particle* p2 = m_mesh->particles[triangle->v2];
b3Particle* p3 = m_mesh->particles[triangle->v3];
p1->m_mass += inv3 * mass;
p2->m_mass += inv3 * mass;
p3->m_mass += inv3 * mass;
}
// Invert
for (u32 i = 0; i < m_particleCount; ++i)
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
{
B3_ASSERT(m_particles[i].mass > 0.0f);
m_particles[i].invMass = 1.0f / m_particles[i].mass;
B3_ASSERT(p->m_mass > 0.0f);
p->m_invMass = 1.0f / p->m_mass;
}
}
bool b3Cloth::RayCast(b3ClothRayCastOutput* output, const b3RayCastInput* input) const
{
output->triangle = ~0;
output->fraction = input->maxFraction;
for (u32 i = 0; i < m_mesh->triangleCount; ++i)
{
b3ClothRayCastOutput subOutput;
if (RayCast(&subOutput, input, i))
{
if (subOutput.fraction < output->fraction)
{
*output = subOutput;
}
}
}
if (output->triangle != ~0)
{
return true;
}
return false;
}
bool b3Cloth::RayCast(b3ClothRayCastOutput* output, const b3RayCastInput* input, u32 triangleIndex) const
{
B3_ASSERT(triangleIndex < m_mesh->triangleCount);
b3ClothMeshTriangle* triangle = m_mesh->triangles + triangleIndex;
b3Vec3 v1 = m_mesh->vertices[triangle->v1];
b3Vec3 v2 = m_mesh->vertices[triangle->v2];
b3Vec3 v3 = m_mesh->vertices[triangle->v3];
b3Vec3 p1 = input->p1;
b3Vec3 p2 = input->p2;
float32 maxFraction = input->maxFraction;
b3Vec3 d = p2 - p1;
B3_ASSERT(b3LengthSquared(d) > B3_EPSILON * B3_EPSILON);
b3Vec3 n = b3Cross(v2 - v1, v3 - v1);
float32 len = b3Length(n);
if (len <= B3_EPSILON)
{
return false;
}
B3_ASSERT(len > B3_EPSILON);
n /= len;
float32 numerator = b3Dot(n, v1 - p1);
float32 denominator = b3Dot(n, d);
if (denominator == 0.0f)
{
return false;
}
float32 fraction = numerator / denominator;
// Is the intersection not on the segment?
if (fraction < 0.0f || maxFraction < fraction)
{
return false;
}
b3Vec3 Q = p1 + fraction * d;
b3Vec3 A = v1;
b3Vec3 B = v2;
b3Vec3 C = v3;
b3Vec3 AB = B - A;
b3Vec3 AC = C - A;
b3Vec3 QA = A - Q;
b3Vec3 QB = B - Q;
b3Vec3 QC = C - Q;
b3Vec3 QB_x_QC = b3Cross(QB, QC);
b3Vec3 QC_x_QA = b3Cross(QC, QA);
b3Vec3 QA_x_QB = b3Cross(QA, QB);
b3Vec3 AB_x_AC = b3Cross(AB, AC);
// Barycentric coordinates for Q
float32 u = b3Dot(QB_x_QC, AB_x_AC);
float32 v = b3Dot(QC_x_QA, AB_x_AC);
float32 w = b3Dot(QA_x_QB, AB_x_AC);
// This tolerance helps intersections lying on
// shared edges to not be missed.
const float32 kTol = -B3_EPSILON;
// Is the intersection on the triangle?
if (u > kTol && v > kTol && w > kTol)
{
output->triangle = triangleIndex;
output->fraction = fraction;
output->point = Q;
// Does the ray start from below or above the triangle?
if (numerator > 0.0f)
{
output->normal = -n;
}
else
{
output->normal = n;
}
return true;
}
return false;
}
void b3Cloth::UpdateContacts()
{
B3_PROFILE("Update Contacts");
// Create contacts
for (u32 i = 0; i < m_particleCount; ++i)
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
{
b3Particle* p = m_particles + i;
// Static and kinematic particles can't participate in unilateral collisions.
if (p->type != e_dynamicParticle)
if (p->m_type != e_dynamicParticle)
{
continue;
}
b3BodyContact* c = m_contacts + i;
b3BodyContact* c = &p->m_contact;
// Save the old contact
b3BodyContact c0 = *c;
@@ -402,8 +481,8 @@ void b3Cloth::UpdateContacts()
c->t2_active = false;
b3Sphere s1;
s1.vertex = p->position;
s1.radius = p->radius;
s1.vertex = p->m_position;
s1.radius = p->m_radius;
// Find the deepest penetration
float32 bestSeparation = 0.0f;
@@ -491,7 +570,7 @@ void b3Cloth::UpdateContacts()
float32 normalForce = c0.Fn;
// Relative velocity
b3Vec3 dv = p->velocity;
b3Vec3 dv = p->m_velocity;
b3Vec3 t1 = dv - b3Dot(dv, n) * n;
if (b3Dot(t1, t1) > B3_EPSILON * B3_EPSILON)
@@ -569,28 +648,28 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
// Solve
b3ClothSolverDef solverDef;
solverDef.stack = m_allocator;
solverDef.particleCapacity = m_particleCount;
solverDef.springCapacity = m_springCount;
solverDef.contactCapacity = m_particleCount;
solverDef.stack = &m_world->m_stackAllocator;
solverDef.particleCapacity = m_particleList.m_count;
solverDef.forceCapacity = m_forceList.m_count;
solverDef.contactCapacity = m_particleList.m_count;
b3ClothSolver solver(solverDef);
for (u32 i = 0; i < m_particleCount; ++i)
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
{
solver.Add(m_particles + i);
solver.Add(p);
}
for (u32 i = 0; i < m_springCount; ++i)
for (b3Force* f = m_forceList.m_head; f; f = f->m_next)
{
solver.Add(m_springs + i);
solver.Add(f);
}
for (u32 i = 0; i < m_particleCount; ++i)
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
{
if (m_contacts[i].n_active)
if (p->m_contact.n_active)
{
solver.Add(m_contacts + i);
solver.Add(&p->m_contact);
}
}
@@ -598,15 +677,11 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
solver.Solve(dt, gravity);
// Clear external applied forces
for (u32 i = 0; i < m_particleCount; ++i)
{
m_particles[i].force.SetZero();
}
// Clear translations
for (u32 i = 0; i < m_particleCount; ++i)
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
{
m_particles[i].translation.SetZero();
p->m_force.SetZero();
p->m_translation.SetZero();
}
}
@@ -626,50 +701,49 @@ void b3Cloth::Step(float32 dt, const b3Vec3& gravity)
void b3Cloth::Apply() const
{
for (u32 i = 0; i < m_particleCount; ++i)
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
{
m_mesh->vertices[i] = m_particles[i].position;
if (p->m_vertex != ~0)
{
m_mesh->vertices[p->m_vertex] = p->m_position;
}
}
}
void b3Cloth::Draw() const
{
for (u32 i = 0; i < m_particleCount; ++i)
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
{
b3Particle* p = m_particles + i;
if (p->type == e_staticParticle)
if (p->m_type == e_staticParticle)
{
b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_white);
b3Draw_draw->DrawPoint(p->m_position, 4.0f, b3Color_white);
}
if (p->type == e_kinematicParticle)
if (p->m_type == e_kinematicParticle)
{
b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_blue);
b3Draw_draw->DrawPoint(p->m_position, 4.0f, b3Color_blue);
}
if (p->type == e_dynamicParticle)
if (p->m_type == e_dynamicParticle)
{
b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_green);
b3Draw_draw->DrawPoint(p->m_position, 4.0f, b3Color_green);
}
b3BodyContact* c = m_contacts + i;
if (c->n_active)
if (p->m_contact.n_active)
{
b3Draw_draw->DrawSegment(p->position, p->position + c->n, b3Color_yellow);
b3Draw_draw->DrawSegment(p->m_position, p->m_position + p->m_contact.n, b3Color_yellow);
}
}
for (u32 i = 0; i < m_springCount; ++i)
for (b3Force* f = m_forceList.m_head; f; f = f->m_next)
{
b3Spring* s = m_springs + i;
b3Particle* p1 = s->p1;
b3Particle* p2 = s->p2;
if (s->type == e_strechSpring)
if (f->m_type == e_springForce)
{
b3Draw_draw->DrawSegment(p1->position, p2->position, b3Color_black);
b3SpringForce* s = (b3SpringForce*)f;
b3Particle* p1 = s->m_p1;
b3Particle* p2 = s->m_p2;
b3Draw_draw->DrawSegment(p1->m_position, p2->m_position, b3Color_black);
}
}
@@ -678,23 +752,23 @@ void b3Cloth::Draw() const
for (u32 i = 0; i < m->sewingLineCount; ++i)
{
b3ClothMeshSewingLine* s = m->sewingLines + i;
b3Particle* p1 = m_particles + s->v1;
b3Particle* p2 = m_particles + s->v2;
b3Particle* p1 = m->particles[s->v1];
b3Particle* p2 = m->particles[s->v2];
b3Draw_draw->DrawSegment(p1->position, p2->position, b3Color_white);
b3Draw_draw->DrawSegment(p1->m_position, p2->m_position, b3Color_white);
}
for (u32 i = 0; i < m->triangleCount; ++i)
{
b3ClothMeshTriangle* t = m->triangles + i;
b3Particle* p1 = m_particles + t->v1;
b3Particle* p2 = m_particles + t->v2;
b3Particle* p3 = m_particles + t->v3;
b3Particle* p1 = m->particles[t->v1];
b3Particle* p2 = m->particles[t->v2];
b3Particle* p3 = m->particles[t->v3];
b3Vec3 v1 = p1->position;
b3Vec3 v2 = p2->position;
b3Vec3 v3 = p3->position;
b3Vec3 v1 = p1->m_position;
b3Vec3 v2 = p2->m_position;
b3Vec3 v3 = p3->m_position;
b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1);
n1.Normalize();

4
src/bounce/dynamics/cloth/cloth_mesh.cpp
View File

@@ -25,6 +25,7 @@ b3GarmentClothMesh::b3GarmentClothMesh()
{
vertexCount = 0;
vertices = nullptr;
particles = nullptr;
triangleCount = 0;
triangles = nullptr;
meshCount = 0;
@@ -36,6 +37,7 @@ b3GarmentClothMesh::b3GarmentClothMesh()
b3GarmentClothMesh::~b3GarmentClothMesh()
{
b3Free(vertices);
b3Free(particles);
b3Free(triangles);
b3Free(meshes);
b3Free(sewingLines);
@@ -49,6 +51,8 @@ void b3GarmentClothMesh::Set(const b3GarmentMesh* garment)
vertexCount += garment->meshes[i].vertexCount;
}
vertices = (b3Vec3*)b3Alloc(vertexCount * sizeof(b3Vec3));
particles = (b3Particle**)b3Alloc(vertexCount * sizeof(b3Particle*));
memset(particles, 0, vertexCount * sizeof(b3Particle*));
B3_ASSERT(triangleCount == 0);
for (u32 i = 0; i < garment->meshCount; ++i)

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

@@ -18,11 +18,35 @@
#include <bounce/dynamics/cloth/cloth_solver.h>
#include <bounce/dynamics/cloth/cloth.h>
#include <bounce/dynamics/shapes/shape.h>
#include <bounce/common/memory/stack_allocator.h>
#include <bounce/dynamics/cloth/particle.h>
#include <bounce/dynamics/cloth/force.h>
#include <bounce/dynamics/cloth/dense_vec3.h>
#include <bounce/dynamics/cloth/diag_mat33.h>
#include <bounce/dynamics/cloth/sparse_mat33.h>
#include <bounce/dynamics/cloth/sparse_sym_mat33.h>
#include <bounce/common/memory/stack_allocator.h>
static B3_FORCE_INLINE void b3Mul(b3DenseVec3& out, const b3SymMat33& A, const b3DenseVec3& v)
{
B3_ASSERT(A.N == v.n);
B3_ASSERT(out.n == A.M);
for (u32 row = 0; row < A.M; ++row)
{
out[row].SetZero();
for (u32 column = 0; column < A.N; ++column)
{
out[row] += A(row, column) * v[column];
}
}
}
static B3_FORCE_INLINE b3DenseVec3 operator*(const b3SymMat33& m, const b3DenseVec3& v)
{
b3DenseVec3 result(v.n);
b3Mul(result, m, v);
return result;
}
// Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm.
// described in the paper:
@@ -41,9 +65,9 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
m_particleCount = 0;
m_particles = (b3Particle**)m_allocator->Allocate(m_particleCapacity * sizeof(b3Particle*));
m_springCapacity = def.springCapacity;
m_springCount = 0;
m_springs = (b3Spring**)m_allocator->Allocate(m_springCapacity * sizeof(b3Spring*));;
m_forceCapacity = def.forceCapacity;
m_forceCount = 0;
m_forces = (b3Force**)m_allocator->Allocate(m_forceCapacity * sizeof(b3Force*));;
m_contactCapacity = def.contactCapacity;
m_contactCount = 0;
@@ -58,13 +82,13 @@ b3ClothSolver::~b3ClothSolver()
{
m_allocator->Free(m_constraints);
m_allocator->Free(m_contacts);
m_allocator->Free(m_springs);
m_allocator->Free(m_forces);
m_allocator->Free(m_particles);
}
void b3ClothSolver::Add(b3Particle* p)
{
p->solverId = m_particleCount;
p->m_solverId = m_particleCount;
m_particles[m_particleCount++] = p;
}
@@ -73,16 +97,24 @@ void b3ClothSolver::Add(b3BodyContact* c)
m_contacts[m_contactCount++] = c;
}
void b3ClothSolver::Add(b3Spring* s)
void b3ClothSolver::Add(b3Force* f)
{
m_springs[m_springCount++] = s;
m_forces[m_forceCount++] = f;
}
void b3ClothSolver::InitializeForces()
{
for (u32 i = 0; i < m_springCount; ++i)
for (u32 i = 0; i < m_forceCount; ++i)
{
m_springs[i]->InitializeForces(&m_solverData);
m_forces[i]->Initialize(&m_solverData);
}
}
void b3ClothSolver::ApplyForces()
{
for (u32 i = 0; i < m_forceCount; ++i)
{
m_forces[i]->Apply(&m_solverData);
}
}
@@ -91,7 +123,7 @@ void b3ClothSolver::InitializeConstraints()
for (u32 i = 0; i < m_particleCount; ++i)
{
b3Particle* p = m_particles[i];
if (p->type != e_dynamicParticle)
if (p->m_type != e_dynamicParticle)
{
b3AccelerationConstraint* ac = m_constraints + m_constraintCount;
++m_constraintCount;
@@ -108,7 +140,7 @@ void b3ClothSolver::InitializeConstraints()
b3AccelerationConstraint* ac = m_constraints + m_constraintCount;
++m_constraintCount;
ac->i1 = p->solverId;
ac->i1 = p->m_solverId;
ac->ndof = 2;
ac->p = pc->n;
ac->z.SetZero();
@@ -134,29 +166,6 @@ void b3ClothSolver::InitializeConstraints()
}
}
struct b3SolverSparseMat33 : public b3SparseMat33
{
b3SolverSparseMat33(b3StackAllocator* a, u32 m, u32 n)
{
allocator = a;
M = m;
N = n;
valueCount = 0;
values = (b3Mat33*)allocator->Allocate(M * N * sizeof(b3Mat33));
cols = (u32*)allocator->Allocate(M * N * sizeof(u32));
row_ptrs = (u32*)allocator->Allocate((M + 1) * sizeof(u32));
}
~b3SolverSparseMat33()
{
allocator->Free(row_ptrs);
allocator->Free(cols);
allocator->Free(values);
}
b3StackAllocator* allocator;
};
void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
{
B3_PROFILE("Integrate");
@@ -167,9 +176,17 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
b3DenseVec3 sy(m_particleCount);
b3DenseVec3 sx0(m_particleCount);
b3SymMat33 dfdx(m_allocator, m_particleCount, m_particleCount);
dfdx.SetZero();
b3SymMat33 dfdv(m_allocator, m_particleCount, m_particleCount);
dfdv.SetZero();
m_solverData.x = sx.v;
m_solverData.v = sv.v;
m_solverData.f = sf.v;
m_solverData.dfdx = &dfdx;
m_solverData.dfdv = &dfdv;
m_solverData.dt = dt;
m_solverData.invdt = 1.0f / dt;
@@ -177,18 +194,18 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
{
b3Particle* p = m_particles[i];
sx[i] = p->position;
sv[i] = p->velocity;
sf[i] = p->force;
sx[i] = p->m_position;
sv[i] = p->m_velocity;
sf[i] = p->m_force;
// Apply weight
if (p->type == e_dynamicParticle)
if (p->m_type == e_dynamicParticle)
{
sf[i] += p->mass * gravity;
sf[i] += p->m_mass * gravity;
}
sy[i] = p->translation;
sx0[i] = p->x;
sy[i] = p->m_translation;
sx0[i] = p->m_x;
}
// Apply contact position correction
@@ -196,19 +213,14 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
{
b3BodyContact* c = m_contacts[i];
b3Particle* p = c->p1;
sy[p->solverId] -= c->s * c->n;
sy[p->m_solverId] -= c->s * c->n;
}
// Initialize forces
// Initialize internal forces
InitializeForces();
// Apply internal forces
for (u32 i = 0; i < m_springCount; ++i)
{
b3Spring* s = m_springs[i];
sf[s->p1->solverId] += s->f;
sf[s->p2->solverId] -= s->f;
}
ApplyForces();
// Initialize constraints
InitializeConstraints();
@@ -223,7 +235,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
// b = h * (f0 + h * dfdx * v0 + dfdx * y)
// A
b3SolverSparseMat33 A(m_allocator, m_particleCount, m_particleCount);
b3SparseSymMat33 A(m_allocator, m_particleCount, m_particleCount);
// b
b3DenseVec3 b(m_particleCount);
@@ -247,14 +259,14 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
// Copy state buffers back to the particles
for (u32 i = 0; i < m_particleCount; ++i)
{
m_particles[i]->position = sx[i];
m_particles[i]->velocity = sv[i];
m_particles[i]->m_position = sx[i];
m_particles[i]->m_velocity = sv[i];
}
// Cache x to improve convergence
for (u32 i = 0; i < m_particleCount; ++i)
{
m_particles[i]->x = x[i];
m_particles[i]->m_x = x[i];
}
// Store the extra contact constraint forces that should have been
@@ -269,7 +281,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
b3BodyContact* c = m_contacts[i];
b3Particle* p = c->p1;
b3Vec3 force = f[p->solverId];
b3Vec3 force = f[p->m_solverId];
// Signed normal force magnitude
c->Fn = b3Dot(force, c->n);
@@ -280,152 +292,45 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
}
}
#define B3_INDEX(i, j, size) (i + j * size)
//
static void b3SetZero(b3Mat33* out, u32 size)
{
for (u32 i = 0; i < size; ++i)
{
out[i].SetZero();
}
}
// dfdx * v
static void b3Mul_Jx(b3DenseVec3& out, b3Spring** springs, u32 springCount, const b3DenseVec3& v)
{
out.SetZero();
for (u32 i = 0; i < springCount; ++i)
{
b3Spring* s = springs[i];
u32 i1 = s->p1->solverId;
u32 i2 = s->p2->solverId;
b3Mat33 J_11 = s->Jx;
b3Mat33 J_12 = -J_11;
b3Mat33 J_21 = J_12;
b3Mat33 J_22 = J_11;
out[i1] += J_11 * v[i1] + J_12 * v[i2];
out[i2] += J_21 * v[i1] + J_22 * v[i2];
}
}
static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A)
{
bool isZeroX = b3Dot(A.x, A.x) == 0.0f;
bool isZeroY = b3Dot(A.y, A.y) == 0.0f;
bool isZeroZ = b3Dot(A.z, A.z) == 0.0f;
return isZeroX * isZeroY * isZeroZ;
}
void b3ClothSolver::Compute_A_b(b3SolverSparseMat33& SA, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const
void b3ClothSolver::Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const
{
float32 h = m_solverData.dt;
// Compute dfdx, dfdv
b3Mat33* dfdx = (b3Mat33*)m_allocator->Allocate(m_particleCount * m_particleCount * sizeof(b3Mat33));
b3SetZero(dfdx, m_particleCount * m_particleCount);
b3Mat33* dfdv = (b3Mat33*)m_allocator->Allocate(m_particleCount * m_particleCount * sizeof(b3Mat33));
b3SetZero(dfdv, m_particleCount * m_particleCount);
for (u32 i = 0; i < m_springCount; ++i)
{
b3Spring* s = m_springs[i];
u32 i1 = s->p1->solverId;
u32 i2 = s->p2->solverId;
b3Mat33 Jx11 = s->Jx;
b3Mat33 Jx12 = -Jx11;
b3Mat33 Jx21 = Jx12;
b3Mat33 Jx22 = Jx11;
dfdx[B3_INDEX(i1, i1, m_particleCount)] += Jx11;
dfdx[B3_INDEX(i1, i2, m_particleCount)] += Jx12;
dfdx[B3_INDEX(i2, i1, m_particleCount)] += Jx21;
dfdx[B3_INDEX(i2, i2, m_particleCount)] += Jx22;
b3Mat33 Jv11 = s->Jv;
b3Mat33 Jv12 = -Jv11;
b3Mat33 Jv21 = Jv12;
b3Mat33 Jv22 = Jv11;
dfdv[B3_INDEX(i1, i1, m_particleCount)] += Jv11;
dfdv[B3_INDEX(i1, i2, m_particleCount)] += Jv12;
dfdv[B3_INDEX(i2, i1, m_particleCount)] += Jv21;
dfdv[B3_INDEX(i2, i2, m_particleCount)] += Jv22;
}
b3SymMat33& dfdx = *m_solverData.dfdx;
b3SymMat33& dfdv = *m_solverData.dfdv;
// Compute A
// A = M - h * dfdv - h * h * dfdx
// A = 0
b3Mat33* A = (b3Mat33*)m_allocator->Allocate(m_particleCount * m_particleCount * sizeof(b3Mat33));
b3SetZero(A, m_particleCount * m_particleCount);
// Set the upper triangle zero
for (u32 i = 0; i < m_particleCount; ++i)
{
for (u32 j = i; j < m_particleCount; ++j)
{
A(i, j).SetZero();
}
}
// A += M
for (u32 i = 0; i < m_particleCount; ++i)
{
A[B3_INDEX(i, i, m_particleCount)] += b3Diagonal(m_particles[i]->mass);
A(i, i) += b3Diagonal(m_particles[i]->m_mass);
}
// A += - h * dfdv - h * h * dfdx
// Set the upper triangle
for (u32 i = 0; i < m_particleCount; ++i)
{
for (u32 j = 0; j < m_particleCount; ++j)
for (u32 j = i; j < m_particleCount; ++j)
{
A[B3_INDEX(i, j, m_particleCount)] += (-h * dfdv[B3_INDEX(i, j, m_particleCount)]) + (-h * h * dfdx[B3_INDEX(i, j, m_particleCount)]);
A(i, j) += (-h * dfdv(i, j)) + (-h * h * dfdx(i, j));
}
}
// Assembly sparsity.
u32 valueCapacity = m_particleCapacity * m_particleCapacity;
SA.row_ptrs[0] = 0;
for (u32 i = 0; i < m_particleCount; ++i)
{
u32 rowValueCount = 0;
for (u32 j = 0; j < m_particleCount; ++j)
{
b3Mat33 a = A[B3_INDEX(i, j, m_particleCount)];
if (b3IsZero(a) == false)
{
SA.values[SA.valueCount] = a;
SA.cols[SA.valueCount] = j;
++SA.valueCount;
++rowValueCount;
}
}
SA.row_ptrs[i + 1] = SA.row_ptrs[(i + 1) - 1] + rowValueCount;
}
B3_ASSERT(SA.valueCount <= valueCapacity);
m_allocator->Free(A);
m_allocator->Free(dfdv);
m_allocator->Free(dfdx);
// Compute b
// Jx_v = dfdx * v
b3DenseVec3 Jx_v(m_particleCount);
b3Mul_Jx(Jx_v, m_springs, m_springCount, v);
// Jx_y = dfdx * y
b3DenseVec3 Jx_y(m_particleCount);
b3Mul_Jx(Jx_y, m_springs, m_springCount, y);
// b = h * (f0 + h * Jx_v + Jx_y)
b = h * (f + h * Jx_v + Jx_y);
// b = h * (f0 + h * dfdx * v + dfdx * y)
b = h * (f + h * (dfdx * v) + dfdx * y);
}
void b3ClothSolver::Compute_S_z(b3DiagMat33& S, b3DenseVec3& z)
@@ -466,13 +371,13 @@ void b3ClothSolver::Compute_S_z(b3DiagMat33& S, b3DenseVec3& z)
}
void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const
const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const
{
B3_PROFILE("Solve Ax = b");
// P = diag(A)
b3DiagMat33 inv_P(m_particleCount);
A.AssembleDiagonal(inv_P);
A.Diagonal(inv_P);
// Invert
for (u32 i = 0; i < m_particleCount; ++i)
@@ -526,8 +431,14 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
u32 iteration = 0;
// Main iteration loop.
while (iteration < max_iterations)
for (;;)
{
// Divergence check.
if (iteration >= max_iterations)
{
break;
}
B3_ASSERT(b3IsValid(delta_new));
// Convergence check.

62
src/bounce/dynamics/cloth/force.cpp Normal file
View File

@@ -0,0 +1,62 @@
/*
* 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.
*/
#include <bounce/dynamics/cloth/force.h>
#include <bounce/dynamics/cloth/spring_force.h>
b3Force* b3Force::Create(const b3ForceDef* def)
{
b3SpringForce* force = NULL;
switch (def->type)
{
case e_springForce:
{
void* block = b3Alloc(sizeof(b3SpringForce));
force = new (block) b3SpringForce((b3SpringForceDef*)def);
break;
}
default:
{
B3_ASSERT(false);
break;
}
}
return force;
}
void b3Force::Destroy(b3Force* force)
{
B3_ASSERT(force);
b3ForceType type = force->GetType();
switch (type)
{
case e_springForce:
{
b3SpringForce* o = (b3SpringForce*)force;
o->~b3SpringForce();
b3Free(force);
break;
}
default:
{
B3_ASSERT(false);
break;
}
};
}

69
src/bounce/dynamics/cloth/particle.cpp Normal file
View File

@@ -0,0 +1,69 @@
/*
* 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.
*/
#include <bounce/dynamics/cloth/particle.h>
#include <bounce/dynamics/cloth/cloth.h>
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;
m_translation.SetZero();
m_mass = 0.0f;
m_invMass = 0.0f;
m_radius = def.radius;
m_userData = nullptr;
m_x.SetZero();
m_vertex = ~0;
m_contact.n_active = false;
m_contact.t1_active = false;
m_contact.t2_active = false;
m_contact.Fn = 0.0f;
m_contact.Ft1 = 0.0f;
m_contact.Ft2 = 0.0f;
}
b3Particle::~b3Particle()
{
}
void b3Particle::SetType(b3ParticleType type)
{
if (m_type == type)
{
return;
}
m_type = type;
m_force.SetZero();
if (type == e_staticParticle)
{
m_velocity.SetZero();
m_translation.SetZero();
m_contact.n_active = false;
m_contact.t1_active = false;
m_contact.t2_active = false;
}
}

121
src/bounce/dynamics/cloth/spring_force.cpp Normal file
View File

@@ -0,0 +1,121 @@
/*
* 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.
*/
#include <bounce/dynamics/cloth/spring_force.h>
#include <bounce/dynamics/cloth/particle.h>
#include <bounce/dynamics/cloth/cloth_solver.h>
void b3SpringForceDef::Initialize(b3Particle* particle1, b3Particle* particle2, float32 structuralStiffness, float32 dampingStiffness)
{
type = e_springForce;
p1 = particle1;
p2 = particle2;
b3Vec3 x1 = p1->GetPosition();
b3Vec3 x2 = p2->GetPosition();
restLength = b3Distance(x1, x2);
structural = structuralStiffness;
damping = dampingStiffness;
}
b3SpringForce::b3SpringForce(const b3SpringForceDef* def)
{
m_type = e_springForce;
m_p1 = def->p1;
m_p2 = def->p2;
m_L0 = def->restLength;
m_ks = def->structural;
m_kd = def->damping;
}
b3SpringForce::~b3SpringForce()
{
}
void b3SpringForce::Initialize(const b3ClothSolverData* data)
{
u32 i1 = m_p1->m_solverId;
u32 i2 = m_p2->m_solverId;
b3Vec3 x1 = data->x[i1];
b3Vec3 v1 = data->v[i1];
b3Vec3 x2 = data->x[i2];
b3Vec3 v2 = data->v[i2];
b3Mat33 I; I.SetIdentity();
b3Vec3 dx = x1 - x2;
float32 L = b3Length(dx);
if (L >= m_L0)
{
b3Vec3 n = dx / L;
// Tension
m_f = -m_ks * (L - m_L0) * n;
// Jacobian
m_Jx = -m_ks * (b3Outer(dx, dx) + (1.0f - m_L0 / L) * (I - b3Outer(dx, dx)));
}
else
{
m_f.SetZero();
m_Jx.SetZero();
}
// Damping
b3Vec3 dv = v1 - v2;
m_f += -m_kd * dv;
m_Jv = -m_kd * I;
}
void b3SpringForce::Apply(const b3ClothSolverData* data)
{
b3Vec3* f = data->f;
b3SymMat33& dfdx = *data->dfdx;
b3SymMat33& dfdv = *data->dfdv;
f[m_p1->m_solverId] += m_f;
f[m_p2->m_solverId] -= m_f;
u32 i1 = m_p1->m_solverId;
u32 i2 = m_p2->m_solverId;
b3Mat33 Jx11 = m_Jx;
b3Mat33 Jx12 = -Jx11;
b3Mat33 Jx21 = Jx12;
b3Mat33 Jx22 = Jx11;
dfdx(i1, i1) += Jx11;
dfdx(i1, i2) += Jx12;
dfdx(i2, i1) += Jx21;
dfdx(i2, i2) += Jx22;
b3Mat33 Jv11 = m_Jv;
b3Mat33 Jv12 = -Jv11;
b3Mat33 Jv21 = Jv12;
b3Mat33 Jv22 = Jv11;
dfdv(i1, i1) += Jv11;
dfdv(i1, i2) += Jv12;
dfdv(i2, i1) += Jv21;
dfdv(i2, i2) += Jv22;
}