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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

243
examples/testbed/tests/cloth_test.h
View File

@ -25,6 +25,7 @@ public:
ClothDragger(Ray3* ray, b3Cloth*& cloth) : m_ray(ray), m_cloth(cloth)
{
m_isSelected = false;
m_spring = false;
}
~ClothDragger()
@ -61,31 +62,32 @@ public:
{
B3_ASSERT(m_isSelected == false);
if (Select(m_selection, m_x) == false)
b3RayCastInput rayIn;
rayIn.p1 = m_ray->A();
rayIn.p2 = m_ray->B();
rayIn.maxFraction = B3_MAX_FLOAT;
b3ClothRayCastOutput rayOut;
if (m_cloth->RayCast(&rayOut, &rayIn) == false)
{
return false;
}
m_isSelected = true;
m_selection = rayOut.triangle;
m_x = rayOut.fraction;
b3ClothMesh* m = m_cloth->GetMesh();
b3ClothMeshTriangle* t = m->triangles + m_selection;
b3Particle* p1 = m_cloth->GetParticle(t->v1);
m_t1 = p1->type;
m_cloth->SetType(p1, e_staticParticle);
b3Particle* p1 = m->particles[t->v1];
b3Particle* p2 = m->particles[t->v2];
b3Particle* p3 = m->particles[t->v3];
b3Particle* p2 = m_cloth->GetParticle(t->v2);
m_t2 = p2->type;
m_cloth->SetType(p2, e_staticParticle);
b3Particle* p3 = m_cloth->GetParticle(t->v3);
m_t3 = p3->type;
m_cloth->SetType(p3, e_staticParticle);
b3Vec3 v1 = p1->position;
b3Vec3 v2 = p2->position;
b3Vec3 v3 = p3->position;
b3Vec3 v1 = p1->GetPosition();
b3Vec3 v2 = p2->GetPosition();
b3Vec3 v3 = p3->GetPosition();
b3Vec3 B = GetPointB();
@ -102,6 +104,53 @@ public:
m_u = m_v = 0.0f;
}
if (m_spring)
{
b3ParticleDef pd;
pd.type = e_staticParticle;
pd.position = B;
m_particle = m_cloth->CreateParticle(pd);
{
b3SpringForceDef sfd;
sfd.p1 = m_particle;
sfd.p2 = p1;
sfd.restLength = 0.0f;
sfd.structural = 10000.0f;
m_s1 = (b3SpringForce*)m_cloth->CreateForce(sfd);
}
{
b3SpringForceDef sfd;
sfd.p1 = m_particle;
sfd.p2 = p2;
sfd.restLength = 0.0f;
sfd.structural = 10000.0f;
m_s2 = (b3SpringForce*)m_cloth->CreateForce(sfd);
}
{
b3SpringForceDef sfd;
sfd.p1 = m_particle;
sfd.p2 = p3;
sfd.restLength = 0.0f;
sfd.structural = 10000.0f;
m_s3 = (b3SpringForce*)m_cloth->CreateForce(sfd);
}
}
else
{
m_t1 = p1->GetType();
p1->SetType(e_staticParticle);
m_t2 = p2->GetType();
p2->SetType(e_staticParticle);
m_t3 = p3->GetType();
p3->SetType(e_staticParticle);
}
return true;
}
@ -117,14 +166,21 @@ public:
b3Vec3 dx = B - A;
b3Particle* p1 = m_cloth->GetParticle(t->v1);
m_cloth->ApplyTranslation(p1, dx);
if (m_spring)
{
m_particle->ApplyTranslation(dx);
}
else
{
b3Particle* p1 = m->particles[t->v1];
p1->ApplyTranslation(dx);
b3Particle* p2 = m_cloth->GetParticle(t->v2);
m_cloth->ApplyTranslation(p2, dx);
b3Particle* p2 = m->particles[t->v2];
p2->ApplyTranslation(dx);
b3Particle* p3 = m_cloth->GetParticle(t->v3);
m_cloth->ApplyTranslation(p3, dx);
b3Particle* p3 = m->particles[t->v3];
p3->ApplyTranslation(dx);
}
}
void StopDragging()
@ -133,141 +189,30 @@ public:
m_isSelected = false;
b3ClothMesh* m = m_cloth->GetMesh();
b3ClothMeshTriangle* t = m->triangles + m_selection;
b3Particle* p1 = m_cloth->GetParticle(t->v1);
m_cloth->SetType(p1, m_t1);
b3Particle* p2 = m_cloth->GetParticle(t->v2);
m_cloth->SetType(p2, m_t2);
b3Particle* p3 = m_cloth->GetParticle(t->v3);
m_cloth->SetType(p3, m_t3);
}
private:
bool RayCast(b3RayCastOutput* output, u32 triangleIndex) const
if (m_spring)
{
b3ClothMesh* mesh = m_cloth->GetMesh();
B3_ASSERT(triangleIndex < mesh->triangleCount);
b3ClothMeshTriangle* triangle = mesh->triangles + triangleIndex;
b3Vec3 v1 = mesh->vertices[triangle->v1];
b3Vec3 v2 = mesh->vertices[triangle->v2];
b3Vec3 v3 = mesh->vertices[triangle->v3];
b3Vec3 p1 = m_ray->A();
b3Vec3 p2 = m_ray->B();
b3Vec3 d = p2 - p1;
float32 maxFraction = b3Length(d);
b3Vec3 n = b3Cross(v2 - v1, v3 - v1);
n.Normalize();
float32 numerator = b3Dot(n, v1 - p1);
float32 denominator = b3Dot(n, d);
if (denominator == 0.0f)
{
return false;
}
float32 t = numerator / denominator;
// Is the intersection not on the segment?
if (t < 0.0f || maxFraction < t)
{
return false;
}
b3Vec3 q = p1 + t * d;
// Barycentric coordinates for q
b3Vec3 Q = q;
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);
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->fraction = t;
// Does the ray start from below or above the triangle?
if (numerator > 0.0f)
{
output->normal = -n;
m_cloth->DestroyForce(m_s1);
m_cloth->DestroyForce(m_s2);
m_cloth->DestroyForce(m_s3);
m_cloth->DestroyParticle(m_particle);
}
else
{
output->normal = n;
}
return true;
}
return false;
}
bool Select(u32& selection, float32& fraction) const
{
b3ClothMesh* m = m_cloth->GetMesh();
b3ClothMeshTriangle* t = m->triangles + m_selection;
b3RayCastInput input;
input.p1 = m_ray->A();
input.p2 = m_ray->B();
input.maxFraction = m_ray->fraction;
b3Particle* p1 = m->particles[t->v1];
p1->SetType(m_t1);
float32 minFraction = B3_MAX_FLOAT;
b3Vec3 minNormal(0.0f, 0.0f, 0.0f);
u32 minIndex = ~0;
b3Particle* p2 = m->particles[t->v2];
p2->SetType(m_t2);
for (u32 i = 0; i < m->triangleCount; ++i)
{
b3RayCastOutput subOutput;
if (RayCast(&subOutput, i) == true)
{
if (subOutput.fraction < minFraction)
{
minFraction = subOutput.fraction;
minNormal = subOutput.normal;
minIndex = i;
}
b3Particle* p3 = m->particles[t->v3];
p3->SetType(m_t3);
}
}
if (minIndex != ~0)
{
selection = minIndex;
fraction = minFraction;
return true;
}
return false;
}
private:
bool m_isSelected;
Ray3* m_ray;
@ -276,6 +221,14 @@ private:
b3Cloth*& m_cloth;
u32 m_selection;
float32 m_u, m_v;
bool m_spring;
b3Particle* m_particle;
b3SpringForce* m_s1;
b3SpringForce* m_s2;
b3SpringForce* m_s3;
b3ParticleType m_t1, m_t2, m_t3;
};

23
examples/testbed/tests/particle_types.h
View File

@ -38,10 +38,9 @@ public:
void SetClothType(b3ParticleType type)
{
for (u32 i = 0; i < m_cloth->GetParticleCount(); ++i)
for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext())
{
b3Particle* p = m_cloth->GetParticle(i);
m_cloth->SetType(p, type);
p->SetType(type);
}
}
@ -62,10 +61,8 @@ public:
SetClothType(e_dynamicParticle);
}
for (u32 i = 0; i < m_cloth->GetParticleCount(); ++i)
for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext())
{
b3Particle* p = m_cloth->GetParticle(i);
b3Vec3 d;
d.SetZero();
@ -94,25 +91,25 @@ public:
button == GLFW_KEY_UP ||
button == GLFW_KEY_DOWN)
{
if (p->type == e_staticParticle)
if (p->GetType() == e_staticParticle)
{
m_cloth->ApplyTranslation(p, d);
p->ApplyTranslation(d);
}
if (p->type == e_kinematicParticle)
if (p->GetType() == e_kinematicParticle)
{
b3Vec3 v = p->velocity;
b3Vec3 v = p->GetVelocity();
v += 5.0f * d;
m_cloth->SetVelocity(p, d);
p->SetVelocity(v);
}
if (p->type == e_dynamicParticle)
if (p->GetType() == e_dynamicParticle)
{
b3Vec3 f = 100.0f * d;
m_cloth->ApplyForce(p, f);
p->ApplyForce(f);
}
}
}

48
examples/testbed/tests/pinned_cloth.h
View File

@ -22,32 +22,27 @@
class PinnedCloth : public ClothTest
{
public:
PinnedCloth()
PinnedCloth() : m_rectangleGarment(5.0f, 5.0f)
{
m_gridClothMesh.vertexCount = m_gridMesh.vertexCount;
m_gridClothMesh.vertices = m_gridMesh.vertices;
// Generate 2D mesh
m_rectangleGarmentMesh.Set(&m_rectangleGarment, 1.0f);
m_gridClothMesh.triangleCount = m_gridMesh.triangleCount;
m_gridClothMesh.triangles = (b3ClothMeshTriangle*)m_gridMesh.triangles;
// Create 3D mesh
m_rectangleClothMesh.Set(&m_rectangleGarmentMesh);
m_gridClothMeshMesh.vertexCount = m_gridClothMesh.vertexCount;
m_gridClothMeshMesh.startVertex = 0;
m_gridClothMeshMesh.triangleCount = m_gridClothMesh.triangleCount;
m_gridClothMeshMesh.startTriangle = 0;
m_gridClothMesh.meshCount = 1;
m_gridClothMesh.meshes = &m_gridClothMeshMesh;
m_gridClothMesh.sewingLineCount = 0;
m_gridClothMesh.sewingLines = nullptr;
//
b3Mat33 dq = b3Mat33RotationX(0.5f * B3_PI);
for (u32 i = 0; i < m_rectangleClothMesh.vertexCount; ++i)
{
m_rectangleClothMesh.vertices[i] = dq * m_rectangleClothMesh.vertices[i];
}
b3ClothDef def;
def.mesh = &m_gridClothMesh;
def.mesh = &m_rectangleClothMesh;
def.radius = 0.05f;
def.density = 0.2f;
def.ks = 10000.0f;
def.kd = 0.0f;
def.r = 0.05f;
def.structural = 10000.0f;
def.damping = 0.0f;
m_cloth = m_world.CreateCloth(def);
@ -55,12 +50,11 @@ public:
aabb.m_lower.Set(-5.0f, -1.0f, -6.0f);
aabb.m_upper.Set(5.0f, 1.0f, -4.0f);
for (u32 i = 0; i < m_cloth->GetParticleCount(); ++i)
for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext())
{
b3Particle* p = m_cloth->GetParticle(i);
if (aabb.Contains(p->position))
if (aabb.Contains(p->GetPosition()))
{
m_cloth->SetType(p, e_staticParticle);
p->SetType(e_staticParticle);
}
}
}
@ -70,9 +64,9 @@ public:
return new PinnedCloth();
}
b3GridMesh<10, 10> m_gridMesh;
b3ClothMeshMesh m_gridClothMeshMesh;
b3ClothMesh m_gridClothMesh;
b3RectangleGarment m_rectangleGarment;
b3GarmentMesh m_rectangleGarmentMesh;
b3GarmentClothMesh m_rectangleClothMesh;
};
#endif

8
examples/testbed/tests/shirt.h
View File

@ -19,10 +19,6 @@
#ifndef SHIRT_H
#define SHIRT_H
#include <bounce/garment/garment.h>
#include <bounce/garment/sewing_pattern.h>
#include <bounce/garment/garment_mesh.h>
class Shirt : public ClothTest
{
public:
@ -57,10 +53,10 @@ public:
// Create cloth
b3ClothDef def;
def.radius = 0.2f;
def.mesh = &m_shirtClothMesh;
def.density = 0.2f;
def.r = 0.2f;
def.ks = 10000.0f;
def.structural = 10000.0f;
m_cloth = m_world.CreateCloth(def);
}

49
examples/testbed/tests/table_cloth.h
View File

@ -22,38 +22,28 @@
class TableCloth : public ClothTest
{
public:
TableCloth()
TableCloth() : m_rectangleGarment(5.0f, 5.0f)
{
// Shift vertices up
for (u32 i = 0; i < m_gridMesh.vertexCount; ++i)
// Generate 2D mesh
m_rectangleGarmentMesh.Set(&m_rectangleGarment, 1.0f);
// Create 3D mesh
m_rectangleClothMesh.Set(&m_rectangleGarmentMesh);
//
b3Mat33 dq = b3Mat33RotationX(0.5f * B3_PI);
for (u32 i = 0; i < m_rectangleClothMesh.vertexCount; ++i)
{
m_gridMesh.vertices[i].y = 5.0f;
m_rectangleClothMesh.vertices[i] = dq * m_rectangleClothMesh.vertices[i];
m_rectangleClothMesh.vertices[i].y += 5.0f;
}
m_gridClothMesh.vertexCount = m_gridMesh.vertexCount;
m_gridClothMesh.vertices = m_gridMesh.vertices;
m_gridClothMesh.triangleCount = m_gridMesh.triangleCount;
m_gridClothMesh.triangles = (b3ClothMeshTriangle*)m_gridMesh.triangles;
m_gridClothMeshMesh.vertexCount = m_gridClothMesh.vertexCount;
m_gridClothMeshMesh.startVertex = 0;
m_gridClothMeshMesh.triangleCount = m_gridClothMesh.triangleCount;
m_gridClothMeshMesh.startTriangle = 0;
m_gridClothMesh.meshCount = 1;
m_gridClothMesh.meshes = &m_gridClothMeshMesh;
m_gridClothMesh.sewingLineCount = 0;
m_gridClothMesh.sewingLines = nullptr;
b3ClothDef def;
def.mesh = &m_gridClothMesh;
def.mesh = &m_rectangleClothMesh;
def.radius = 0.05f;
def.density = 0.2f;
def.ks = 10000.0f;
def.kd = 0.0f;
def.r = 0.05f;
def.structural = 10000.0f;
def.damping = 0.0f;
m_cloth = m_world.CreateCloth(def);
@ -87,10 +77,9 @@ public:
return new TableCloth();
}
//b3GridMesh<2, 2> m_gridMesh;
b3GridMesh<10, 10> m_gridMesh;
b3ClothMeshMesh m_gridClothMeshMesh;
b3ClothMesh m_gridClothMesh;
b3RectangleGarment m_rectangleGarment;
b3GarmentMesh m_rectangleGarmentMesh;
b3GarmentClothMesh m_rectangleClothMesh;
b3QHull m_tableHull;
};

85
examples/testbed/tests/tension_mapping.h
View File

@ -58,32 +58,26 @@ static inline b3Color Color(float32 x, float32 a, float32 b)
class TensionMapping : public ClothTest
{
public:
TensionMapping()
TensionMapping() : m_rectangleGarment(5.0f, 5.0f)
{
m_gridClothMesh.vertexCount = m_gridMesh.vertexCount;
m_gridClothMesh.vertices = m_gridMesh.vertices;
// Generate 2D mesh
m_rectangleGarmentMesh.Set(&m_rectangleGarment, 1.0f);
m_gridClothMesh.triangleCount = m_gridMesh.triangleCount;
m_gridClothMesh.triangles = (b3ClothMeshTriangle*)m_gridMesh.triangles;
// Create 3D mesh
m_rectangleClothMesh.Set(&m_rectangleGarmentMesh);
m_gridClothMeshMesh.vertexCount = m_gridClothMesh.vertexCount;
m_gridClothMeshMesh.startVertex = 0;
m_gridClothMeshMesh.triangleCount = m_gridClothMesh.triangleCount;
m_gridClothMeshMesh.startTriangle = 0;
m_gridClothMesh.meshCount = 1;
m_gridClothMesh.meshes = &m_gridClothMeshMesh;
m_gridClothMesh.sewingLineCount = 0;
m_gridClothMesh.sewingLines = nullptr;
//
b3Mat33 dq = b3Mat33RotationX(0.5f * B3_PI);
for (u32 i = 0; i < m_rectangleClothMesh.vertexCount; ++i)
{
m_rectangleClothMesh.vertices[i] = dq * m_rectangleClothMesh.vertices[i];
}
b3ClothDef def;
def.mesh = &m_gridClothMesh;
def.mesh = &m_rectangleClothMesh;
def.radius = 0.2f;
def.density = 0.2f;
def.ks = 10000.0f;
def.kd = 0.0f;
def.r = 0.2f;
def.structural = 10000.0f;
m_cloth = m_world.CreateCloth(def);
@ -91,12 +85,11 @@ public:
aabb.m_lower.Set(-5.0f, -1.0f, -6.0f);
aabb.m_upper.Set(5.0f, 1.0f, -4.0f);
for (u32 i = 0; i < m_cloth->GetParticleCount(); ++i)
for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext())
{
b3Particle* p = m_cloth->GetParticle(i);
if (aabb.Contains(p->position))
if (aabb.Contains(p->GetPosition()))
{
m_cloth->SetType(p, e_staticParticle);
p->SetType(e_staticParticle);
}
}
}
@ -107,38 +100,36 @@ public:
m_cloth->Apply();
b3ClothMesh* mesh = m_cloth->GetMesh();
b3StackArray<b3Vec3, 256> tension;
tension.Resize(m_cloth->GetParticleCount());
for (u32 i = 0; i < tension.Count(); ++i)
tension.Resize(mesh->vertexCount);
for (u32 i = 0; i < mesh->vertexCount; ++i)
{
tension[i].SetZero();
}
for (u32 i = 0; i < m_cloth->GetSpringCount(); ++i)
for (b3Force* f = m_cloth->GetForceList().m_head; f; f = f->GetNext())
{
b3Spring* s = m_cloth->GetSpring(i);
if (f->GetType() == e_springForce)
{
b3SpringForce* s = (b3SpringForce*)f;
b3Particle* p1 = s->p1;
b3Particle* p2 = s->p2;
u32 v1 = s->GetParticle1()->GetVertex();
u32 v2 = s->GetParticle2()->GetVertex();
u32 i1 = m_cloth->GetParticleIndex(p1);
u32 i2 = m_cloth->GetParticleIndex(p2);
tension[i1] += s->f;
tension[i2] -= s->f;
tension[v1] += s->GetActionForce();
tension[v2] -= s->GetActionForce();
}
}
for (u32 i = 0; i < m_gridClothMesh.triangleCount; ++i)
for (u32 i = 0; i < m_rectangleClothMesh.triangleCount; ++i)
{
b3ClothMeshTriangle* t = m_gridClothMesh.triangles + i;
b3ClothMeshTriangle* t = m_rectangleClothMesh.triangles + i;
b3Particle* p1 = m_cloth->GetParticle(t->v1);
b3Particle* p2 = m_cloth->GetParticle(t->v2);
b3Particle* p3 = m_cloth->GetParticle(t->v3);
b3Vec3 v1 = p1->position;
b3Vec3 v2 = p2->position;
b3Vec3 v3 = p3->position;
b3Vec3 v1 = mesh->vertices[t->v1];
b3Vec3 v2 = mesh->vertices[t->v2];
b3Vec3 v3 = mesh->vertices[t->v3];
b3Draw_draw->DrawSegment(v1, v2, b3Color_black);
b3Draw_draw->DrawSegment(v2, v3, b3Color_black);
@ -183,9 +174,9 @@ public:
return new TensionMapping();
}
b3GridMesh<10, 10> m_gridMesh;
b3ClothMeshMesh m_gridClothMeshMesh;
b3ClothMesh m_gridClothMesh;
b3RectangleGarment m_rectangleGarment;
b3GarmentMesh m_rectangleGarmentMesh;
b3GarmentClothMesh m_rectangleClothMesh;
};
#endif

13
include/bounce/bounce.h
View File

@ -58,18 +58,17 @@
#include <bounce/dynamics/rope/rope.h>
#include <bounce/garment/sewing_pattern.h>
#include <bounce/garment/garment.h>
#include <bounce/garment/garment_mesh.h>
#include <bounce/dynamics/cloth/cloth_mesh.h>
#include <bounce/dynamics/cloth/cloth.h>
#include <bounce/dynamics/cloth/particle.h>
#include <bounce/dynamics/cloth/spring_force.h>
#include <bounce/dynamics/body.h>
//#include <bounce/dynamics/tree/joints/tree_weld_joint.h>
//#include <bounce/dynamics/tree/joints/tree_prismatic_joint.h>
//#include <bounce/dynamics/tree/joints/tree_revolute_joint.h>
//#include <bounce/dynamics/tree/joints/tree_spherical_joint.h>
//#include <bounce/dynamics/tree/tree_body.h>
//#include <bounce/dynamics/tree/body_tree.h>
#include <bounce/dynamics/world.h>
#include <bounce/dynamics/world_listeners.h>

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

@ -21,153 +21,66 @@
#include <bounce/common/math/transform.h>
#include <bounce/common/template/list.h>
#include <bounce/common/memory/block_pool.h>
class b3StackAllocator;
class b3World;
class b3Shape;
class b3Particle;
class b3Force;
struct b3ParticleDef;
struct b3ForceDef;
struct b3ClothMesh;
// Cloth mesh definition
struct b3RayCastInput;
struct b3ClothRayCastOutput
{
u32 triangle; // intersected triangle
b3Vec3 point; // intersection point on surface
b3Vec3 normal; // surface normal of intersection
float32 fraction; // time of intersection on segment
};
// Cloth definition
// This requires defining a cloth mesh which is typically bound to a render mesh
struct b3ClothDef
{
b3ClothDef()
{
mesh = nullptr;
density = 0.0f;
r = 0.05f;
ks = 0.0f;
kb = 0.0f;
kd = 0.0f;
radius = 0.05f;
structural = 0.0f;
bending = 0.0f;
damping = 0.0f;
}
// Cloth proxy mesh
// Cloth mesh
b3ClothMesh* mesh;
// Radius
// This should be a small value. It can be used for correcting visual artifacts when
// the masses are colliding against a solid.
float32 r;
float32 radius;
// Cloth density in kg/m^3
float32 density;
// Streching stiffness
float32 ks;
// Structural stiffness
float32 structural;
// Bending stiffness
float32 kb;
float32 bending;
// Damping stiffness
float32 kd;
float32 damping;
};
// Static particle: Has zero mass, can be moved manually.
// Kinematic particle: Has zero mass, non-zero velocity, can be moved by the solver.
// Dynamic particle: Has non-zero mass, non-zero velocity determined by force, can be moved by the solver.
enum b3ParticleType
{
e_staticParticle,
e_kinematicParticle,
e_dynamicParticle
};
// Read-only particle
struct b3Particle
{
// Type
b3ParticleType type;
// Position
b3Vec3 position;
// Velocity
b3Vec3 velocity;
// Applied external force
b3Vec3 force;
// Mass
float32 mass;
// Inverse mass
float32 invMass;
// Radius
float32 radius;
// User data.
void* userData;
// Applied external translation
b3Vec3 translation;
// Solver temp
// Identifier
u32 solverId;
// Solution
b3Vec3 x;
};
// Spring types
enum b3SpringType
{
e_strechSpring,
e_bendSpring,
};
struct b3ClothSolverData;
// Read-only spring
struct b3Spring
{
// Solver shared
// Spring type
b3SpringType type;
// Particle 1
b3Particle* p1;
// Particle 2
b3Particle* p2;
// Rest length
float32 L0;
// Structural stiffness
float32 ks;
// Damping stiffness
float32 kd;
// Solver temp
// Force (f_i entry)
b3Vec3 f;
// Jacobian (J_ii entry)
b3Mat33 Jx, Jv;
// Initialize forces and its derivatives.
void InitializeForces(const b3ClothSolverData* data);
};
// Read-only body contact between a particle and a solid
struct b3BodyContact
{
b3Particle* p1;
b3Shape* s2;
float32 s;
b3Vec3 n, t1, t2;
float32 Fn, Ft1, Ft2;
bool n_active, t1_active, t2_active;
};
// A cloth represents a deformable surface/mesh.
// b3Cloth simulates this surface motion using particles and springs.
// A cloth represents a deformable surface as a collection of particles.
// Particles may be connected with each other.
class b3Cloth
{
public:
@ -175,36 +88,32 @@ public:
const b3World* GetWorld() const;
b3World* GetWorld();
// Return the cloth mesh used to initialize this cloth.
// Create a particle.
b3Particle* CreateParticle(const b3ParticleDef& def);
// Destroy a particle
void DestroyParticle(b3Particle* particle);
// Create a force.
b3Force* CreateForce(const b3ForceDef& def);
// Destroy a force.
void DestroyForce(b3Force* force);
// Perform a ray cast with the cloth.
bool RayCast(b3ClothRayCastOutput* output, const b3RayCastInput* input) const;
// Perform a ray cast with a given cloth mesh triangle.
bool RayCast(b3ClothRayCastOutput* output, const b3RayCastInput* input, u32 triangleIndex) const;
// Return the cloth mesh proxy.
b3ClothMesh* GetMesh() const;
// Return the number of particles in this cloth.
u32 GetParticleCount() const;
// Return the list of particles in this cloth.
const b3List2<b3Particle>& GetParticleList() const;
// Return the particle at a given index in this cloth.
b3Particle* GetParticle(u32 i) const;
// Convenience function.
// Return the index of a given particle.
u32 GetParticleIndex(const b3Particle* p) const;
// Set the type of a given particle.
void SetType(b3Particle* p, b3ParticleType type);
// Set the velocity of a given particle.
void SetVelocity(b3Particle* p, const b3Vec3& velocity);
// Apply a force to a given particle.
void ApplyForce(b3Particle* p, const b3Vec3& force);
// Apply a translation to a given particle.
void ApplyTranslation(b3Particle* p, const b3Vec3& translation);
// Return the number of springs in this cloth.
u32 GetSpringCount() const;
// Return the spring at a given index in this cloth.
b3Spring* GetSpring(u32 i) const;
// Return the list of forces in this cloth.
const b3List2<b3Force>& GetForceList() const;
// Return the kinetic (or dynamic) energy in this system.
float32 GetEnergy() const;
@ -228,11 +137,12 @@ private:
b3Cloth(const b3ClothDef& def, b3World* world);
~b3Cloth();
// Perform a time step. Called only inside b3World.
// Perform a time step.
// Called only inside b3World.
void Step(float32 dt, const b3Vec3& gravity);
// Compute mass of each particle.
void ResetMass();
void ComputeMass();
// Update contacts.
// This is where some contacts might be initiated or terminated.
@ -241,19 +151,18 @@ private:
// Solve
void Solve(float32 dt, const b3Vec3& gravity);
b3StackAllocator* m_allocator;
// Proxy mesh
b3ClothMesh* m_mesh;
float32 m_density;
u32 m_particleCount;
b3Particle* m_particles;
// Particle pool
b3BlockPool m_particleBlocks;
u32 m_springCount;
b3Spring* m_springs;
// List of particles
b3List2<b3Particle> m_particleList;
b3BodyContact* m_contacts;
//u32 m_contactCount;
// List of forces
b3List2<b3Force> m_forceList;
// The parent world of this cloth.
b3World* m_world;
@ -278,87 +187,14 @@ inline b3ClothMesh* b3Cloth::GetMesh() const
return m_mesh;
}
inline u32 b3Cloth::GetParticleCount() const
inline const b3List2<b3Particle>& b3Cloth::GetParticleList() const
{
return m_particleCount;
return m_particleList;
}
inline b3Particle* b3Cloth::GetParticle(u32 i) const
inline const b3List2<b3Force>& b3Cloth::GetForceList() const
{
B3_ASSERT(i < m_particleCount);
return m_particles + i;
}
inline u32 b3Cloth::GetParticleIndex(const b3Particle* p) const
{
return u32(p - m_particles);
}
inline void b3Cloth::SetType(b3Particle* p, b3ParticleType type)
{
if (p->type == type)
{
return;
}
p->type = type;
p->force.SetZero();
if (type == e_staticParticle)
{
p->velocity.SetZero();
p->translation.SetZero();
u32 ip = u32(p - m_particles);
m_contacts[ip].n_active = false;
m_contacts[ip].t1_active = false;
m_contacts[ip].t2_active = false;
}
}
inline void b3Cloth::SetVelocity(b3Particle* p, const b3Vec3& velocity)
{
if (p->type == e_staticParticle)
{
return;
}
p->velocity = velocity;
}
inline void b3Cloth::ApplyForce(b3Particle* p, const b3Vec3& force)
{
if (p->type != e_dynamicParticle)
{
return;
}
p->force += force;
}
inline void b3Cloth::ApplyTranslation(b3Particle* p, const b3Vec3& translation)
{
p->translation += translation;
}
inline u32 b3Cloth::GetSpringCount() const
{
return m_springCount;
}
inline b3Spring* b3Cloth::GetSpring(u32 i) const
{
B3_ASSERT(i < m_springCount);
return m_springs + i;
}
inline float32 b3Cloth::GetEnergy() const
{
float32 E = 0.0f;
for (u32 i = 0; i < m_particleCount; ++i)
{
E += m_particles[i].mass * b3Dot(m_particles[i].velocity, m_particles[i].velocity);
}
return 0.5f * E;
return m_forceList;
}
inline const b3Cloth* b3Cloth::GetNext() const

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

@ -41,10 +41,13 @@ struct b3ClothMeshSewingLine
u32 v1, v2;
};
class b3Particle;
struct b3ClothMesh
{
u32 vertexCount;
b3Vec3* vertices;
b3Particle** particles;
u32 triangleCount;
b3ClothMeshTriangle* triangles;
u32 meshCount;
@ -61,8 +64,7 @@ struct b3GarmentClothMesh : public b3ClothMesh
b3GarmentClothMesh();
~b3GarmentClothMesh();
// Maps a given garment mesh to this mesh.
// This garment mesh must be empty.
// Set this mesh from a 2D garment mesh.
void Set(const b3GarmentMesh* garment);
};

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

@ -22,23 +22,22 @@
#include <bounce/common/math/vec3.h>
#include <bounce/common/math/mat33.h>
class b3StackAllocator;
struct b3DenseVec3;
struct b3DiagMat33;
struct b3SparseMat33;
struct b3SolverSparseMat33;
struct b3SparseSymMat33;
struct b3SymMat33;
struct b3Particle;
struct b3Spring;
class b3Particle;
class b3Force;
struct b3BodyContact;
class b3Shape;
class b3StackAllocator;
struct b3ClothSolverDef
{
b3StackAllocator* stack;
u32 particleCapacity;
u32 springCapacity;
u32 forceCapacity;
u32 contactCapacity;
};
@ -47,17 +46,12 @@ struct b3ClothSolverData
b3Vec3* x;
b3Vec3* v;
b3Vec3* f;
b3SymMat33* dfdx;
b3SymMat33* dfdv;
float32 dt;
float32 invdt;
};
struct b3SpringForce
{
u32 i1, i2;
b3Vec3 f;
b3Mat33 Jx, Jv;
};
struct b3AccelerationConstraint
{
u32 i1;
@ -65,6 +59,45 @@ struct b3AccelerationConstraint
b3Vec3 p, q, z;
};
struct b3SymMat33
{
b3SymMat33(b3StackAllocator* a, u32 m, u32 n)
{
allocator = a;
M = m;
N = n;
values = (b3Mat33*)b3Alloc(M * N * sizeof(b3Mat33));
}
~b3SymMat33()
{
b3Free(values);
}
b3Mat33& operator()(u32 i, u32 j)
{
return values[i * N + j];
}
const b3Mat33& operator()(u32 i, u32 j) const
{
return values[i * N + j];
}
void SetZero()
{
for (u32 v = 0; v < M * N; ++v)
{
values[v].SetZero();
}
}
u32 M;
u32 N;
b3Mat33* values;
b3StackAllocator* allocator;
};
class b3ClothSolver
{
public:
@ -72,7 +105,7 @@ public:
~b3ClothSolver();
void Add(b3Particle* p);
void Add(b3Spring* s);
void Add(b3Force* f);
void Add(b3BodyContact* c);
void Solve(float32 dt, const b3Vec3& gravity);
@ -80,17 +113,20 @@ private:
// Initialize forces.
void InitializeForces();
// Apply forces.
void ApplyForces();
// Initialize constraints.
void InitializeConstraints();
// Compute A and b in Ax = b
void Compute_A_b(b3SolverSparseMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const;
void Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const;
// Compute S and z.
void Compute_S_z(b3DiagMat33& S, b3DenseVec3& z);
// Solve Ax = b.
void Solve(b3DenseVec3& x, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const;
void Solve(b3DenseVec3& x, u32& iterations, const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const;
b3StackAllocator* m_allocator;
@ -98,9 +134,9 @@ private:
u32 m_particleCount;
b3Particle** m_particles;
u32 m_springCapacity;
u32 m_springCount;
b3Spring** m_springs;
u32 m_forceCapacity;
u32 m_forceCount;
b3Force** m_forces;
u32 m_contactCapacity;
u32 m_contactCount;

87
include/bounce/dynamics/cloth/force.h Normal file
View File

@ -0,0 +1,87 @@
/*
* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_FORCE_H
#define B3_FORCE_H
#include <bounce/common/math/transform.h>
#include <bounce/common/template/list.h>
struct b3ClothSolverData;
class b3Particle;
// Force types
enum b3ForceType
{
e_springForce,
};
struct b3ForceDef
{
b3ForceType type;
};
//
class b3Force
{
public:
//
b3ForceType GetType() const;
//
b3Force* GetNext();
protected:
friend class b3List2<b3Force>;
friend class b3Cloth;
friend class b3ClothSolver;
friend class b3Particle;
friend class b3Force;
static b3Force* Create(const b3ForceDef* def);
static void Destroy(b3Force* f);
b3Force() { }
virtual ~b3Force() { }
virtual void Initialize(const b3ClothSolverData* data) = 0;
virtual void Apply(const b3ClothSolverData* data) = 0;
// Solver shared
// Force type
b3ForceType m_type;
//
b3Force* m_prev;
//
b3Force* m_next;
};
inline b3ForceType b3Force::GetType() const
{
return m_type;
}
inline b3Force* b3Force::GetNext()
{
return m_next;
}
#endif

226
include/bounce/dynamics/cloth/particle.h Normal file
View File

@ -0,0 +1,226 @@
/*
* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_PARTICLE_H
#define B3_PARTICLE_H
#include <bounce/common/math/transform.h>
#include <bounce/common/template/list.h>
class b3Shape;
class b3Cloth;
class b3Particle;
// Static particle: Can be moved manually.
// Kinematic particle: Non-zero velocity, can be moved by the solver.
// Dynamic particle: Non-zero velocity determined by force, can be moved by the solver.
enum b3ParticleType
{
e_staticParticle,
e_kinematicParticle,
e_dynamicParticle
};
//
struct b3ParticleDef
{
b3ParticleDef()
{
type = e_staticParticle;
position.SetZero();
velocity.SetZero();
force.SetZero();
radius = 0.0f;
userData = nullptr;
}
b3ParticleType type;
b3Vec3 position;
b3Vec3 velocity;
b3Vec3 force;
float32 radius;
void* userData;
};
// A contact between a particle and a solid
struct b3BodyContact
{
b3Particle* p1;
b3Shape* s2;
float32 s;
b3Vec3 n, t1, t2;
float32 Fn, Ft1, Ft2;
bool n_active, t1_active, t2_active;
};
// A cloth particle.
class b3Particle
{
public:
// Set the particle type.
void SetType(b3ParticleType type);
// Get the particle type.
b3ParticleType GetType() const;
// Get the vertex index.
u32 GetVertex() const;
// Get the particle position.
const b3Vec3& GetPosition() const;
// Set the particle velocity.
void SetVelocity(const b3Vec3& velocity);
// Get the particle velocity.
const b3Vec3& GetVelocity() const;
// Get the particle mass.
float32 GetMass() const;
// Get the particle radius;
float32 GetRadius() const;
// Apply a force.
void ApplyForce(const b3Vec3& force);
// Apply a translation.
void ApplyTranslation(const b3Vec3& translation);
// Get the next particle.
b3Particle* GetNext();
private:
friend class b3List2<b3Particle>;
friend class b3Cloth;
friend class b3ClothSolver;
friend class b3Force;
friend class b3SpringForce;
b3Particle(const b3ParticleDef& def, b3Cloth* cloth);
~b3Particle();
// Type
b3ParticleType m_type;
// Position
b3Vec3 m_position;
// Velocity
b3Vec3 m_velocity;
// Applied external force
b3Vec3 m_force;
// Mass
float32 m_mass;
// Inverse mass
float32 m_invMass;
// Radius
float32 m_radius;
// User data.
void* m_userData;
// Cloth mesh vertex index.
u32 m_vertex;
// Applied external translation
b3Vec3 m_translation;
// Contact
b3BodyContact m_contact;
// Solver temp
// Identifier
u32 m_solverId;
// Solution
b3Vec3 m_x;
//
b3Cloth* m_cloth;
//
b3Particle* m_prev;
//
b3Particle* m_next;
};
inline b3ParticleType b3Particle::GetType() const
{
return m_type;
}
inline u32 b3Particle::GetVertex() const
{
return m_vertex;
}
inline const b3Vec3& b3Particle::GetPosition() const
{
return m_position;
}
inline void b3Particle::SetVelocity(const b3Vec3& velocity)
{
if (m_type == e_staticParticle)
{
return;
}
m_velocity = velocity;
}
inline const b3Vec3& b3Particle::GetVelocity() const
{
return m_velocity;
}
inline float32 b3Particle::GetMass() const
{
return m_mass;
}
inline float32 b3Particle::GetRadius() const
{
return m_radius;
}
inline void b3Particle::ApplyForce(const b3Vec3& force)
{
if (m_type != e_dynamicParticle)
{
return;
}
m_force += force;
}
inline void b3Particle::ApplyTranslation(const b3Vec3& translation)
{
m_translation += translation;
}
inline b3Particle* b3Particle::GetNext()
{
return m_next;
}
#endif

154
include/bounce/dynamics/cloth/sparse_mat33.h
View File

@ -1,154 +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.
*/
#ifndef B3_SPARSE_MAT_33_H
#define B3_SPARSE_MAT_33_H
#include <bounce/common/math/mat33.h>
#include <bounce/dynamics/cloth/diag_mat33.h>
#include <bounce/dynamics/cloth/dense_vec3.h>
// A static sparse matrix stored in Compressed Sparse Row (CSR) format.
// It's efficient when using in iterative solvers such as CG method, where
// the coefficient matrix must be multiplied with a vector at each iteration.
// See https://en.wikipedia.org/wiki/Sparse_matrix
struct b3SparseMat33
{
b3SparseMat33() { }
b3SparseMat33(u32 _M, u32 _N,
u32 _valueCount, b3Mat33* _values,
u32* _row_ptrs, u32* _cols)
{
M = _M;
N = _N;
values = _values;
valueCount = _valueCount;
row_ptrs = _row_ptrs;
cols = _cols;
}
~b3SparseMat33()
{
}
// Output any given row of the original matrix.
// The given buffer must have size of greater or equal than M.
void AssembleRow(b3Mat33* out, u32 row) const;
// Decompresses this matrix into its original form.
// The output matrix is stored in row-major order.
// The given buffer must have size of greater or equal than M * N.
void AssembleMatrix(b3Mat33* out) const;
// Output the block diagonal part of the original matrix.
// This matrix must be a square matrix.
// The given buffer must have size of greater or equal than M.
void AssembleDiagonal(b3DiagMat33& out) const;
// Dimensions of the original 2D matrix
u32 M;
u32 N;
// Non-zero values
b3Mat33* values;
u32 valueCount;
// Sparsity structure
u32* row_ptrs; // pointers to the first non-zero value of each row (size is M + 1)
u32* cols; // column indices for each non-zero value (size is valueCount)
};
inline void b3SparseMat33::AssembleRow(b3Mat33* out, u32 row) const
{
B3_ASSERT(row < M + 1);
// Start with zero row
for (u32 i = 0; i < N; ++i)
{
out[i].SetZero();
}
for (u32 i = row_ptrs[row]; i < row_ptrs[row + 1]; ++i)
{
u32 col = cols[i];
out[col] = values[i];
}
}
inline void b3SparseMat33::AssembleMatrix(b3Mat33* out) const
{
for (u32 i = 0; i < M; ++i)
{
AssembleRow(out + i * N, i);
}
}
inline void b3SparseMat33::AssembleDiagonal(b3DiagMat33& out) const
{
B3_ASSERT(M == N);
for (u32 row = 0; row < M; ++row)
{
out[row].SetZero();
for (u32 row_ptr = row_ptrs[row]; row_ptr < row_ptrs[row + 1]; ++row_ptr)
{
if (cols[row_ptr] > row)
{
break;
}
if (cols[row_ptr] == row)
{
out[row] = values[row_ptr];
break;
}
}
}
}
inline void b3Mul(b3DenseVec3& out, const b3SparseMat33& A, const b3DenseVec3& v)
{
B3_ASSERT(A.N == out.n);
for (u32 row = 0; row < A.N; ++row)
{
out[row].SetZero();
for (u32 j = A.row_ptrs[row]; j < A.row_ptrs[row + 1]; ++j)
{
u32 col = A.cols[j];
out[row] += A.values[j] * v[col];
}
}
}
inline b3DenseVec3 operator*(const b3SparseMat33& A, const b3DenseVec3& v)
{
b3DenseVec3 result(v.n);
b3Mul(result, A, v);
return result;
}
#endif

236
include/bounce/dynamics/cloth/sparse_sym_mat33.h Normal file
View File

@ -0,0 +1,236 @@
/*
* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_SPARSE_SYM_MAT_33_H
#define B3_SPARSE_SYM_MAT_33_H
#include <bounce/common/memory/stack_allocator.h>
#include <bounce/common/math/mat33.h>
#include <bounce/dynamics/cloth/diag_mat33.h>
#include <bounce/dynamics/cloth/dense_vec3.h>
struct b3SparseSymMat33
{
//
b3SparseSymMat33(b3StackAllocator* a, u32 m, u32 n);
//
~b3SparseSymMat33();
//
b3Mat33& operator()(u32 i, u32 j);
//
const b3Mat33& operator()(u32 i, u32 j) const;
//
void Diagonal(b3DiagMat33& out) const;
//
b3StackAllocator* allocator;
u32 M;
u32 N;
u32* row_ptrs;
u32 value_capacity;
u32 value_count;
b3Mat33* values;
u32* value_columns;
};
inline b3SparseSymMat33::b3SparseSymMat33(b3StackAllocator* a, u32 m, u32 n)
{
B3_ASSERT(m == n);
allocator = a;
M = m;
N = n;
row_ptrs = (u32*)allocator->Allocate((M + 1) * sizeof(u32));
memset(row_ptrs, 0, (M + 1) * sizeof(u32));
value_count = 0;
value_capacity = n * (n + 1) / 2;
values = (b3Mat33*)allocator->Allocate(value_capacity * sizeof(b3Mat33));
value_columns = (u32*)allocator->Allocate(value_capacity * sizeof(u32));
}
inline b3SparseSymMat33::~b3SparseSymMat33()
{
allocator->Free(value_columns);
allocator->Free(values);
allocator->Free(row_ptrs);
}
inline const b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j) const
{
B3_ASSERT(i < M);
B3_ASSERT(j < N);
// Ensure i, and j is on the upper triangle
if (i > j)
{
b3Swap(i, j);
}
u32 row_value_begin = row_ptrs[i];
u32 row_value_count = row_ptrs[i + 1] - row_value_begin;
for (u32 row_value = 0; row_value < row_value_count; ++row_value)
{
u32 row_value_index = row_value_begin + row_value;
u32 row_value_column = value_columns[row_value_index];
if (row_value_column == j)
{
return values[row_value_index];
}
}
return b3Mat33_zero;
}
inline b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j)
{
B3_ASSERT(i < M);
B3_ASSERT(j < N);
// Ensure i, and j is on the upper triangle
if (i > j)
{
b3Swap(i, j);
}
u32 row_value_begin = row_ptrs[i];
u32 row_value_count = row_ptrs[i + 1] - row_value_begin;
for (u32 row_value = 0; row_value < row_value_count; ++row_value)
{
u32 row_value_index = row_value_begin + row_value;
u32 row_value_column = value_columns[row_value_index];
if (row_value_column == j)
{
return values[row_value_index];
}
}
// Insert sorted by column
u32 max_column_row_value = 0;
for (u32 row_value = 0; row_value < row_value_count; ++row_value)
{
u32 row_value_index = row_value_begin + row_value;
u32 row_value_column = value_columns[row_value_index];
if (row_value_column >= j)
{
max_column_row_value = row_value;
break;
}
}
u32 max_column_row_value_index = row_value_begin + max_column_row_value;
// Copy the values to be shifted
u32 shift_count = value_count - max_column_row_value_index;
b3Mat33* shift_values = (b3Mat33*)allocator->Allocate(shift_count * sizeof(b3Mat33));
memcpy(shift_values, values + max_column_row_value_index, shift_count * sizeof(b3Mat33));
u32* shift_value_columns = (u32*)allocator->Allocate(shift_count * sizeof(u32));
memcpy(shift_value_columns, value_columns + max_column_row_value_index, shift_count * sizeof(u32));
// Insert the new value
B3_ASSERT(value_count < value_capacity);
value_columns[max_column_row_value_index] = j;
++value_count;
// Shift the old values
memcpy(values + max_column_row_value_index + 1, shift_values, shift_count * sizeof(b3Mat33));
memcpy(value_columns + max_column_row_value_index + 1, shift_value_columns, shift_count * sizeof(u32));
allocator->Free(shift_value_columns);
allocator->Free(shift_values);
// Shift the old row pointers as well
for (u32 row_ptr_index = i + 1; row_ptr_index < M + 1; ++row_ptr_index)
{
++row_ptrs[row_ptr_index];
}
return values[max_column_row_value_index];
}
inline void b3SparseSymMat33::Diagonal(b3DiagMat33& out) const
{
B3_ASSERT(N == out.n);
for (u32 row = 0; row < M; ++row)
{
out[row].SetZero();
u32 row_value_begin = row_ptrs[row];
u32 row_value_count = row_ptrs[row + 1] - row_value_begin;
for (u32 row_value = 0; row_value < row_value_count; ++row_value)
{
u32 row_value_index = row_value_begin + row_value;
u32 row_value_column = value_columns[row_value_index];
if (row == row_value_column)
{
out[row] = values[row_value_index];
break;
}
}
}
}
inline void b3Mul(b3DenseVec3& out, const b3SparseSymMat33& A, const b3DenseVec3& v)
{
B3_ASSERT(A.N == out.n);
out.SetZero();
for (u32 row = 0; row < A.N; ++row)
{
u32 row_value_begin = A.row_ptrs[row];
u32 row_value_count = A.row_ptrs[row + 1] - row_value_begin;
for (u32 row_value = 0; row_value < row_value_count; ++row_value)
{
u32 row_value_index = row_value_begin + row_value;
u32 row_value_column = A.value_columns[row_value_index];
out[row] += A.values[row_value_index] * v[row_value_column];
if (row != row_value_column)
{
// A(i, j) == A(j, i)
out[row_value_column] += A.values[row_value_index] * v[row];
}
}
}
}
inline b3DenseVec3 operator*(const b3SparseSymMat33& A, const b3DenseVec3& v)
{
b3DenseVec3 result(v.n);
b3Mul(result, A, v);
return result;
}
#endif

140
include/bounce/dynamics/cloth/spring_force.h Normal file
View File

@ -0,0 +1,140 @@
/*
* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_SPRING_FORCE_H
#define B3_SPRING_FORCE_H
#include <bounce/dynamics/cloth/force.h>
struct b3SpringForceDef : public b3ForceDef
{
b3SpringForceDef()
{
type = e_springForce;
p1 = nullptr;
p2 = nullptr;
restLength = 0.0f;
structural = 0.0f;
damping = 0.0f;
}
//
void Initialize(b3Particle* particle1, b3Particle* particle2, float32 structuralStiffness, float32 dampingStiffness);
// Particle 1
b3Particle* p1;
// Particle 2
b3Particle* p2;
// Rest length
float32 restLength;
// Structural stiffness
float32 structural;
// Damping stiffness
float32 damping;
};
//
class b3SpringForce : public b3Force
{
public:
b3Particle* GetParticle1();
b3Particle* GetParticle2();
float32 GetRestLenght() const;
float32 GetStructuralStiffness() const;
float32 GetDampingStiffness() const;
b3Vec3 GetActionForce() const;
private:
friend class b3Force;
friend class b3Cloth;
b3SpringForce(const b3SpringForceDef* def);
~b3SpringForce();
void Initialize(const b3ClothSolverData* data);
void Apply(const b3ClothSolverData* data);
// Solver shared
// Particle 1
b3Particle* m_p1;
// Particle 2
b3Particle* m_p2;
// Rest length
float32 m_L0;
// Structural stiffness
float32 m_ks;
// Damping stiffness
float32 m_kd;
// Solver temp
// Force (f_1 entry)
b3Vec3 m_f;
// Jacobian (J_11 entry)
b3Mat33 m_Jx;
// Jacobian (J_11 entry)
b3Mat33 m_Jv;
};
inline b3Particle * b3SpringForce::GetParticle1()
{
return m_p1;
}
inline b3Particle* b3SpringForce::GetParticle2()
{
return m_p2;
}
inline float32 b3SpringForce::GetRestLenght() const
{
return m_L0;
}
inline float32 b3SpringForce::GetStructuralStiffness() const
{
return m_ks;
}
inline float32 b3SpringForce::GetDampingStiffness() const
{
return m_kd;
}
inline b3Vec3 b3SpringForce::GetActionForce() const
{
return m_f;
}
#endif

484
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;
b3SpringForceDef fd;
fd.Initialize(p1, p2, def.bending, def.damping);
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();
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();
}
void b3Cloth::ResetMass()
b3Force* f = m_forceList.m_head;
while (f)
{
for (u32 i = 0; i < m_particleCount; ++i)
b3Force* f0 = f;
f = f->m_next;
f0->~b3Force();
b3Free(f0);
}
}
b3Particle* b3Cloth::CreateParticle(const b3ParticleDef& def)
{
m_particles[i].mass = 0.0f;
m_particles[i].invMass = 0.0f;
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)
{
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 (f->m_type == e_springForce)
{
b3SpringForce* s = (b3SpringForce*)f;
b3Particle* p1 = s->m_p1;
b3Particle* p2 = s->m_p2;
if (s->type == e_strechSpring)
{
b3Draw_draw->DrawSegment(p1->position, p2->position, b3Color_black);
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)

279
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;
}