refactor cloth
This commit is contained in:
Irlan
@@ -17,137 +17,116 @@
|
||||
*/
|
||||
|
||||
#include <bounce/dynamics/cloth/cloth.h>
|
||||
#include <bounce/collision/shapes/mesh.h>
|
||||
#include <bounce/common/template/array.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/common/memory/stack_allocator.h>
|
||||
#include <bounce/common/draw.h>
|
||||
|
||||
#define B3_FORCE_THRESHOLD 0.005f
|
||||
|
||||
#define B3_CLOTH_BENDING 0
|
||||
|
||||
#define B3_CLOTH_FRICTION 0
|
||||
|
||||
b3Cloth::b3Cloth()
|
||||
{
|
||||
m_pCount = 0;
|
||||
m_ps = NULL;
|
||||
m_c1Count = 0;
|
||||
m_c1s = NULL;
|
||||
m_c2Count = 0;
|
||||
m_c2s = NULL;
|
||||
m_gravity.SetZero();
|
||||
|
||||
m_particleCount = 0;
|
||||
m_particles = nullptr;
|
||||
|
||||
m_springs = nullptr;
|
||||
m_springCount = 0;
|
||||
|
||||
m_contacts = nullptr;
|
||||
//m_contactCount = 0;
|
||||
|
||||
m_shapeCount = 0;
|
||||
|
||||
m_mesh = nullptr;
|
||||
|
||||
m_k1 = 0.0f;
|
||||
m_k2 = 0.0f;
|
||||
m_kd = 0.0f;
|
||||
m_r = 0.0f;
|
||||
m_gravity.SetZero();
|
||||
}
|
||||
|
||||
b3Cloth::~b3Cloth()
|
||||
{
|
||||
b3Free(m_ps);
|
||||
b3Free(m_c1s);
|
||||
b3Free(m_c2s);
|
||||
b3Free(m_particles);
|
||||
b3Free(m_springs);
|
||||
b3Free(m_contacts);
|
||||
}
|
||||
|
||||
void b3Cloth::Initialize(const b3ClothDef& def)
|
||||
static B3_FORCE_INLINE u32 b3NextIndex(u32 i)
|
||||
{
|
||||
B3_ASSERT(def.mesh);
|
||||
m_mesh = def.mesh;
|
||||
|
||||
const b3Mesh* m = m_mesh;
|
||||
return i + 1 < 3 ? i + 1 : 0;
|
||||
}
|
||||
|
||||
m_pCount = m->vertexCount;
|
||||
m_ps = (b3Particle*)b3Alloc(m_pCount * sizeof(b3Particle));
|
||||
|
||||
for (u32 i = 0; i < m->vertexCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_ps + i;
|
||||
p->im = 0.0f;
|
||||
p->p = m->vertices[i];
|
||||
p->p0 = p->p;
|
||||
p->v.SetZero();
|
||||
}
|
||||
struct b3UniqueEdge
|
||||
{
|
||||
u32 v1, v2;
|
||||
};
|
||||
|
||||
m_c1s = (b3C1*)b3Alloc(3 * m->triangleCount * sizeof(b3C1));
|
||||
m_c1Count = 0;
|
||||
static u32 b3FindUniqueEdges(b3UniqueEdge* uniqueEdges, const b3ClothMesh* m)
|
||||
{
|
||||
u32 uniqueCount = 0;
|
||||
|
||||
for (u32 i = 0; i < m->triangleCount; ++i)
|
||||
{
|
||||
b3Triangle* t = m->triangles + i;
|
||||
|
||||
u32 is[3] = { t->v1, t->v2, t->v3 };
|
||||
for (u32 j = 0; j < 3; ++j)
|
||||
{
|
||||
u32 k = j + 1 < 3 ? j + 1 : 0;
|
||||
|
||||
u32 v1 = is[j];
|
||||
u32 v2 = is[k];
|
||||
|
||||
b3Vec3 p1 = m->vertices[v1];
|
||||
b3Vec3 p2 = m->vertices[v2];
|
||||
|
||||
b3C1* C = m_c1s + m_c1Count;
|
||||
C->i1 = v1;
|
||||
C->i2 = v2;
|
||||
C->L = b3Distance(p1, p2);
|
||||
++m_c1Count;
|
||||
}
|
||||
|
||||
b3Vec3 p1 = m->vertices[t->v1];
|
||||
b3Vec3 p2 = m->vertices[t->v2];
|
||||
b3Vec3 p3 = m->vertices[t->v3];
|
||||
|
||||
float32 area = b3Area(p1, p2, p3);
|
||||
float32 mass = def.density * area;
|
||||
|
||||
const float32 inv3 = 1.0f / 3.0f;
|
||||
|
||||
m_ps[t->v1].im += inv3 * mass;
|
||||
m_ps[t->v2].im += inv3 * mass;
|
||||
m_ps[t->v3].im += inv3 * mass;
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_pCount; ++i)
|
||||
{
|
||||
m_ps[i].im = m_ps[i].im > 0.0f ? 1.0f / m_ps[i].im : 0.0f;
|
||||
}
|
||||
|
||||
u32 c2Capacity = 0;
|
||||
|
||||
for (u32 i = 0; i < m->triangleCount; ++i)
|
||||
{
|
||||
b3Triangle* t1 = m->triangles + i;
|
||||
b3ClothMeshTriangle* t1 = m->triangles + i;
|
||||
u32 i1s[3] = { t1->v1, t1->v2, t1->v3 };
|
||||
|
||||
for (u32 j1 = 0; j1 < 3; ++j1)
|
||||
{
|
||||
u32 k1 = j1 + 1 < 3 ? j1 + 1 : 0;
|
||||
|
||||
u32 t1v1 = i1s[j1];
|
||||
u32 t1v2 = i1s[k1];
|
||||
u32 t1v2 = i1s[b3NextIndex(j1)];
|
||||
|
||||
for (u32 j = i + 1; j < m->triangleCount; ++j)
|
||||
bool unique = true;
|
||||
|
||||
for (u32 j = 0; j < uniqueCount; ++j)
|
||||
{
|
||||
b3Triangle* t2 = m->triangles + j;
|
||||
u32 i2s[3] = { t2->v1, t2->v2, t2->v3 };
|
||||
b3UniqueEdge* ue = uniqueEdges + j;
|
||||
|
||||
for (u32 j2 = 0; j2 < 3; ++j2)
|
||||
if (ue->v1 == t1v1 && ue->v2 == t1v2)
|
||||
{
|
||||
u32 k2 = j2 + 1 < 3 ? j2 + 1 : 0;
|
||||
|
||||
u32 t2v1 = i2s[j2];
|
||||
u32 t2v2 = i2s[k2];
|
||||
|
||||
if (t1v1 == t2v2 && t1v2 == t2v1)
|
||||
{
|
||||
++c2Capacity;
|
||||
}
|
||||
unique = false;
|
||||
break;
|
||||
}
|
||||
|
||||
if (ue->v2 == t1v1 && ue->v1 == t1v2)
|
||||
{
|
||||
unique = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (unique)
|
||||
{
|
||||
b3UniqueEdge ue;
|
||||
ue.v1 = t1v1;
|
||||
ue.v2 = t1v2;
|
||||
uniqueEdges[uniqueCount++] = ue;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
m_c2Count = 0;
|
||||
m_c2s = (b3C2*)b3Alloc(c2Capacity * sizeof(b3C2));
|
||||
return uniqueCount;
|
||||
}
|
||||
|
||||
struct b3SharedEdge
|
||||
{
|
||||
u32 v1, v2;
|
||||
u32 nsv1, nsv2;
|
||||
};
|
||||
|
||||
static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m)
|
||||
{
|
||||
u32 sharedCount = 0;
|
||||
|
||||
for (u32 i = 0; i < m->triangleCount; ++i)
|
||||
{
|
||||
b3Triangle* t1 = m->triangles + i;
|
||||
b3ClothMeshTriangle* t1 = m->triangles + i;
|
||||
u32 i1s[3] = { t1->v1, t1->v2, t1->v3 };
|
||||
|
||||
for (u32 j1 = 0; j1 < 3; ++j1)
|
||||
@@ -159,7 +138,7 @@ void b3Cloth::Initialize(const b3ClothDef& def)
|
||||
|
||||
for (u32 j = i + 1; j < m->triangleCount; ++j)
|
||||
{
|
||||
b3Triangle* t2 = m->triangles + j;
|
||||
b3ClothMeshTriangle* t2 = m->triangles + j;
|
||||
u32 i2s[3] = { t2->v1, t2->v2, t2->v3 };
|
||||
|
||||
for (u32 j2 = 0; j2 < 3; ++j2)
|
||||
@@ -171,36 +150,21 @@ void b3Cloth::Initialize(const b3ClothDef& def)
|
||||
|
||||
if (t1v1 == t2v2 && t1v2 == t2v1)
|
||||
{
|
||||
// The triangles are adjacent.
|
||||
u32 k3 = k1 + 1 < 3 ? k1 + 1 : 0;
|
||||
u32 t1v3 = i1s[k3];
|
||||
|
||||
u32 k4 = k2 + 1 < 3 ? k2 + 1 : 0;
|
||||
u32 t2v3 = i2s[k4];
|
||||
|
||||
b3Vec3 p1 = m->vertices[t1v1];
|
||||
b3Vec3 p2 = m->vertices[t1v2];
|
||||
b3Vec3 p3 = m->vertices[t1v3];
|
||||
b3Vec3 p4 = m->vertices[t2v3];
|
||||
// Add shared edge and non-shared vertices.
|
||||
b3SharedEdge se;
|
||||
se.v1 = t1v1;
|
||||
se.v2 = t1v2;
|
||||
se.nsv1 = t1v3;
|
||||
se.nsv2 = t2v3;
|
||||
|
||||
b3Vec3 n1 = b3Cross(p2 - p1, p3 - p1);
|
||||
n1.Normalize();
|
||||
|
||||
b3Vec3 n2 = b3Cross(p2 - p1, p4 - p1);
|
||||
n2.Normalize();
|
||||
|
||||
float32 x = b3Dot(n1, n2);
|
||||
|
||||
b3Vec3 n3 = b3Cross(n1, n2);
|
||||
float32 y = b3Length(n3);
|
||||
|
||||
b3C2* c = m_c2s + m_c2Count;
|
||||
c->i1 = t1v1;
|
||||
c->i2 = t1v2;
|
||||
c->i3 = t1v3;
|
||||
c->i4 = t2v3;
|
||||
c->angle = atan2(y, x);
|
||||
|
||||
++m_c2Count;
|
||||
sharedEdges[sharedCount++] = se;
|
||||
|
||||
break;
|
||||
}
|
||||
@@ -209,188 +173,525 @@ void b3Cloth::Initialize(const b3ClothDef& def)
|
||||
}
|
||||
}
|
||||
|
||||
m_k1 = def.k1;
|
||||
m_k2 = def.k2;
|
||||
m_kd = def.kd;
|
||||
m_r = def.r;
|
||||
m_gravity = def.gravity;
|
||||
return sharedCount;
|
||||
}
|
||||
|
||||
void b3Cloth::Step(float32 h, u32 iterations)
|
||||
void b3Cloth::Initialize(const b3ClothDef& def)
|
||||
{
|
||||
if (h == 0.0f)
|
||||
B3_ASSERT(def.mesh);
|
||||
B3_ASSERT(def.density > 0.0f);
|
||||
|
||||
m_mesh = def.mesh;
|
||||
m_density = def.density;
|
||||
|
||||
const b3ClothMesh* m = m_mesh;
|
||||
|
||||
m_particleCount = m->vertexCount;
|
||||
m_particles = (b3Particle*)b3Alloc(m_particleCount * sizeof(b3Particle));
|
||||
m_contacts = (b3ParticleContact*)b3Alloc(m_particleCount * sizeof(b3ParticleContact));
|
||||
|
||||
for (u32 i = 0; i < m_particleCount; ++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;
|
||||
|
||||
p->translation.SetZero();
|
||||
p->x.SetZero();
|
||||
p->tension.SetZero();
|
||||
|
||||
b3ParticleContact* c = m_contacts + i;
|
||||
c->n_active = false;
|
||||
c->t1_active = false;
|
||||
c->t2_active = false;
|
||||
}
|
||||
|
||||
// Compute mass
|
||||
ResetMass();
|
||||
|
||||
// Initialize springs
|
||||
u32 edgeCount = 3 * m->triangleCount;
|
||||
|
||||
b3UniqueEdge* uniqueEdges = (b3UniqueEdge*)m_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));
|
||||
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;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
#if B3_CLOTH_BENDING
|
||||
|
||||
// Bending
|
||||
for (u32 i = 0; i < sharedCount; ++i)
|
||||
{
|
||||
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;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
m_allocator.Free(sharedEdges);
|
||||
#endif
|
||||
|
||||
m_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;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
B3_ASSERT(m_springCount <= springCapacity);
|
||||
}
|
||||
|
||||
void b3Cloth::ResetMass()
|
||||
{
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
m_particles[i].mass = 0.0f;
|
||||
m_particles[i].invMass = 0.0f;
|
||||
}
|
||||
|
||||
const float32 inv3 = 1.0f / 3.0f;
|
||||
const float32 rho = m_density;
|
||||
|
||||
// Accumulate the mass about the body 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];
|
||||
|
||||
float32 area = b3Area(p1, p2, p3);
|
||||
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;
|
||||
}
|
||||
|
||||
// Invert
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
B3_ASSERT(m_particles[i].mass > 0.0f);
|
||||
m_particles[i].invMass = 1.0f / m_particles[i].mass;
|
||||
}
|
||||
}
|
||||
|
||||
void b3Cloth::AddShape(b3Shape* shape)
|
||||
{
|
||||
B3_ASSERT(m_shapeCount < B3_CLOTH_SHAPE_CAPACITY);
|
||||
|
||||
if (m_shapeCount == B3_CLOTH_SHAPE_CAPACITY)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
float32 d = exp(-h * m_kd);
|
||||
|
||||
for (u32 i = 0; i < m_pCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_ps + i;
|
||||
|
||||
p->v += h * p->im * m_gravity;
|
||||
p->v *= d;
|
||||
|
||||
p->p0 = p->p;
|
||||
p->p += h * p->v;
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < iterations; ++i)
|
||||
{
|
||||
SolveC2();
|
||||
SolveC1();
|
||||
}
|
||||
|
||||
float32 inv_h = 1.0f / h;
|
||||
for (u32 i = 0; i < m_pCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_ps + i;
|
||||
p->v = inv_h * (p->p - p->p0);
|
||||
}
|
||||
m_shapes[m_shapeCount++] = shape;
|
||||
}
|
||||
|
||||
void b3Cloth::SolveC1()
|
||||
void b3Cloth::UpdateContacts()
|
||||
{
|
||||
for (u32 i = 0; i < m_c1Count; ++i)
|
||||
B3_PROFILE("Update Contacts");
|
||||
|
||||
// Clear active flags
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b3C1* c = m_c1s + i;
|
||||
|
||||
b3Particle* p1 = m_ps + c->i1;
|
||||
b3Particle* p2 = m_ps + c->i2;
|
||||
m_contacts[i].n_active = false;
|
||||
m_contacts[i].t1_active = false;
|
||||
m_contacts[i].t2_active = false;
|
||||
}
|
||||
|
||||
float32 m1 = p1->im;
|
||||
float32 m2 = p2->im;
|
||||
// Create contacts
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_particles + i;
|
||||
|
||||
float32 mass = m1 + m2;
|
||||
if (mass == 0.0f)
|
||||
// Static particles can't participate in collisions.
|
||||
if (p->type == e_staticParticle)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
mass = 1.0f / mass;
|
||||
b3ParticleContact* c = m_contacts + i;
|
||||
|
||||
b3Vec3 J2 = p2->p - p1->p;
|
||||
float32 L = b3Length(J2);
|
||||
if (L > B3_EPSILON)
|
||||
// Save the old contact
|
||||
b3ParticleContact c0 = *c;
|
||||
|
||||
b3Sphere s1;
|
||||
s1.vertex = p->position;
|
||||
s1.radius = p->radius;
|
||||
|
||||
// Find the deepest penetration
|
||||
float32 bestSeparation = 0.0f;
|
||||
b3Vec3 bestNormal(0.0f, 0.0f, 0.0f);
|
||||
u32 bestIndex = ~0;
|
||||
|
||||
for (u32 j = 0; j < m_shapeCount; ++j)
|
||||
{
|
||||
J2 /= L;
|
||||
b3Shape* s2 = m_shapes[j];
|
||||
|
||||
b3Transform xf2;
|
||||
xf2.SetIdentity();
|
||||
|
||||
b3TestSphereOutput output;
|
||||
if (s2->TestSphere(&output, s1, xf2) == false)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (output.separation < bestSeparation)
|
||||
{
|
||||
bestSeparation = output.separation;
|
||||
bestNormal = output.normal;
|
||||
bestIndex = j;
|
||||
}
|
||||
}
|
||||
|
||||
b3Vec3 J1 = -J2;
|
||||
|
||||
float32 C = L - c->L;
|
||||
float32 impulse = -m_k1 * mass * C;
|
||||
|
||||
p1->p += (m1 * impulse) * J1;
|
||||
p2->p += (m2 * impulse) * J2;
|
||||
}
|
||||
}
|
||||
|
||||
void b3Cloth::SolveC2()
|
||||
{
|
||||
for (u32 i = 0; i < m_c2Count; ++i)
|
||||
{
|
||||
b3C2* c = m_c2s + i;
|
||||
|
||||
b3Particle* p1 = m_ps + c->i1;
|
||||
b3Particle* p2 = m_ps + c->i2;
|
||||
b3Particle* p3 = m_ps + c->i3;
|
||||
b3Particle* p4 = m_ps + c->i4;
|
||||
|
||||
float32 m1 = p1->im;
|
||||
float32 m2 = p2->im;
|
||||
float32 m3 = p3->im;
|
||||
float32 m4 = p4->im;
|
||||
|
||||
b3Vec3 v2 = p2->p - p1->p;
|
||||
b3Vec3 v3 = p3->p - p1->p;
|
||||
b3Vec3 v4 = p4->p - p1->p;
|
||||
|
||||
b3Vec3 n1 = b3Cross(v2, v3);
|
||||
n1.Normalize();
|
||||
|
||||
b3Vec3 n2 = b3Cross(v2, v4);
|
||||
n2.Normalize();
|
||||
|
||||
float32 x = b3Dot(n1, n2);
|
||||
|
||||
b3Vec3 J3 = b3Cross(v2, n2) + x * b3Cross(n1, v2);
|
||||
float32 L3 = b3Length(b3Cross(v2, v3));
|
||||
if (L3 > B3_EPSILON)
|
||||
if (bestIndex != ~0)
|
||||
{
|
||||
J3 /= L3;
|
||||
}
|
||||
|
||||
b3Vec3 J4 = b3Cross(v2, n1) + x * b3Cross(n2, v2);
|
||||
float32 L4 = b3Length(b3Cross(v2, v4));
|
||||
if (L4 > B3_EPSILON)
|
||||
{
|
||||
J4 /= L4;
|
||||
B3_ASSERT(bestSeparation <= 0.0f);
|
||||
|
||||
b3Shape* shape = m_shapes[bestIndex];
|
||||
float32 s = bestSeparation;
|
||||
b3Vec3 n = bestNormal;
|
||||
|
||||
// Update contact manifold
|
||||
// Remember the normal orientation is from shape 2 to shape 1 (mass)
|
||||
c->n_active = true;
|
||||
c->p1 = p;
|
||||
c->s2 = shape;
|
||||
c->n = n;
|
||||
c->t1 = b3Perp(n);
|
||||
c->t2 = b3Cross(c->t1, n);
|
||||
|
||||
// Apply position correction
|
||||
p->translation -= s * n;
|
||||
}
|
||||
|
||||
b3Vec3 J2_1 = b3Cross(v3, n2) + x * b3Cross(n1, v3);
|
||||
if (L3 > B3_EPSILON)
|
||||
// Update contact state
|
||||
if (c0.n_active == true && c->n_active == true)
|
||||
{
|
||||
J2_1 /= L3;
|
||||
// The contact persists
|
||||
|
||||
// Has the contact constraint been satisfied?
|
||||
if (c0.Fn <= -B3_FORCE_THRESHOLD)
|
||||
{
|
||||
// Contact force is attractive.
|
||||
|
||||
// Terminate the contact.
|
||||
c->n_active = false;
|
||||
}
|
||||
}
|
||||
|
||||
b3Vec3 J2_2 = b3Cross(v4, n1) + x * b3Cross(n2, v4);
|
||||
if (L4 > B3_EPSILON)
|
||||
#if 0
|
||||
// Notify the new contact state
|
||||
if (c0.n_active == false && c->n_active == true)
|
||||
{
|
||||
J2_2 /= L4;
|
||||
// The contact has begun
|
||||
}
|
||||
|
||||
b3Vec3 J2 = -J2_1 - J2_2;
|
||||
|
||||
b3Vec3 J1 = -J2 - J3 - J4;
|
||||
|
||||
float32 mass = m1 * b3Dot(J1, J1) + m2 * b3Dot(J2, J2) + m3 * b3Dot(J3, J3) + m4 * b3Dot(J4, J4);
|
||||
if (mass == 0.0f)
|
||||
if (c0.n_active == true && c->active_n == false)
|
||||
{
|
||||
// The contact has ended
|
||||
}
|
||||
#endif
|
||||
if (c->n_active == false)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
mass = 1.0f / mass;
|
||||
|
||||
b3Vec3 n3 = b3Cross(n1, n2);
|
||||
float32 y = b3Length(n3);
|
||||
#if B3_CLOTH_FRICTION == 1
|
||||
|
||||
float32 angle = atan2(y, x);
|
||||
float32 C = angle - c->angle;
|
||||
// A friction force requires an associated normal force.
|
||||
if (c0.n_active == false)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
float32 impulse = -m_k2 * mass * y * C;
|
||||
b3Shape* s = c->s;
|
||||
b3Vec3 n = c->n;
|
||||
float32 u = s->GetFriction();
|
||||
float32 normalForce = c0.Fn;
|
||||
|
||||
p1->p += (m1 * impulse) * J1;
|
||||
p2->p += (m2 * impulse) * J2;
|
||||
p3->p += (m3 * impulse) * J3;
|
||||
p4->p += (m4 * impulse) * J4;
|
||||
// Relative velocity
|
||||
b3Vec3 dv = p->velocity;
|
||||
|
||||
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;
|
||||
}
|
||||
else
|
||||
{
|
||||
c->t1_active = true;
|
||||
c->t2_active = true;
|
||||
continue;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
if (t_active0[k] == true && t_active[k] == true)
|
||||
{
|
||||
// The contact persists
|
||||
float32 maxForce = u * normalForce;
|
||||
|
||||
if (Ft0[k] * Ft0[k] > maxForce * maxForce)
|
||||
{
|
||||
// Unlock particle off surface
|
||||
t_active[k] = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
c->t1_active = t_active[0];
|
||||
c->t2_active = t_active[1];
|
||||
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void b3Cloth::Solve(float32 dt)
|
||||
{
|
||||
B3_PROFILE("Solve");
|
||||
|
||||
// Clear tension
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
m_particles[i].tension.SetZero();
|
||||
}
|
||||
|
||||
// Solve
|
||||
b3ClothSolverDef solverDef;
|
||||
solverDef.stack = &m_allocator;
|
||||
solverDef.particleCapacity = m_particleCount;
|
||||
solverDef.springCapacity = m_springCount;
|
||||
solverDef.contactCapacity = m_particleCount;
|
||||
|
||||
b3ClothSolver solver(solverDef);
|
||||
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
solver.Add(m_particles + i);
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_springCount; ++i)
|
||||
{
|
||||
solver.Add(m_springs + i);
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
if (m_contacts[i].n_active)
|
||||
{
|
||||
solver.Add(m_contacts + i);
|
||||
}
|
||||
}
|
||||
|
||||
// Solve
|
||||
solver.Solve(dt, m_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)
|
||||
{
|
||||
m_particles[i].translation.SetZero();
|
||||
}
|
||||
}
|
||||
|
||||
void b3Cloth::Step(float32 dt)
|
||||
{
|
||||
B3_PROFILE("Step");
|
||||
|
||||
// Update contacts
|
||||
UpdateContacts();
|
||||
|
||||
// Solve constraints, integrate state, clear forces and translations.
|
||||
if (dt > 0.0f)
|
||||
{
|
||||
Solve(dt);
|
||||
}
|
||||
}
|
||||
|
||||
void b3Cloth::Apply() const
|
||||
{
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
m_mesh->vertices[i] = m_particles[i].position;
|
||||
}
|
||||
}
|
||||
|
||||
void b3Cloth::Draw() const
|
||||
{
|
||||
const b3Mesh* m = m_mesh;
|
||||
|
||||
const b3ClothMesh* m = m_mesh;
|
||||
|
||||
for (u32 i = 0; i < m->vertexCount; ++i)
|
||||
{
|
||||
b3Draw_draw->DrawPoint(m_ps[i].p, 6.0f, b3Color_green);
|
||||
b3Particle* p = m_particles + i;
|
||||
|
||||
if (p->type == e_staticParticle)
|
||||
{
|
||||
b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_white);
|
||||
}
|
||||
|
||||
if (p->type == e_kinematicParticle)
|
||||
{
|
||||
b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_blue);
|
||||
}
|
||||
|
||||
if (p->type == e_dynamicParticle)
|
||||
{
|
||||
b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_green);
|
||||
}
|
||||
|
||||
b3ParticleContact* c = m_contacts + i;
|
||||
|
||||
if (c->n_active)
|
||||
{
|
||||
b3Draw_draw->DrawSegment(p->position, p->position + c->n, b3Color_yellow);
|
||||
}
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_springCount; ++i)
|
||||
{
|
||||
b3Spring* s = m_springs + i;
|
||||
b3Particle* p1 = s->p1;
|
||||
b3Particle* p2 = s->p2;
|
||||
|
||||
if (s->type == e_strechSpring)
|
||||
{
|
||||
b3Draw_draw->DrawSegment(p1->position, p2->position, b3Color_black);
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
b3Draw_draw->DrawSegment(p1->position, p2->position, b3Color_white);
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m->triangleCount; ++i)
|
||||
{
|
||||
b3Triangle* t = m->triangles + i;
|
||||
b3ClothMeshTriangle* t = m->triangles + i;
|
||||
|
||||
b3Particle* p1 = m_ps + t->v1;
|
||||
b3Particle* p2 = m_ps + t->v2;
|
||||
b3Particle* p3 = m_ps + t->v3;
|
||||
b3Particle* p1 = m_particles + t->v1;
|
||||
b3Particle* p2 = m_particles + t->v2;
|
||||
b3Particle* p3 = m_particles + t->v3;
|
||||
|
||||
b3Vec3 v1 = p1->p;
|
||||
b3Vec3 v2 = p2->p;
|
||||
b3Vec3 v3 = p3->p;
|
||||
b3Vec3 v1 = p1->position;
|
||||
b3Vec3 v2 = p2->position;
|
||||
b3Vec3 v3 = p3->position;
|
||||
|
||||
b3Draw_draw->DrawTriangle(v1, v2, v3, b3Color_black);
|
||||
|
||||
b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1);
|
||||
n1.Normalize();
|
||||
b3Draw_draw->DrawSolidTriangle(n1, v1, v2, v3, b3Color_blue);
|
||||
|
||||
656
src/bounce/dynamics/cloth/cloth_solver.cpp
Normal file
656
src/bounce/dynamics/cloth/cloth_solver.cpp
Normal file
@@ -0,0 +1,656 @@
|
||||
/*
|
||||
* 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/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/dense_vec3.h>
|
||||
#include <bounce/dynamics/cloth/diag_mat33.h>
|
||||
#include <bounce/dynamics/cloth/sparse_mat33.h>
|
||||
|
||||
// Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm.
|
||||
// described in the paper:
|
||||
// "Large Steps in Cloth Simulation - David Baraff, Andrew Witkin".
|
||||
|
||||
// Some improvements for the original MPCG algorithm are described in the paper:
|
||||
// "On the modified conjugate gradient method in cloth simulation - Uri M. Ascher, Eddy Boxerman".
|
||||
|
||||
u32 b3_clothSolverIterations = 0;
|
||||
|
||||
b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
|
||||
{
|
||||
m_allocator = def.stack;
|
||||
|
||||
m_particleCapacity = def.particleCapacity;
|
||||
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_Jx = (b3Mat33*)m_allocator->Allocate(m_springCapacity * sizeof(b3Mat33));
|
||||
m_Jv = (b3Mat33*)m_allocator->Allocate(m_springCapacity * sizeof(b3Mat33));
|
||||
|
||||
m_contactCapacity = def.contactCapacity;
|
||||
m_contactCount = 0;
|
||||
m_contacts = (b3ParticleContact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3ParticleContact*));;
|
||||
}
|
||||
|
||||
b3ClothSolver::~b3ClothSolver()
|
||||
{
|
||||
m_allocator->Free(m_contacts);
|
||||
m_allocator->Free(m_Jv);
|
||||
m_allocator->Free(m_Jx);
|
||||
m_allocator->Free(m_springs);
|
||||
m_allocator->Free(m_particles);
|
||||
}
|
||||
|
||||
void b3ClothSolver::Add(b3Particle* p)
|
||||
{
|
||||
p->solverId = m_particleCount;
|
||||
m_particles[m_particleCount++] = p;
|
||||
}
|
||||
|
||||
void b3ClothSolver::Add(b3ParticleContact* c)
|
||||
{
|
||||
m_contacts[m_contactCount++] = c;
|
||||
}
|
||||
|
||||
void b3ClothSolver::Add(b3Spring* s)
|
||||
{
|
||||
m_springs[m_springCount++] = s;
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE b3SparseMat33 b3AllocSparse(b3StackAllocator* allocator, u32 M, u32 N)
|
||||
{
|
||||
u32 size = M * N;
|
||||
b3Mat33* elements = (b3Mat33*)allocator->Allocate(size * sizeof(b3Mat33));
|
||||
u32* cols = (u32*)allocator->Allocate(size * sizeof(u32));
|
||||
u32* row_ptrs = (u32*)allocator->Allocate((M + 1) * sizeof(u32));
|
||||
|
||||
return b3SparseMat33(M, N, size, elements, row_ptrs, cols);
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3FreeSparse(b3SparseMat33& matrix, b3StackAllocator* allocator)
|
||||
{
|
||||
allocator->Free(matrix.row_ptrs);
|
||||
allocator->Free(matrix.cols);
|
||||
allocator->Free(matrix.values);
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3CopyPosition(b3DenseVec3& v, b3Particle** const particles, u32 count)
|
||||
{
|
||||
for (u32 i = 0; i < count; ++i)
|
||||
{
|
||||
v[i] = particles[i]->position;
|
||||
}
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3CopyVelocity(b3DenseVec3& v, b3Particle** const particles, u32 count)
|
||||
{
|
||||
for (u32 i = 0; i < count; ++i)
|
||||
{
|
||||
v[i] = particles[i]->velocity;
|
||||
}
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3CopyForce(b3DenseVec3& v, b3Particle** const particles, u32 count)
|
||||
{
|
||||
for (u32 i = 0; i < count; ++i)
|
||||
{
|
||||
v[i] = particles[i]->force;
|
||||
}
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3CopyTranslation(b3DenseVec3& v, b3Particle** const particles, u32 count)
|
||||
{
|
||||
for (u32 i = 0; i < count; ++i)
|
||||
{
|
||||
v[i] = particles[i]->translation;
|
||||
}
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3CopyGuess(b3DenseVec3& v, b3Particle** const particles, u32 count)
|
||||
{
|
||||
for (u32 i = 0; i < count; ++i)
|
||||
{
|
||||
v[i] = particles[i]->x;
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
|
||||
{
|
||||
B3_PROFILE("Integrate");
|
||||
|
||||
m_h = dt;
|
||||
|
||||
b3DenseVec3 sx(m_particleCount);
|
||||
b3CopyPosition(sx, m_particles, m_particleCount);
|
||||
|
||||
b3DenseVec3 sv(m_particleCount);
|
||||
b3CopyVelocity(sv, m_particles, m_particleCount);
|
||||
|
||||
b3DenseVec3 sf(m_particleCount);
|
||||
b3CopyForce(sf, m_particles, m_particleCount);
|
||||
|
||||
b3DenseVec3 sy(m_particleCount);
|
||||
b3CopyTranslation(sy, m_particles, m_particleCount);
|
||||
|
||||
Compute_f(sf, sx, sv, gravity);
|
||||
|
||||
// Solve Ax = b, where
|
||||
// A = M - h * dfdv - h * h * dfdx
|
||||
// b = h * (f0 + h * dfdx * v0 + dfdx * y)
|
||||
|
||||
// A
|
||||
b3SparseMat33 A = b3AllocSparse(m_allocator, m_particleCount, m_particleCount);
|
||||
|
||||
// b
|
||||
b3DenseVec3 b(m_particleCount);
|
||||
|
||||
Compute_A_b(A, b, sf, sx, sv, sy);
|
||||
|
||||
// S
|
||||
b3DiagMat33 S(m_particleCount);
|
||||
Compute_S(S);
|
||||
|
||||
// z
|
||||
b3DenseVec3 z(m_particleCount);
|
||||
Compute_z(z);
|
||||
|
||||
// x0
|
||||
b3DenseVec3 x0(m_particleCount);
|
||||
b3CopyGuess(x0, m_particles, m_particleCount);
|
||||
|
||||
// x
|
||||
b3DenseVec3 x(m_particleCount);
|
||||
|
||||
// Solve Ax = b
|
||||
u32 iterations = 0;
|
||||
Solve(x, iterations, A, b, S, z, x0);
|
||||
|
||||
b3_clothSolverIterations = iterations;
|
||||
|
||||
// f = A * x - b
|
||||
b3DenseVec3 f = A * x - b;
|
||||
|
||||
// Update state
|
||||
float32 h = m_h;
|
||||
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_particles[i];
|
||||
b3ParticleType type = p->type;
|
||||
|
||||
b3Vec3 ix0 = sx[i];
|
||||
b3Vec3 iv0 = sv[i];
|
||||
b3Vec3 iy = sy[i];
|
||||
|
||||
b3Vec3 dv = x[i];
|
||||
|
||||
// v1 = v0 + dv
|
||||
b3Vec3 v1 = iv0;
|
||||
if (type == e_dynamicParticle)
|
||||
{
|
||||
v1 += dv;
|
||||
}
|
||||
|
||||
// dx = h * (v0 + dv) + y = h * v1 + y
|
||||
b3Vec3 dx = h * v1 + iy;
|
||||
|
||||
// x1 = x0 + dx
|
||||
b3Vec3 x1 = ix0 + dx;
|
||||
|
||||
sv[i] = v1;
|
||||
sx[i] = x1;
|
||||
}
|
||||
|
||||
// Write x to the solution cache for improving convergence
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
m_particles[i]->x = x[i];
|
||||
}
|
||||
|
||||
// Store the extra contact constraint forces that should have been
|
||||
// supplied in order to enforce the contact constraints exactly
|
||||
// These forces can be used in contact constraint logic
|
||||
for (u32 i = 0; i < m_contactCount; ++i)
|
||||
{
|
||||
b3ParticleContact* c = m_contacts[i];
|
||||
b3Particle* p = c->p1;
|
||||
|
||||
b3Vec3 force = f[p->solverId];
|
||||
|
||||
// Signed normal force magnitude
|
||||
c->Fn = b3Dot(force, c->n);
|
||||
|
||||
// Signed tangent force magnitude
|
||||
c->Ft1 = b3Dot(force, c->t1);
|
||||
c->Ft2 = b3Dot(force, c->t2);
|
||||
}
|
||||
|
||||
// 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];
|
||||
}
|
||||
|
||||
b3FreeSparse(A, m_allocator);
|
||||
}
|
||||
|
||||
#define B3_INDEX(i, j, size) (i + j * size)
|
||||
|
||||
//
|
||||
static void b3SetZero(b3Vec3* out, u32 size)
|
||||
{
|
||||
for (u32 i = 0; i < size; ++i)
|
||||
{
|
||||
out[i].SetZero();
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
static void b3SetZero(b3Mat33* out, u32 size)
|
||||
{
|
||||
for (u32 i = 0; i < size; ++i)
|
||||
{
|
||||
out[i].SetZero();
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
static void b3Mul_Jacobian(b3Vec3* out, const b3Vec3* v, u32 massCount,
|
||||
const b3Mat33* J_ii, b3Spring** const springs, u32 springCount)
|
||||
{
|
||||
b3SetZero(out, massCount);
|
||||
|
||||
for (u32 i = 0; i < springCount; ++i)
|
||||
{
|
||||
const b3Spring* S = springs[i];
|
||||
u32 i1 = S->p1->solverId;
|
||||
u32 i2 = S->p2->solverId;
|
||||
|
||||
b3Mat33 J_11 = J_ii[i];
|
||||
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];
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothSolver::Compute_f(b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3Vec3& gravity)
|
||||
{
|
||||
// Zero force
|
||||
f.SetZero();
|
||||
|
||||
// Apply weight
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_particles[i];
|
||||
|
||||
if (p->type == e_dynamicParticle)
|
||||
{
|
||||
f[i] += p->mass * gravity;
|
||||
}
|
||||
}
|
||||
|
||||
// Apply tension, damping
|
||||
// Store the Jacobians
|
||||
|
||||
// Zero Jacobians
|
||||
b3SetZero(m_Jx, m_springCount);
|
||||
b3SetZero(m_Jv, m_springCount);
|
||||
|
||||
for (u32 i = 0; i < m_springCount; ++i)
|
||||
{
|
||||
b3Spring* S = m_springs[i];
|
||||
b3SpringType type = S->type;
|
||||
b3Particle* p1 = S->p1;
|
||||
b3Particle* p2 = S->p2;
|
||||
float32 L0 = S->L0;
|
||||
float32 ks = S->ks;
|
||||
float32 kd = S->kd;
|
||||
|
||||
u32 i1 = p1->solverId;
|
||||
u32 i2 = p2->solverId;
|
||||
|
||||
b3Vec3 x1 = x[i1];
|
||||
b3Vec3 v1 = v[i1];
|
||||
|
||||
b3Vec3 x2 = x[i2];
|
||||
b3Vec3 v2 = v[i2];
|
||||
|
||||
const b3Mat33 I = b3Mat33_identity;
|
||||
|
||||
b3Vec3 dx = x1 - x2;
|
||||
|
||||
if (b3Dot(dx, dx) >= L0 * L0)
|
||||
{
|
||||
// Tension
|
||||
float32 L = b3Length(dx);
|
||||
b3Vec3 n = dx / L;
|
||||
|
||||
b3Vec3 sf1 = -ks * (L - L0) * n;
|
||||
b3Vec3 sf2 = -sf1;
|
||||
|
||||
f[i1] += sf1;
|
||||
f[i2] += sf2;
|
||||
|
||||
p1->tension += sf1;
|
||||
p2->tension += sf2;
|
||||
|
||||
b3Mat33 Jx11 = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx)));
|
||||
|
||||
m_Jx[i] = Jx11;
|
||||
}
|
||||
|
||||
// Damping
|
||||
b3Vec3 dv = v1 - v2;
|
||||
|
||||
b3Vec3 df1 = -kd * dv;
|
||||
b3Vec3 df2 = -df1;
|
||||
|
||||
f[i1] += df1;
|
||||
f[i2] += df2;
|
||||
|
||||
b3Mat33 Jv11 = -kd * I;
|
||||
|
||||
m_Jv[i] = Jv11;
|
||||
}
|
||||
}
|
||||
|
||||
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(b3SparseMat33& SA, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const
|
||||
{
|
||||
// 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)
|
||||
{
|
||||
const b3Spring* S = m_springs[i];
|
||||
u32 i1 = S->p1->solverId;
|
||||
u32 i2 = S->p2->solverId;
|
||||
|
||||
b3Mat33 Jx11 = m_Jx[i];
|
||||
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 = m_Jv[i];
|
||||
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;
|
||||
}
|
||||
|
||||
// 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);
|
||||
|
||||
// A += M
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
A[B3_INDEX(i, i, m_particleCount)] += b3Diagonal(m_particles[i]->mass);
|
||||
}
|
||||
|
||||
// A += - h * dfdv - h * h * dfdx
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
for (u32 j = 0; j < m_particleCount; ++j)
|
||||
{
|
||||
A[B3_INDEX(i, j, m_particleCount)] += (-m_h * dfdv[B3_INDEX(i, j, m_particleCount)]) + (-m_h * m_h * dfdx[B3_INDEX(i, j, m_particleCount)]);
|
||||
}
|
||||
}
|
||||
|
||||
// Assembly sparsity
|
||||
u32 nzCount = 0;
|
||||
|
||||
SA.row_ptrs[0] = 0;
|
||||
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
u32 rowNzCount = 0;
|
||||
|
||||
for (u32 j = 0; j < m_particleCount; ++j)
|
||||
{
|
||||
b3Mat33 a = A[B3_INDEX(i, j, m_particleCount)];
|
||||
|
||||
if (b3IsZero(a) == false)
|
||||
{
|
||||
B3_ASSERT(nzCount <= SA.valueCount);
|
||||
|
||||
SA.values[nzCount] = a;
|
||||
SA.cols[nzCount] = j;
|
||||
|
||||
++nzCount;
|
||||
++rowNzCount;
|
||||
}
|
||||
}
|
||||
|
||||
SA.row_ptrs[i + 1] = SA.row_ptrs[(i + 1) - 1] + rowNzCount;
|
||||
}
|
||||
|
||||
B3_ASSERT(nzCount <= SA.valueCount);
|
||||
SA.valueCount = nzCount;
|
||||
|
||||
m_allocator->Free(A);
|
||||
|
||||
m_allocator->Free(dfdv);
|
||||
m_allocator->Free(dfdx);
|
||||
|
||||
// Compute b
|
||||
// b = h * (f0 + h * Jx_v + Jx_y)
|
||||
|
||||
// Jx_v = dfdx * v
|
||||
b3Vec3* Jx_v = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3));
|
||||
b3Mul_Jacobian(Jx_v, v.v, m_particleCount, m_Jx, m_springs, m_springCount);
|
||||
|
||||
// Jx_y = dfdx * y
|
||||
b3Vec3* Jx_y = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3));
|
||||
b3Mul_Jacobian(Jx_y, y.v, m_particleCount, m_Jx, m_springs, m_springCount);
|
||||
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b[i] = m_h * (f[i] + m_h * Jx_v[i] + Jx_y[i]);
|
||||
}
|
||||
|
||||
m_allocator->Free(Jx_y);
|
||||
m_allocator->Free(Jx_v);
|
||||
}
|
||||
|
||||
void b3ClothSolver::Compute_z(b3DenseVec3& z)
|
||||
{
|
||||
// TODO
|
||||
z.SetZero();
|
||||
}
|
||||
|
||||
void b3ClothSolver::Compute_S(b3DiagMat33& out)
|
||||
{
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_particles[i];
|
||||
|
||||
if (p->type == e_staticParticle)
|
||||
{
|
||||
out[i].SetZero();
|
||||
continue;
|
||||
}
|
||||
|
||||
out[i].SetIdentity();
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_contactCount; ++i)
|
||||
{
|
||||
b3ParticleContact* c = m_contacts[i];
|
||||
|
||||
B3_ASSERT(c->n_active);
|
||||
|
||||
b3Vec3 n = c->n;
|
||||
|
||||
b3Mat33 S = b3Mat33_identity - b3Outer(n, n);
|
||||
|
||||
if (c->t1_active == true && c->t2_active == true)
|
||||
{
|
||||
S.SetZero();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (c->t1_active == true)
|
||||
{
|
||||
b3Vec3 t1 = c->t1;
|
||||
|
||||
S -= b3Outer(t1, t1);
|
||||
}
|
||||
|
||||
if (c->t2_active == true)
|
||||
{
|
||||
b3Vec3 t2 = c->t2;
|
||||
|
||||
S -= b3Outer(t2, t2);
|
||||
}
|
||||
}
|
||||
|
||||
b3Particle* p = c->p1;
|
||||
out[p->solverId] = S;
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
|
||||
const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const
|
||||
{
|
||||
B3_PROFILE("Solve A * x = b");
|
||||
|
||||
// P = diag(A)
|
||||
b3DiagMat33 inv_P(m_particleCount);
|
||||
A.AssembleDiagonal(inv_P);
|
||||
|
||||
// Invert
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b3Mat33& D = inv_P[i];
|
||||
|
||||
// Sylvester Criterion to ensure PD-ness
|
||||
B3_ASSERT(b3Det(D.x, D.y, D.z) > B3_EPSILON);
|
||||
|
||||
B3_ASSERT(D.x.x != 0.0f);
|
||||
float32 xx = 1.0f / D.x.x;
|
||||
|
||||
B3_ASSERT(D.y.y != 0.0f);
|
||||
float32 yy = 1.0f / D.y.y;
|
||||
|
||||
B3_ASSERT(D.z.z != 0.0f);
|
||||
float32 zz = 1.0f / D.z.z;
|
||||
|
||||
D = b3Diagonal(xx, yy, zz);
|
||||
}
|
||||
|
||||
// I
|
||||
b3DiagMat33 I(m_particleCount);
|
||||
I.SetIdentity();
|
||||
|
||||
// x = S * y + (I - S) * z
|
||||
x = (S * y) + (I - S) * z;
|
||||
|
||||
// b^ = S * (b - A * (I - S) * z)
|
||||
b3DenseVec3 b_hat = S * (b - A * ((I - S) * z));
|
||||
|
||||
// b_delta = dot(b^, P^-1 * b_^)
|
||||
float32 b_delta = b3Dot(b_hat, inv_P * b_hat);
|
||||
|
||||
// r = S * (b - A * x)
|
||||
b3DenseVec3 r = S * (b - A * x);
|
||||
|
||||
// p = S * (P^-1 * r)
|
||||
b3DenseVec3 p = S * (inv_P * r);
|
||||
|
||||
// deltaNew = dot(r, p)
|
||||
float32 deltaNew = b3Dot(r, p);
|
||||
|
||||
// Tolerance.
|
||||
// This is the main stopping criteria.
|
||||
// [0, 1]
|
||||
const float32 tolerance = 10.0f * B3_EPSILON;
|
||||
|
||||
// Maximum number of iterations.
|
||||
const u32 maxIters = 1000;
|
||||
|
||||
// Main iteration loop.
|
||||
u32 iter = 0;
|
||||
while (deltaNew > tolerance * tolerance * b_delta && iter < maxIters)
|
||||
{
|
||||
// s = S * (A * p)
|
||||
b3DenseVec3 s = S * (A * p);
|
||||
|
||||
// alpha = deltaNew / dot(c, q)
|
||||
float32 alpha = deltaNew / b3Dot(p, s);
|
||||
|
||||
// x = x + alpha * p
|
||||
x = x + alpha * p;
|
||||
|
||||
// r = r - alpha * s
|
||||
r = r - alpha * s;
|
||||
|
||||
// h = inv_P * r
|
||||
b3DenseVec3 h = inv_P * r;
|
||||
|
||||
// deltaOld = deltaNew
|
||||
float32 deltaOld = deltaNew;
|
||||
|
||||
// deltaNew = dot(r, h)
|
||||
deltaNew = b3Dot(r, h);
|
||||
//B3_ASSERT(b3IsValid(deltaNew));
|
||||
|
||||
// beta = deltaNew / deltaOld
|
||||
float32 beta = deltaNew / deltaOld;
|
||||
|
||||
// p = S * (h + beta * p)
|
||||
p = S * (h + beta * p);
|
||||
|
||||
++iter;
|
||||
}
|
||||
|
||||
iterations = iter;
|
||||
}
|
||||
@@ -1,765 +0,0 @@
|
||||
/*
|
||||
* 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_cloth.h>
|
||||
#include <bounce/dynamics/cloth/spring_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/common/memory/stack_allocator.h>
|
||||
#include <bounce/common/draw.h>
|
||||
|
||||
#define B3_FORCE_THRESHOLD 0.005f
|
||||
|
||||
#define B3_CLOTH_BENDING 0
|
||||
|
||||
#define B3_CLOTH_FRICTION 1
|
||||
|
||||
b3SpringCloth::b3SpringCloth()
|
||||
{
|
||||
m_allocator = nullptr;
|
||||
|
||||
m_mesh = nullptr;
|
||||
|
||||
m_gravity.SetZero();
|
||||
|
||||
m_x = nullptr;
|
||||
m_v = nullptr;
|
||||
m_f = nullptr;
|
||||
m_y = nullptr;
|
||||
m_z = nullptr;
|
||||
m_x0 = nullptr;
|
||||
m_m = nullptr;
|
||||
m_inv_m = nullptr;
|
||||
m_types = nullptr;
|
||||
m_contacts = nullptr;
|
||||
m_massCount = 0;
|
||||
|
||||
m_springs = nullptr;
|
||||
m_springCount = 0;
|
||||
|
||||
m_r = 0.0f;
|
||||
|
||||
m_shapeCount = 0;
|
||||
|
||||
m_step.iterations = 0;
|
||||
}
|
||||
|
||||
b3SpringCloth::~b3SpringCloth()
|
||||
{
|
||||
b3Free(m_x);
|
||||
b3Free(m_v);
|
||||
b3Free(m_f);
|
||||
b3Free(m_inv_m);
|
||||
b3Free(m_m);
|
||||
b3Free(m_y);
|
||||
b3Free(m_z);
|
||||
b3Free(m_x0);
|
||||
b3Free(m_types);
|
||||
b3Free(m_contacts);
|
||||
b3Free(m_springs);
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE u32 b3NextIndex(u32 i)
|
||||
{
|
||||
return i + 1 < 3 ? i + 1 : 0;
|
||||
}
|
||||
|
||||
struct b3UniqueEdge
|
||||
{
|
||||
u32 v1, v2;
|
||||
};
|
||||
|
||||
static u32 b3FindUniqueEdges(b3UniqueEdge* uniqueEdges, const b3ClothMesh* m)
|
||||
{
|
||||
u32 uniqueCount = 0;
|
||||
|
||||
for (u32 i = 0; i < m->triangleCount; ++i)
|
||||
{
|
||||
b3ClothMeshTriangle* t1 = m->triangles + i;
|
||||
u32 i1s[3] = { t1->v1, t1->v2, t1->v3 };
|
||||
|
||||
for (u32 j1 = 0; j1 < 3; ++j1)
|
||||
{
|
||||
u32 t1v1 = i1s[j1];
|
||||
u32 t1v2 = i1s[b3NextIndex(j1)];
|
||||
|
||||
bool unique = true;
|
||||
|
||||
for (u32 j = 0; j < uniqueCount; ++j)
|
||||
{
|
||||
b3UniqueEdge* ue = uniqueEdges + j;
|
||||
|
||||
if (ue->v1 == t1v1 && ue->v2 == t1v2)
|
||||
{
|
||||
unique = false;
|
||||
break;
|
||||
}
|
||||
|
||||
if (ue->v2 == t1v1 && ue->v1 == t1v2)
|
||||
{
|
||||
unique = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (unique)
|
||||
{
|
||||
b3UniqueEdge ue;
|
||||
ue.v1 = t1v1;
|
||||
ue.v2 = t1v2;
|
||||
uniqueEdges[uniqueCount++] = ue;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return uniqueCount;
|
||||
}
|
||||
|
||||
struct b3SharedEdge
|
||||
{
|
||||
u32 v1, v2;
|
||||
u32 nsv1, nsv2;
|
||||
};
|
||||
|
||||
static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m)
|
||||
{
|
||||
u32 sharedCount = 0;
|
||||
|
||||
for (u32 i = 0; i < m->triangleCount; ++i)
|
||||
{
|
||||
b3ClothMeshTriangle* t1 = m->triangles + i;
|
||||
u32 i1s[3] = { t1->v1, t1->v2, t1->v3 };
|
||||
|
||||
for (u32 j1 = 0; j1 < 3; ++j1)
|
||||
{
|
||||
u32 k1 = j1 + 1 < 3 ? j1 + 1 : 0;
|
||||
|
||||
u32 t1v1 = i1s[j1];
|
||||
u32 t1v2 = i1s[k1];
|
||||
|
||||
for (u32 j = i + 1; j < m->triangleCount; ++j)
|
||||
{
|
||||
b3ClothMeshTriangle* t2 = m->triangles + j;
|
||||
u32 i2s[3] = { t2->v1, t2->v2, t2->v3 };
|
||||
|
||||
for (u32 j2 = 0; j2 < 3; ++j2)
|
||||
{
|
||||
u32 k2 = j2 + 1 < 3 ? j2 + 1 : 0;
|
||||
|
||||
u32 t2v1 = i2s[j2];
|
||||
u32 t2v2 = i2s[k2];
|
||||
|
||||
if (t1v1 == t2v2 && t1v2 == t2v1)
|
||||
{
|
||||
// The triangles are adjacent.
|
||||
u32 k3 = k1 + 1 < 3 ? k1 + 1 : 0;
|
||||
u32 t1v3 = i1s[k3];
|
||||
|
||||
u32 k4 = k2 + 1 < 3 ? k2 + 1 : 0;
|
||||
u32 t2v3 = i2s[k4];
|
||||
|
||||
// Add shared edge and non-shared vertices.
|
||||
b3SharedEdge se;
|
||||
se.v1 = t1v1;
|
||||
se.v2 = t1v2;
|
||||
se.nsv1 = t1v3;
|
||||
se.nsv2 = t2v3;
|
||||
|
||||
sharedEdges[sharedCount++] = se;
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return sharedCount;
|
||||
}
|
||||
|
||||
void b3SpringCloth::Initialize(const b3SpringClothDef& def)
|
||||
{
|
||||
B3_ASSERT(def.allocator);
|
||||
B3_ASSERT(def.mesh);
|
||||
B3_ASSERT(def.density > 0.0f);
|
||||
|
||||
m_allocator = def.allocator;
|
||||
|
||||
m_mesh = def.mesh;
|
||||
m_r = def.r;
|
||||
|
||||
m_gravity = def.gravity;
|
||||
|
||||
const b3ClothMesh* m = m_mesh;
|
||||
|
||||
m_massCount = m->vertexCount;
|
||||
m_x = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3));
|
||||
m_v = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3));
|
||||
m_f = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3));
|
||||
m_m = (float32*)b3Alloc(m_massCount * sizeof(float32));
|
||||
m_inv_m = (float32*)b3Alloc(m_massCount * sizeof(float32));
|
||||
m_y = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3));
|
||||
m_z = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3));
|
||||
m_x0 = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3));
|
||||
m_types = (b3MassType*)b3Alloc(m_massCount * sizeof(b3MassType));
|
||||
m_contacts = (b3MassContact*)b3Alloc(m_massCount * sizeof(b3MassContact));
|
||||
|
||||
for (u32 i = 0; i < m->vertexCount; ++i)
|
||||
{
|
||||
m_contacts[i].Fn = 0.0f;
|
||||
m_contacts[i].Ft1 = 0.0f;
|
||||
m_contacts[i].Ft2 = 0.0f;
|
||||
m_contacts[i].lockN = false;
|
||||
m_contacts[i].lockT1 = false;
|
||||
m_contacts[i].lockT2 = false;
|
||||
|
||||
m_x[i] = m->vertices[i];
|
||||
m_v[i].SetZero();
|
||||
m_f[i].SetZero();
|
||||
m_m[i] = 0.0f;
|
||||
m_inv_m[i] = 0.0f;
|
||||
m_y[i].SetZero();
|
||||
m_z[i].SetZero();
|
||||
m_x0[i].SetZero();
|
||||
m_types[i] = b3MassType::e_staticMass;
|
||||
}
|
||||
|
||||
// Initialize mass
|
||||
for (u32 i = 0; i < m->triangleCount; ++i)
|
||||
{
|
||||
b3ClothMeshTriangle* t = m->triangles + i;
|
||||
|
||||
b3Vec3 p1 = m->vertices[t->v1];
|
||||
b3Vec3 p2 = m->vertices[t->v2];
|
||||
b3Vec3 p3 = m->vertices[t->v3];
|
||||
|
||||
float32 area = b3Area(p1, p2, p3);
|
||||
|
||||
B3_ASSERT(area > B3_EPSILON);
|
||||
|
||||
float32 mass = def.density * area;
|
||||
|
||||
const float32 inv3 = 1.0f / 3.0f;
|
||||
|
||||
m_m[t->v1] += inv3 * mass;
|
||||
m_m[t->v2] += inv3 * mass;
|
||||
m_m[t->v3] += inv3 * mass;
|
||||
}
|
||||
|
||||
// Invert
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
B3_ASSERT(m_m[i] > 0.0f);
|
||||
m_inv_m[i] = 1.0f / m_m[i];
|
||||
m_types[i] = b3MassType::e_dynamicMass;
|
||||
}
|
||||
|
||||
// Initialize springs
|
||||
u32 edgeCount = 3 * m->triangleCount;
|
||||
|
||||
b3UniqueEdge* uniqueEdges = (b3UniqueEdge*)m_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));
|
||||
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 p1 = m->vertices[e->v1];
|
||||
b3Vec3 p2 = m->vertices[e->v2];
|
||||
|
||||
b3Spring* S = m_springs + m_springCount;
|
||||
S->type = e_strechSpring;
|
||||
S->i1 = e->v1;
|
||||
S->i2 = e->v2;
|
||||
S->L0 = b3Distance(p1, p2);
|
||||
|
||||
B3_ASSERT(S->L0 > B3_EPSILON);
|
||||
|
||||
S->ks = def.ks;
|
||||
S->kd = def.kd;
|
||||
++m_springCount;
|
||||
}
|
||||
|
||||
#if B3_CLOTH_BENDING
|
||||
|
||||
// Bending
|
||||
for (u32 i = 0; i < sharedCount; ++i)
|
||||
{
|
||||
b3SharedEdge* e = sharedEdges + i;
|
||||
|
||||
b3Vec3 p1 = m->vertices[e->nsv1];
|
||||
b3Vec3 p2 = m->vertices[e->nsv2];
|
||||
|
||||
b3Spring* S = m_springs + m_springCount;
|
||||
S->type = e_bendSpring;
|
||||
S->i1 = e->nsv1;
|
||||
S->i2 = e->nsv2;
|
||||
S->L0 = b3Distance(p1, p2);
|
||||
S->ks = def.kb;
|
||||
S->kd = def.kd;
|
||||
++m_springCount;
|
||||
}
|
||||
|
||||
m_allocator->Free(sharedEdges);
|
||||
#endif
|
||||
|
||||
m_allocator->Free(uniqueEdges);
|
||||
|
||||
// Sewing
|
||||
for (u32 i = 0; i < m->sewingLineCount; ++i)
|
||||
{
|
||||
b3ClothMeshSewingLine* line = m->sewingLines + i;
|
||||
|
||||
b3Spring* S = m_springs + m_springCount;
|
||||
S->type = e_strechSpring;
|
||||
S->i1 = line->v1;
|
||||
S->i2 = line->v2;
|
||||
S->L0 = 0.0f;
|
||||
S->ks = def.ks;
|
||||
S->kd = def.kd;
|
||||
++m_springCount;
|
||||
}
|
||||
|
||||
B3_ASSERT(m_springCount <= springCapacity);
|
||||
}
|
||||
|
||||
void b3SpringCloth::AddShape(b3Shape* shape)
|
||||
{
|
||||
B3_ASSERT(m_shapeCount < B3_CLOTH_SHAPE_CAPACITY);
|
||||
|
||||
if (m_shapeCount == B3_CLOTH_SHAPE_CAPACITY)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
m_shapes[m_shapeCount++] = shape;
|
||||
}
|
||||
|
||||
void b3SpringCloth::GetTension(b3Array<b3Vec3>& T) const
|
||||
{
|
||||
B3_ASSERT(T.Count() == 0);
|
||||
|
||||
T.Resize(m_massCount);
|
||||
|
||||
for (u32 i = 0; i < T.Count(); ++i)
|
||||
{
|
||||
T[i].SetZero();
|
||||
}
|
||||
|
||||
// T = F - mg
|
||||
for (u32 i = 0; i < m_springCount; ++i)
|
||||
{
|
||||
b3Spring* S = m_springs + i;
|
||||
|
||||
b3SpringType type = S->type;
|
||||
u32 i1 = S->i1;
|
||||
u32 i2 = S->i2;
|
||||
float32 L0 = S->L0;
|
||||
float32 ks = S->ks;
|
||||
float32 kd = S->kd;
|
||||
|
||||
b3Vec3 x1 = m_x[i1];
|
||||
b3Vec3 v1 = m_v[i1];
|
||||
|
||||
b3Vec3 x2 = m_x[i2];
|
||||
b3Vec3 v2 = m_v[i2];
|
||||
|
||||
// Streching
|
||||
b3Vec3 dx = x1 - x2;
|
||||
float32 L = b3Length(dx);
|
||||
|
||||
if (L >= L0)
|
||||
{
|
||||
// Force is tension.
|
||||
b3Vec3 n = dx / L;
|
||||
|
||||
b3Vec3 sf1 = -ks * (L - L0) * n;
|
||||
b3Vec3 sf2 = -sf1;
|
||||
|
||||
T[i1] += sf1;
|
||||
T[i2] += sf2;
|
||||
}
|
||||
|
||||
// Damping
|
||||
b3Vec3 dv = v1 - v2;
|
||||
|
||||
b3Vec3 df1 = -kd * dv;
|
||||
b3Vec3 df2 = -df1;
|
||||
|
||||
T[i1] += df1;
|
||||
T[i2] += df2;
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < T.Count(); ++i)
|
||||
{
|
||||
if (m_types[i] != b3MassType::e_dynamicMass)
|
||||
{
|
||||
T[i].SetZero();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void b3SpringCloth::UpdateContacts()
|
||||
{
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
// Static masses can't participate in collisions.
|
||||
if (m_types[i] == b3MassType::e_staticMass)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
b3MassContact* c = m_contacts + i;
|
||||
|
||||
// Save the old contact
|
||||
b3MassContact c0 = *c;
|
||||
|
||||
// Create a new contact
|
||||
c->lockN = false;
|
||||
c->lockT1 = false;
|
||||
c->lockT2 = false;
|
||||
|
||||
b3Sphere s1;
|
||||
s1.vertex = m_x[i];
|
||||
s1.radius = m_r;
|
||||
|
||||
// Find the deepest penetration
|
||||
float32 bestSeparation = 0.0f;
|
||||
b3Vec3 bestNormal(0.0f, 0.0f, 0.0f);
|
||||
u32 bestIndex = ~0;
|
||||
|
||||
for (u32 j = 0; j < m_shapeCount; ++j)
|
||||
{
|
||||
b3Shape* s2 = m_shapes[j];
|
||||
|
||||
b3Transform xf2;
|
||||
xf2.SetIdentity();
|
||||
|
||||
b3TestSphereOutput output;
|
||||
if (s2->TestSphere(&output, s1, xf2) == false)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (output.separation < bestSeparation)
|
||||
{
|
||||
bestSeparation = output.separation;
|
||||
bestNormal = output.normal;
|
||||
bestIndex = j;
|
||||
}
|
||||
}
|
||||
|
||||
if (bestIndex != ~0)
|
||||
{
|
||||
B3_ASSERT(bestSeparation <= 0.0f);
|
||||
|
||||
b3Shape* shape = m_shapes[bestIndex];
|
||||
float32 s = bestSeparation;
|
||||
b3Vec3 n = bestNormal;
|
||||
|
||||
// Update contact manifold
|
||||
// Remember the normal orientation is from shape 2 to shape 1 (mass)
|
||||
c->j = bestIndex;
|
||||
c->n = n;
|
||||
c->t1 = b3Perp(n);
|
||||
c->t2 = b3Cross(c->t1, n);
|
||||
c->lockN = true;
|
||||
|
||||
// Apply position correction
|
||||
m_y[i] -= s * n;
|
||||
}
|
||||
|
||||
// Update contact state
|
||||
if (c0.lockN == true && c->lockN == true)
|
||||
{
|
||||
// The contact persists
|
||||
|
||||
// Has the contact constraint been satisfied?
|
||||
if (c0.Fn <= -B3_FORCE_THRESHOLD)
|
||||
{
|
||||
// Contact force is attractive.
|
||||
|
||||
// Terminate the contact.
|
||||
c->lockN = false;
|
||||
}
|
||||
}
|
||||
|
||||
#if 0
|
||||
// Notify the new contact state
|
||||
if (wasLockedN == false && c->lockN == true)
|
||||
{
|
||||
// The contact has begun
|
||||
}
|
||||
|
||||
if (wasLockedN == true && c->lockN == false)
|
||||
{
|
||||
// The contact has ended
|
||||
}
|
||||
#endif
|
||||
if (c->lockN == false)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
#if B3_CLOTH_FRICTION == 1
|
||||
|
||||
// A friction force requires an associated normal force.
|
||||
if (c0.lockN == false)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
b3Shape* s = m_shapes[c->j];
|
||||
b3Vec3 n = c->n;
|
||||
float32 u = s->GetFriction();
|
||||
float32 normalForce = c0.Fn;
|
||||
|
||||
// Relative velocity
|
||||
b3Vec3 dv = m_v[i];
|
||||
|
||||
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;
|
||||
}
|
||||
else
|
||||
{
|
||||
c->lockT1 = true;
|
||||
c->lockT2 = true;
|
||||
continue;
|
||||
}
|
||||
|
||||
b3Vec3 ts[2];
|
||||
ts[0] = c->t1;
|
||||
ts[1] = c->t2;
|
||||
|
||||
bool lockT[2];
|
||||
lockT[0] = c->lockT1;
|
||||
lockT[1] = c->lockT2;
|
||||
|
||||
bool lockT0[2];
|
||||
lockT0[0] = c0.lockT1;
|
||||
lockT0[1] = c0.lockT2;
|
||||
|
||||
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 mass on surface
|
||||
lockT[k] = true;
|
||||
}
|
||||
|
||||
if (lockT0[k] == true && lockT[k] == true)
|
||||
{
|
||||
// The contact persists
|
||||
float32 maxForce = u * normalForce;
|
||||
|
||||
if (Ft0[k] * Ft0[k] > maxForce * maxForce)
|
||||
{
|
||||
// Unlock mass off surface
|
||||
lockT[k] = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
c->lockT1 = lockT[0];
|
||||
c->lockT2 = lockT[1];
|
||||
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void b3SpringCloth::Step(float32 dt)
|
||||
{
|
||||
if (dt == 0.0f)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// Update contacts
|
||||
UpdateContacts();
|
||||
|
||||
// Apply weights
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
if (m_types[i] == b3MassType::e_dynamicMass)
|
||||
{
|
||||
m_f[i] += m_m[i] * m_gravity;
|
||||
}
|
||||
}
|
||||
|
||||
// Integrate
|
||||
b3SpringSolverDef solverDef;
|
||||
solverDef.cloth = this;
|
||||
solverDef.dt = dt;
|
||||
|
||||
b3SpringSolver solver(solverDef);
|
||||
|
||||
// Extra constraint forces that should have been applied to satisfy the constraints
|
||||
// todo Find the applied constraint forces.
|
||||
b3DenseVec3 forces(m_massCount);
|
||||
|
||||
solver.Solve(forces);
|
||||
|
||||
m_step.iterations = solver.GetIterations();
|
||||
|
||||
// Store constraint forces for physics logic
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
b3MassContact* c = m_contacts + i;
|
||||
|
||||
if (c->lockN == false)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
b3Vec3 force = forces[i];
|
||||
|
||||
// Signed normal force magnitude
|
||||
c->Fn = b3Dot(force, c->n);
|
||||
|
||||
// Signed tangent force magnitude
|
||||
c->Ft1 = b3Dot(force, c->t1);
|
||||
c->Ft2 = b3Dot(force, c->t2);
|
||||
}
|
||||
|
||||
// Clear position alteration
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
m_y[i].SetZero();
|
||||
}
|
||||
|
||||
// Clear forces
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
m_f[i].SetZero();
|
||||
}
|
||||
}
|
||||
|
||||
void b3SpringCloth::Apply() const
|
||||
{
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
m_mesh->vertices[i] = m_x[i];
|
||||
}
|
||||
}
|
||||
|
||||
void b3SpringCloth::Draw() const
|
||||
{
|
||||
const b3ClothMesh* m = m_mesh;
|
||||
|
||||
for (u32 i = 0; i < m->vertexCount; ++i)
|
||||
{
|
||||
if (m_types[i] == b3MassType::e_staticMass)
|
||||
{
|
||||
b3Draw_draw->DrawPoint(m_x[i], 4.0f, b3Color_white);
|
||||
}
|
||||
|
||||
if (m_types[i] == b3MassType::e_kinematicMass)
|
||||
{
|
||||
b3Draw_draw->DrawPoint(m_x[i], 4.0f, b3Color_blue);
|
||||
}
|
||||
|
||||
if (m_types[i] == b3MassType::e_dynamicMass)
|
||||
{
|
||||
b3Draw_draw->DrawPoint(m_x[i], 4.0f, b3Color_green);
|
||||
}
|
||||
|
||||
b3MassContact* c = m_contacts + i;
|
||||
|
||||
if (c->lockN)
|
||||
{
|
||||
b3Draw_draw->DrawSegment(m_x[i], m_x[i] + c->n, b3Color_yellow);
|
||||
}
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_springCount; ++i)
|
||||
{
|
||||
b3Spring* s = m_springs + i;
|
||||
|
||||
b3Vec3 x1 = m_x[s->i1];
|
||||
b3Vec3 x2 = m_x[s->i2];
|
||||
|
||||
if (s->type == e_strechSpring)
|
||||
{
|
||||
b3Draw_draw->DrawSegment(x1, x2, b3Color_black);
|
||||
}
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m->sewingLineCount; ++i)
|
||||
{
|
||||
b3ClothMeshSewingLine* s = m->sewingLines + i;
|
||||
|
||||
b3Vec3 x1 = m_x[s->v1];
|
||||
b3Vec3 x2 = m_x[s->v2];
|
||||
|
||||
b3Draw_draw->DrawSegment(x1, x2, b3Color_white);
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m->triangleCount; ++i)
|
||||
{
|
||||
b3ClothMeshTriangle* t = m->triangles + i;
|
||||
|
||||
b3Vec3 v1 = m_x[t->v1];
|
||||
b3Vec3 v2 = m_x[t->v2];
|
||||
b3Vec3 v3 = m_x[t->v3];
|
||||
|
||||
b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1);
|
||||
n1.Normalize();
|
||||
b3Draw_draw->DrawSolidTriangle(n1, v1, v2, v3, b3Color_blue);
|
||||
|
||||
b3Vec3 n2 = -n1;
|
||||
b3Draw_draw->DrawSolidTriangle(n2, v1, v3, v2, b3Color_blue);
|
||||
}
|
||||
}
|
||||
@@ -1,550 +0,0 @@
|
||||
/*
|
||||
* 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_solver.h>
|
||||
#include <bounce/dynamics/cloth/spring_cloth.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/shapes/shape.h>
|
||||
#include <bounce/common/memory/stack_allocator.h>
|
||||
|
||||
// Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm.
|
||||
// described in the paper:
|
||||
// "Large Steps in Cloth Simulation - David Baraff, Andrew Witkin".
|
||||
|
||||
// Some improvements for the original MPCG algorithm are described in the paper:
|
||||
// "On the modified conjugate gradient method in cloth simulation - Uri M. Ascher, Eddy Boxerman".
|
||||
|
||||
b3SpringSolver::b3SpringSolver(const b3SpringSolverDef& def)
|
||||
{
|
||||
m_cloth = def.cloth;
|
||||
m_h = def.dt;
|
||||
m_iterations = 0;
|
||||
m_Jx = nullptr;
|
||||
m_Jv = nullptr;
|
||||
|
||||
m_allocator = m_cloth->m_allocator;
|
||||
|
||||
m_x = m_cloth->m_x;
|
||||
m_v = m_cloth->m_v;
|
||||
m_f = m_cloth->m_f;
|
||||
m_m = m_cloth->m_m;
|
||||
m_inv_m = m_cloth->m_inv_m;
|
||||
m_y = m_cloth->m_y;
|
||||
m_z = m_cloth->m_y;
|
||||
m_x0 = m_cloth->m_x0;
|
||||
m_types = m_cloth->m_types;
|
||||
m_massCount = m_cloth->m_massCount;
|
||||
|
||||
m_contacts = m_cloth->m_contacts;
|
||||
|
||||
m_springs = m_cloth->m_springs;
|
||||
m_springCount = m_cloth->m_springCount;
|
||||
|
||||
m_shapes = m_cloth->m_shapes;
|
||||
}
|
||||
|
||||
b3SpringSolver::~b3SpringSolver()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE b3SparseMat33 b3AllocSparse(b3StackAllocator* allocator, u32 M, u32 N)
|
||||
{
|
||||
u32 size = M * N;
|
||||
b3Mat33* elements = (b3Mat33*)allocator->Allocate(size * sizeof(b3Mat33));
|
||||
u32* cols = (u32*)allocator->Allocate(size * sizeof(u32));
|
||||
u32* row_ptrs = (u32*)allocator->Allocate((M + 1) * sizeof(u32));
|
||||
|
||||
return b3SparseMat33(M, N, size, elements, row_ptrs, cols);
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3FreeSparse(b3SparseMat33& matrix, b3StackAllocator* allocator)
|
||||
{
|
||||
allocator->Free(matrix.row_ptrs);
|
||||
allocator->Free(matrix.cols);
|
||||
allocator->Free(matrix.values);
|
||||
}
|
||||
|
||||
void b3SpringSolver::Solve(b3DenseVec3& f)
|
||||
{
|
||||
B3_PROFILE("Solve");
|
||||
|
||||
//
|
||||
m_Jx = (b3Mat33*)m_allocator->Allocate(m_springCount * sizeof(b3Mat33));
|
||||
m_Jv = (b3Mat33*)m_allocator->Allocate(m_springCount * sizeof(b3Mat33));
|
||||
|
||||
// Apply internal forces. Also, store their unique derivatives.
|
||||
ApplySpringForces();
|
||||
|
||||
// Integrate
|
||||
|
||||
// Solve Ax = b, where
|
||||
// A = M - h * dfdv - h * h * dfdx
|
||||
// b = h * (f0 + h * dfdx * v0 + dfdx * y)
|
||||
|
||||
{
|
||||
// A
|
||||
b3SparseMat33 A = b3AllocSparse(m_allocator, m_massCount, m_massCount);
|
||||
|
||||
// b
|
||||
b3DenseVec3 b(m_massCount);
|
||||
|
||||
Compute_A_b(A, b);
|
||||
|
||||
// S
|
||||
b3DiagMat33 S(m_massCount);
|
||||
Compute_S(S);
|
||||
|
||||
// z
|
||||
b3DenseVec3 z(m_massCount);
|
||||
Compute_z(z);
|
||||
|
||||
// x0
|
||||
b3DenseVec3 x0(m_massCount);
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
x0[i] = m_x0[i];
|
||||
}
|
||||
|
||||
// x
|
||||
b3DenseVec3 x(m_massCount);
|
||||
|
||||
// Solve Ax = b
|
||||
Solve(x, f, m_iterations, A, b, S, z, x0);
|
||||
|
||||
// Store x0
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
m_x0[i] = x[i];
|
||||
}
|
||||
|
||||
// Update state
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
if (m_types[i] == b3MassType::e_dynamicMass)
|
||||
{
|
||||
m_v[i] += x[i];
|
||||
}
|
||||
|
||||
// dx = h * (v0 + dv) + y = h * v1 + y
|
||||
m_x[i] += m_h * m_v[i] + m_y[i];
|
||||
}
|
||||
|
||||
b3FreeSparse(A, m_allocator);
|
||||
}
|
||||
|
||||
m_allocator->Free(m_Jv);
|
||||
m_allocator->Free(m_Jx);
|
||||
m_Jv = nullptr;
|
||||
m_Jx = nullptr;
|
||||
}
|
||||
|
||||
#define B3_INDEX(i, j, size) (i + j * size)
|
||||
|
||||
//
|
||||
static void b3SetZero(b3Vec3* out, u32 size)
|
||||
{
|
||||
for (u32 i = 0; i < size; ++i)
|
||||
{
|
||||
out[i].SetZero();
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
static void b3SetZero(b3Mat33* out, u32 size)
|
||||
{
|
||||
for (u32 i = 0; i < size; ++i)
|
||||
{
|
||||
out[i].SetZero();
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
static void b3Mul_Jacobian(b3Vec3* out, const b3Vec3* v, u32 massCount,
|
||||
const b3Mat33* J_ii, const b3Spring* springs, u32 springCount)
|
||||
{
|
||||
b3SetZero(out, massCount);
|
||||
|
||||
for (u32 i = 0; i < springCount; ++i)
|
||||
{
|
||||
const b3Spring* S = springs + i;
|
||||
u32 i1 = S->i1;
|
||||
u32 i2 = S->i2;
|
||||
|
||||
b3Mat33 J_11 = J_ii[i];
|
||||
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];
|
||||
}
|
||||
}
|
||||
|
||||
void b3SpringSolver::ApplySpringForces()
|
||||
{
|
||||
// Zero Jacobians
|
||||
b3SetZero(m_Jx, m_springCount);
|
||||
b3SetZero(m_Jv, m_springCount);
|
||||
|
||||
// Compute spring forces and Jacobians
|
||||
for (u32 i = 0; i < m_springCount; ++i)
|
||||
{
|
||||
b3Spring* S = m_springs + i;
|
||||
|
||||
b3SpringType type = S->type;
|
||||
u32 i1 = S->i1;
|
||||
u32 i2 = S->i2;
|
||||
float32 L0 = S->L0;
|
||||
B3_ASSERT(L0 > 0.0f);
|
||||
float32 ks = S->ks;
|
||||
float32 kd = S->kd;
|
||||
|
||||
b3Vec3 x1 = m_x[i1];
|
||||
b3Vec3 v1 = m_v[i1];
|
||||
|
||||
b3Vec3 x2 = m_x[i2];
|
||||
b3Vec3 v2 = m_v[i2];
|
||||
|
||||
const b3Mat33 I = b3Mat33_identity;
|
||||
|
||||
// Strech
|
||||
b3Vec3 dx = x1 - x2;
|
||||
float32 L = b3Length(dx);
|
||||
|
||||
if (L >= L0)
|
||||
{
|
||||
// Force is tension.
|
||||
b3Vec3 n = dx / L;
|
||||
|
||||
b3Vec3 sf1 = -ks * (L - L0) * n;
|
||||
b3Vec3 sf2 = -sf1;
|
||||
|
||||
m_f[i1] += sf1;
|
||||
m_f[i2] += sf2;
|
||||
|
||||
b3Mat33 Jx11 = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx)));
|
||||
|
||||
m_Jx[i] = Jx11;
|
||||
}
|
||||
|
||||
// Damping
|
||||
b3Vec3 dv = v1 - v2;
|
||||
|
||||
b3Vec3 df1 = -kd * dv;
|
||||
b3Vec3 df2 = -df1;
|
||||
|
||||
m_f[i1] += df1;
|
||||
m_f[i2] += df2;
|
||||
|
||||
b3Mat33 Jv11 = -kd * I;
|
||||
|
||||
m_Jv[i] = Jv11;
|
||||
}
|
||||
}
|
||||
|
||||
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 b3SpringSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b) const
|
||||
{
|
||||
// Compute dfdx, dfdv
|
||||
b3Mat33* dfdx = (b3Mat33*)m_allocator->Allocate(m_massCount * m_massCount * sizeof(b3Mat33));
|
||||
b3SetZero(dfdx, m_massCount * m_massCount);
|
||||
|
||||
b3Mat33* dfdv = (b3Mat33*)m_allocator->Allocate(m_massCount * m_massCount * sizeof(b3Mat33));
|
||||
b3SetZero(dfdv, m_massCount * m_massCount);
|
||||
|
||||
for (u32 i = 0; i < m_springCount; ++i)
|
||||
{
|
||||
const b3Spring* S = m_springs + i;
|
||||
u32 i1 = S->i1;
|
||||
u32 i2 = S->i2;
|
||||
|
||||
b3Mat33 Jx11 = m_Jx[i];
|
||||
b3Mat33 Jx12 = -Jx11;
|
||||
b3Mat33 Jx21 = Jx12;
|
||||
b3Mat33 Jx22 = Jx11;
|
||||
|
||||
dfdx[B3_INDEX(i1, i1, m_massCount)] += Jx11;
|
||||
dfdx[B3_INDEX(i1, i2, m_massCount)] += Jx12;
|
||||
dfdx[B3_INDEX(i2, i1, m_massCount)] += Jx21;
|
||||
dfdx[B3_INDEX(i2, i2, m_massCount)] += Jx22;
|
||||
|
||||
b3Mat33 Jv11 = m_Jv[i];
|
||||
b3Mat33 Jv12 = -Jv11;
|
||||
b3Mat33 Jv21 = Jv12;
|
||||
b3Mat33 Jv22 = Jv11;
|
||||
|
||||
dfdv[B3_INDEX(i1, i1, m_massCount)] += Jv11;
|
||||
dfdv[B3_INDEX(i1, i2, m_massCount)] += Jv12;
|
||||
dfdv[B3_INDEX(i2, i1, m_massCount)] += Jv21;
|
||||
dfdv[B3_INDEX(i2, i2, m_massCount)] += Jv22;
|
||||
}
|
||||
|
||||
// Compute A
|
||||
// A = M - h * dfdv - h * h * dfdx
|
||||
|
||||
// A = 0
|
||||
b3Mat33* A = (b3Mat33*)m_allocator->Allocate(m_massCount * m_massCount * sizeof(b3Mat33));
|
||||
b3SetZero(A, m_massCount * m_massCount);
|
||||
|
||||
// A += M
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
A[B3_INDEX(i, i, m_massCount)] += b3Diagonal(m_m[i]);
|
||||
}
|
||||
|
||||
// A += - h * dfdv - h * h * dfdx
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
for (u32 j = 0; j < m_massCount; ++j)
|
||||
{
|
||||
A[B3_INDEX(i, j, m_massCount)] += (-m_h * dfdv[B3_INDEX(i, j, m_massCount)]) + (-m_h * m_h * dfdx[B3_INDEX(i, j, m_massCount)]);
|
||||
}
|
||||
}
|
||||
|
||||
// Assembly sparsity
|
||||
u32 nzCount = 0;
|
||||
|
||||
SA.row_ptrs[0] = 0;
|
||||
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
u32 rowNzCount = 0;
|
||||
|
||||
for (u32 j = 0; j < m_massCount; ++j)
|
||||
{
|
||||
b3Mat33 a = A[B3_INDEX(i, j, m_massCount)];
|
||||
|
||||
if (b3IsZero(a) == false)
|
||||
{
|
||||
B3_ASSERT(nzCount <= SA.valueCount);
|
||||
|
||||
SA.values[nzCount] = a;
|
||||
SA.cols[nzCount] = j;
|
||||
|
||||
++nzCount;
|
||||
++rowNzCount;
|
||||
}
|
||||
}
|
||||
|
||||
SA.row_ptrs[i + 1] = SA.row_ptrs[(i + 1) - 1] + rowNzCount;
|
||||
}
|
||||
|
||||
B3_ASSERT(nzCount <= SA.valueCount);
|
||||
SA.valueCount = nzCount;
|
||||
|
||||
m_allocator->Free(A);
|
||||
|
||||
// Compute b
|
||||
// b = h * (f0 + h * Jx_v + Jx_y)
|
||||
|
||||
// Jx_v = dfdx * v
|
||||
b3Vec3* Jx_v = (b3Vec3*)m_allocator->Allocate(m_massCount * sizeof(b3Vec3));
|
||||
b3Mul_Jacobian(Jx_v, m_v, m_massCount, m_Jx, m_springs, m_springCount);
|
||||
|
||||
// Jx_y = dfdx * y
|
||||
b3Vec3* Jx_y = (b3Vec3*)m_allocator->Allocate(m_massCount * sizeof(b3Vec3));
|
||||
b3Mul_Jacobian(Jx_y, m_y, m_massCount, m_Jx, m_springs, m_springCount);
|
||||
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
b[i] = m_h * (m_f[i] + m_h * Jx_v[i] + Jx_y[i]);
|
||||
}
|
||||
|
||||
m_allocator->Free(Jx_y);
|
||||
m_allocator->Free(Jx_v);
|
||||
|
||||
m_allocator->Free(dfdv);
|
||||
m_allocator->Free(dfdx);
|
||||
}
|
||||
|
||||
void b3SpringSolver::Compute_z(b3DenseVec3& z)
|
||||
{
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
z[i] = m_z[i];
|
||||
}
|
||||
}
|
||||
|
||||
void b3SpringSolver::Compute_S(b3DiagMat33& out)
|
||||
{
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
out[i].SetIdentity();
|
||||
|
||||
if (m_types[i] == b3MassType::e_staticMass)
|
||||
{
|
||||
out[i].SetZero();
|
||||
continue;
|
||||
}
|
||||
|
||||
b3MassContact* c = m_contacts + i;
|
||||
|
||||
if (c->lockN == true)
|
||||
{
|
||||
b3Vec3 n = c->n;
|
||||
|
||||
b3Mat33 S = b3Mat33_identity - b3Outer(n, n);
|
||||
|
||||
if (c->lockT1 == true && c->lockT2 == true)
|
||||
{
|
||||
S.SetZero();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (c->lockT1 == true)
|
||||
{
|
||||
b3Vec3 t1 = c->t1;
|
||||
|
||||
S -= b3Outer(t1, t1);
|
||||
}
|
||||
|
||||
if (c->lockT2 == true)
|
||||
{
|
||||
b3Vec3 t2 = c->t2;
|
||||
|
||||
S -= b3Outer(t2, t2);
|
||||
}
|
||||
}
|
||||
|
||||
out[i] = S;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void b3SpringSolver::Solve(b3DenseVec3& x, b3DenseVec3& f, u32& iterations,
|
||||
const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const
|
||||
{
|
||||
// P = diag(A)
|
||||
b3DiagMat33 inv_P(m_massCount);
|
||||
A.AssembleDiagonal(inv_P);
|
||||
|
||||
// Invert
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
b3Mat33& D = inv_P[i];
|
||||
|
||||
// Sylvester Criterion to ensure PD-ness
|
||||
B3_ASSERT(b3Det(D.x, D.y, D.z) > B3_EPSILON);
|
||||
|
||||
B3_ASSERT(D.x.x != 0.0f);
|
||||
float32 xx = 1.0f / D.x.x;
|
||||
|
||||
B3_ASSERT(D.y.y != 0.0f);
|
||||
float32 yy = 1.0f / D.y.y;
|
||||
|
||||
B3_ASSERT(D.z.z != 0.0f);
|
||||
float32 zz = 1.0f / D.z.z;
|
||||
|
||||
D = b3Diagonal(xx, yy, zz);
|
||||
}
|
||||
|
||||
// I
|
||||
b3DiagMat33 I(m_massCount);
|
||||
I.SetIdentity();
|
||||
|
||||
// x = S * y + (I - S) * z
|
||||
x = (S * y) + (I - S) * z;
|
||||
|
||||
// b^ = S * (b - A * (I - S) * z)
|
||||
b3DenseVec3 b_hat = S * (b - A * ((I - S) * z));
|
||||
|
||||
// b_delta = dot(b^, P^-1 * b_^)
|
||||
float32 b_delta = b3Dot(b_hat, inv_P * b_hat);
|
||||
|
||||
// r = S * (b - A * x)
|
||||
b3DenseVec3 r = S * (b - A * x);
|
||||
|
||||
// p = S * (P^-1 * r)
|
||||
b3DenseVec3 p = S * (inv_P * r);
|
||||
|
||||
// deltaNew = dot(r, p)
|
||||
float32 deltaNew = b3Dot(r, p);
|
||||
|
||||
// Tolerance.
|
||||
// This is the main stopping criteria.
|
||||
// [0, 1]
|
||||
const float32 tolerance = 10.0f * B3_EPSILON;
|
||||
|
||||
// Maximum number of iterations.
|
||||
const u32 maxIters = 1000;
|
||||
|
||||
// Main iteration loop.
|
||||
u32 iter = 0;
|
||||
while (deltaNew > tolerance * tolerance * b_delta && iter < maxIters)
|
||||
{
|
||||
// s = S * (A * p)
|
||||
b3DenseVec3 s = S * (A * p);
|
||||
|
||||
// alpha = deltaNew / dot(c, q)
|
||||
float32 alpha = deltaNew / b3Dot(p, s);
|
||||
|
||||
// x = x + alpha * p
|
||||
x = x + alpha * p;
|
||||
|
||||
// r = r - alpha * s
|
||||
r = r - alpha * s;
|
||||
|
||||
// h = inv_P * r
|
||||
b3DenseVec3 h = inv_P * r;
|
||||
|
||||
// deltaOld = deltaNew
|
||||
float32 deltaOld = deltaNew;
|
||||
|
||||
// deltaNew = dot(r, h)
|
||||
deltaNew = b3Dot(r, h);
|
||||
//B3_ASSERT(b3IsValid(deltaNew));
|
||||
|
||||
// beta = deltaNew / deltaOld
|
||||
float32 beta = deltaNew / deltaOld;
|
||||
|
||||
// p = S * (h + beta * p)
|
||||
p = S * (h + beta * p);
|
||||
|
||||
++iter;
|
||||
}
|
||||
|
||||
iterations = iter;
|
||||
|
||||
float32 x2 = b3Dot(x, x);
|
||||
if (b3IsValid(x2) == false)
|
||||
{
|
||||
// Probably the condition number of A is large.
|
||||
// Solve unconstrained (S = I)?
|
||||
|
||||
// Reject the solution.
|
||||
x.SetZero();
|
||||
f.SetZero();
|
||||
}
|
||||
else
|
||||
{
|
||||
// Accept the solution.
|
||||
// Residual
|
||||
f = A * x - b;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user