refactor cloth
This commit is contained in:
Irlan
parent
47a2c12160
commit
4ae3b7cc79
@ -60,11 +60,10 @@
|
||||
#include <testbed/tests/tumbler.h>
|
||||
#include <testbed/tests/multiple_pendulum.h>
|
||||
#include <testbed/tests/cloth_test.h>
|
||||
#include <testbed/tests/spring_cloth_test.h>
|
||||
#include <testbed/tests/table_cloth.h>
|
||||
#include <testbed/tests/pinned_cloth.h>
|
||||
#include <testbed/tests/shirt.h>
|
||||
#include <testbed/tests/mass_types.h>
|
||||
#include <testbed/tests/particle_types.h>
|
||||
#include <testbed/tests/tension_mapping.h>
|
||||
#include <testbed/tests/single_pendulum.h>
|
||||
#include <testbed/tests/rope_test.h>
|
||||
@ -113,11 +112,10 @@ TestEntry g_tests[] =
|
||||
{ "Tumbler", &Tumbler::Create },
|
||||
{ "Initial Overlap", &InitialOverlap::Create },
|
||||
{ "Multiple Pendulum", &MultiplePendulum::Create },
|
||||
{ "Cloth", &Cloth::Create },
|
||||
{ "Table Cloth", &TableCloth::Create },
|
||||
{ "Pinned Cloth", &PinnedCloth::Create },
|
||||
{ "Shirt", &Shirt::Create },
|
||||
{ "Mass Types", &MassTypes::Create },
|
||||
{ "Particle Types", &ParticleTypes::Create },
|
||||
{ "Tension Mapping", &TensionMapping::Create },
|
||||
{ "Mass-Spring System", &MassSpring::Create },
|
||||
{ "Single Pendulum", &SinglePendulum::Create },
|
||||
|
||||
@ -16,52 +16,344 @@
|
||||
* 3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef CLOTH_H
|
||||
#define CLOTH_H
|
||||
#ifndef CLOTH_TESH_H
|
||||
#define CLOTH_TESH_H
|
||||
|
||||
class Cloth : public Test
|
||||
class ClothDragger
|
||||
{
|
||||
public:
|
||||
Cloth()
|
||||
ClothDragger(Ray3* ray, b3Cloth* cloth)
|
||||
{
|
||||
b3ClothDef def;
|
||||
def.mesh = &m_clothMesh;
|
||||
def.density = 0.2f;
|
||||
def.gravity.Set(0.0f, -10.0f, 0.0f);
|
||||
def.k1 = 0.2f;
|
||||
def.k2 = 0.1f;
|
||||
def.kd = 0.005f;
|
||||
def.r = 1.0f;
|
||||
m_ray = ray;
|
||||
m_cloth = cloth;
|
||||
m_isSelected = false;
|
||||
}
|
||||
|
||||
m_cloth.Initialize(def);
|
||||
~ClothDragger()
|
||||
{
|
||||
|
||||
b3AABB3 aabb;
|
||||
aabb.m_lower.Set(-5.0f, -1.0f, -6.0f);
|
||||
aabb.m_upper.Set(5.0f, 1.0f, -4.0f);
|
||||
}
|
||||
|
||||
b3Particle* vs = m_cloth.GetVertices();
|
||||
for (u32 i = 0; i < m_cloth.GetVertexCount(); ++i)
|
||||
bool IsSelected() const
|
||||
{
|
||||
return m_isSelected;
|
||||
}
|
||||
|
||||
b3Vec3 GetPointA() const
|
||||
{
|
||||
B3_ASSERT(m_isSelected);
|
||||
|
||||
b3ClothMesh* m = m_cloth->GetMesh();
|
||||
b3ClothMeshTriangle* t = m->triangles + m_selection;
|
||||
|
||||
b3Vec3 A = m->vertices[t->v1];
|
||||
b3Vec3 B = m->vertices[t->v2];
|
||||
b3Vec3 C = m->vertices[t->v3];
|
||||
|
||||
return m_u * A + m_v * B + (1.0f - m_u - m_v) * C;
|
||||
}
|
||||
|
||||
b3Vec3 GetPointB() const
|
||||
{
|
||||
B3_ASSERT(m_isSelected);
|
||||
return (1.0f - m_x) * m_ray->A() + m_x * m_ray->B();
|
||||
}
|
||||
|
||||
bool StartDragging()
|
||||
{
|
||||
B3_ASSERT(m_isSelected == false);
|
||||
|
||||
if (Select(m_selection, m_x) == false)
|
||||
{
|
||||
if (aabb.Contains(vs[i].p))
|
||||
return false;
|
||||
}
|
||||
|
||||
m_isSelected = true;
|
||||
|
||||
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* 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 B = GetPointB();
|
||||
|
||||
float32 wABC[4];
|
||||
b3BarycentricCoordinates(wABC, v1, v2, v3, B);
|
||||
|
||||
if (wABC[3] > B3_EPSILON)
|
||||
{
|
||||
m_u = wABC[0] / wABC[3];
|
||||
m_v = wABC[1] / wABC[3];
|
||||
}
|
||||
else
|
||||
{
|
||||
m_u = m_v = 0.0f;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Drag()
|
||||
{
|
||||
B3_ASSERT(m_isSelected);
|
||||
|
||||
b3ClothMesh* m = m_cloth->GetMesh();
|
||||
b3ClothMeshTriangle* t = m->triangles + m_selection;
|
||||
|
||||
b3Vec3 A = GetPointA();
|
||||
b3Vec3 B = GetPointB();
|
||||
|
||||
b3Vec3 dx = B - A;
|
||||
|
||||
b3Particle* p1 = m_cloth->GetParticle(t->v1);
|
||||
m_cloth->Translate(p1, dx);
|
||||
|
||||
b3Particle* p2 = m_cloth->GetParticle(t->v2);
|
||||
m_cloth->Translate(p2, dx);
|
||||
|
||||
b3Particle* p3 = m_cloth->GetParticle(t->v3);
|
||||
m_cloth->Translate(p3, dx);
|
||||
}
|
||||
|
||||
void StopDragging()
|
||||
{
|
||||
B3_ASSERT(m_isSelected);
|
||||
|
||||
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
|
||||
{
|
||||
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 = -0.005f;
|
||||
|
||||
// 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)
|
||||
{
|
||||
vs[i].im = 0.0f;
|
||||
output->normal = -n;
|
||||
}
|
||||
else
|
||||
{
|
||||
output->normal = n;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Select(u32& selection, float32& fraction) const
|
||||
{
|
||||
b3ClothMesh* m = m_cloth->GetMesh();
|
||||
|
||||
b3RayCastInput input;
|
||||
input.p1 = m_ray->A();
|
||||
input.p2 = m_ray->B();
|
||||
input.maxFraction = m_ray->fraction;
|
||||
|
||||
float32 minFraction = B3_MAX_FLOAT;
|
||||
b3Vec3 minNormal(0.0f, 0.0f, 0.0f);
|
||||
u32 minIndex = ~0;
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (minIndex != ~0)
|
||||
{
|
||||
selection = minIndex;
|
||||
fraction = minFraction;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool m_isSelected;
|
||||
|
||||
Ray3* m_ray;
|
||||
float32 m_x;
|
||||
|
||||
b3Cloth * m_cloth;
|
||||
u32 m_selection;
|
||||
float32 m_u, m_v;
|
||||
b3ParticleType m_t1, m_t2, m_t3;
|
||||
};
|
||||
|
||||
class ClothTest : public Test
|
||||
{
|
||||
public:
|
||||
ClothTest() : m_clothDragger(&m_clothRay, &m_cloth)
|
||||
{
|
||||
m_cloth.SetGravity(b3Vec3(0.0f, -10.0f, 0.0f));
|
||||
|
||||
m_clothRay.origin.SetZero();
|
||||
m_clothRay.direction.Set(0.0f, 0.0f, -1.0f);
|
||||
m_clothRay.fraction = g_camera->m_zFar;
|
||||
}
|
||||
|
||||
void Step()
|
||||
{
|
||||
m_cloth.Step(g_testSettings->inv_hertz, g_testSettings->positionIterations);
|
||||
float32 dt = g_testSettings->inv_hertz;
|
||||
|
||||
m_cloth.Step(dt);
|
||||
m_cloth.Apply();
|
||||
|
||||
b3Shape** shapes = m_cloth.GetShapeList();
|
||||
for (u32 i = 0; i < m_cloth.GetShapeCount(); ++i)
|
||||
{
|
||||
b3Shape* s = shapes[i];
|
||||
|
||||
b3Transform xf;
|
||||
xf.SetIdentity();
|
||||
|
||||
g_draw->DrawSolidShape(s, b3Color_white, xf);
|
||||
}
|
||||
|
||||
m_cloth.Draw();
|
||||
|
||||
extern u32 b3_clothSolverIterations;
|
||||
g_draw->DrawString(b3Color_white, "Iterations = %u", b3_clothSolverIterations);
|
||||
|
||||
float32 E = m_cloth.GetEnergy();
|
||||
g_draw->DrawString(b3Color_white, "E = %f", E);
|
||||
|
||||
if (m_clothDragger.IsSelected() == true)
|
||||
{
|
||||
g_draw->DrawSegment(m_clothDragger.GetPointA(), m_clothDragger.GetPointB(), b3Color_white);
|
||||
}
|
||||
}
|
||||
|
||||
static Test* Create()
|
||||
void MouseMove(const Ray3& pw)
|
||||
{
|
||||
return new Cloth();
|
||||
m_clothRay = pw;
|
||||
|
||||
if (m_clothDragger.IsSelected() == true)
|
||||
{
|
||||
m_clothDragger.Drag();
|
||||
}
|
||||
}
|
||||
|
||||
b3GridMesh<10, 10> m_clothMesh;
|
||||
void MouseLeftDown(const Ray3& pw)
|
||||
{
|
||||
if (m_clothDragger.IsSelected() == false)
|
||||
{
|
||||
m_clothDragger.StartDragging();
|
||||
}
|
||||
}
|
||||
|
||||
void MouseLeftUp(const Ray3& pw)
|
||||
{
|
||||
if (m_clothDragger.IsSelected() == true)
|
||||
{
|
||||
m_clothDragger.StopDragging();
|
||||
}
|
||||
}
|
||||
|
||||
Ray3 m_clothRay;
|
||||
b3Cloth m_cloth;
|
||||
ClothDragger m_clothDragger;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -1,175 +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 MASS_TYPES_H
|
||||
#define MASS_TYPES_H
|
||||
|
||||
class MassTypes : public PinnedCloth
|
||||
{
|
||||
public:
|
||||
MassTypes()
|
||||
{
|
||||
}
|
||||
|
||||
void Step()
|
||||
{
|
||||
PinnedCloth::Step();
|
||||
|
||||
g_draw->DrawString(b3Color_white, "S - Static");
|
||||
g_draw->DrawString(b3Color_white, "D - Dynamic");
|
||||
g_draw->DrawString(b3Color_white, "K - Kinematic");
|
||||
g_draw->DrawString(b3Color_white, "Arrows - Apply Force/Velocity/Position");
|
||||
}
|
||||
|
||||
void SetClothType(b3MassType type)
|
||||
{
|
||||
for (u32 i = 0; i < m_cloth.GetMassCount(); ++i)
|
||||
{
|
||||
m_cloth.SetType(i, type);
|
||||
}
|
||||
}
|
||||
|
||||
void KeyDown(int button)
|
||||
{
|
||||
if (button == GLFW_KEY_S)
|
||||
{
|
||||
SetClothType(b3MassType::e_staticMass);
|
||||
}
|
||||
|
||||
if (button == GLFW_KEY_K)
|
||||
{
|
||||
SetClothType(b3MassType::e_kinematicMass);
|
||||
}
|
||||
|
||||
if (button == GLFW_KEY_D)
|
||||
{
|
||||
SetClothType(b3MassType::e_dynamicMass);
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_cloth.GetMassCount(); ++i)
|
||||
{
|
||||
if (m_cloth.GetType(i) == b3MassType::e_staticMass)
|
||||
{
|
||||
if (button == GLFW_KEY_LEFT)
|
||||
{
|
||||
b3Vec3 p = m_cloth.GetPosition(i);
|
||||
|
||||
p.x -= 1.0f;
|
||||
|
||||
m_cloth.SetPosition(i, p);
|
||||
}
|
||||
|
||||
if (button == GLFW_KEY_RIGHT)
|
||||
{
|
||||
b3Vec3 p = m_cloth.GetPosition(i);
|
||||
|
||||
p.x += 1.0f;
|
||||
|
||||
m_cloth.SetPosition(i, p);
|
||||
}
|
||||
|
||||
if (button == GLFW_KEY_UP)
|
||||
{
|
||||
b3Vec3 p = m_cloth.GetPosition(i);
|
||||
|
||||
p.z += 1.0f;
|
||||
|
||||
m_cloth.SetPosition(i, p);
|
||||
}
|
||||
|
||||
if (button == GLFW_KEY_DOWN)
|
||||
{
|
||||
b3Vec3 p = m_cloth.GetPosition(i);
|
||||
|
||||
p.z -= 1.0f;
|
||||
|
||||
m_cloth.SetPosition(i, p);
|
||||
}
|
||||
}
|
||||
|
||||
if (m_cloth.GetType(i) == b3MassType::e_kinematicMass)
|
||||
{
|
||||
if (button == GLFW_KEY_LEFT)
|
||||
{
|
||||
b3Vec3 v = m_cloth.GetVelocity(i);
|
||||
|
||||
v.x -= 5.0f;
|
||||
|
||||
m_cloth.SetVelocity(i, v);
|
||||
}
|
||||
|
||||
if (button == GLFW_KEY_RIGHT)
|
||||
{
|
||||
b3Vec3 v = m_cloth.GetVelocity(i);
|
||||
|
||||
v.x += 5.0f;
|
||||
|
||||
m_cloth.SetVelocity(i, v);
|
||||
}
|
||||
|
||||
if (button == GLFW_KEY_UP)
|
||||
{
|
||||
b3Vec3 v = m_cloth.GetVelocity(i);
|
||||
|
||||
v.z -= 5.0f;
|
||||
|
||||
m_cloth.SetVelocity(i, v);
|
||||
}
|
||||
|
||||
if (button == GLFW_KEY_DOWN)
|
||||
{
|
||||
b3Vec3 v = m_cloth.GetVelocity(i);
|
||||
|
||||
v.z += 5.0f;
|
||||
|
||||
m_cloth.SetVelocity(i, v);
|
||||
}
|
||||
}
|
||||
|
||||
if (m_cloth.GetType(i) == b3MassType::e_dynamicMass)
|
||||
{
|
||||
if (button == GLFW_KEY_LEFT)
|
||||
{
|
||||
m_cloth.ApplyForce(i, b3Vec3(-100.0f, 0.0f, 0.0f));
|
||||
}
|
||||
|
||||
if (button == GLFW_KEY_RIGHT)
|
||||
{
|
||||
m_cloth.ApplyForce(i, b3Vec3(100.0f, 0.0f, 0.0f));
|
||||
}
|
||||
|
||||
if (button == GLFW_KEY_UP)
|
||||
{
|
||||
m_cloth.ApplyForce(i, b3Vec3(0.0f, 0.0f, -100.0f));
|
||||
}
|
||||
|
||||
if (button == GLFW_KEY_DOWN)
|
||||
{
|
||||
m_cloth.ApplyForce(i, b3Vec3(0.0f, 0.0f, 100.0f));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static Test* Create()
|
||||
{
|
||||
return new MassTypes();
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
127
examples/testbed/tests/particle_types.h
Normal file
127
examples/testbed/tests/particle_types.h
Normal file
@ -0,0 +1,127 @@
|
||||
/*
|
||||
* 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 PARTICLE_TYPES_H
|
||||
#define PARTICLE_TYPES_H
|
||||
|
||||
class ParticleTypes : public PinnedCloth
|
||||
{
|
||||
public:
|
||||
ParticleTypes()
|
||||
{
|
||||
}
|
||||
|
||||
void Step()
|
||||
{
|
||||
PinnedCloth::Step();
|
||||
|
||||
g_draw->DrawString(b3Color_white, "S - Static");
|
||||
g_draw->DrawString(b3Color_white, "D - Dynamic");
|
||||
g_draw->DrawString(b3Color_white, "K - Kinematic");
|
||||
g_draw->DrawString(b3Color_white, "Arrows - Apply Force/Velocity/Position");
|
||||
}
|
||||
|
||||
void SetClothType(b3ParticleType type)
|
||||
{
|
||||
for (u32 i = 0; i < m_cloth.GetParticleCount(); ++i)
|
||||
{
|
||||
b3Particle* p = m_cloth.GetParticle(i);
|
||||
m_cloth.SetType(p, type);
|
||||
}
|
||||
}
|
||||
|
||||
void KeyDown(int button)
|
||||
{
|
||||
if (button == GLFW_KEY_S)
|
||||
{
|
||||
SetClothType(e_staticParticle);
|
||||
}
|
||||
|
||||
if (button == GLFW_KEY_K)
|
||||
{
|
||||
SetClothType(e_kinematicParticle);
|
||||
}
|
||||
|
||||
if (button == GLFW_KEY_D)
|
||||
{
|
||||
SetClothType(e_dynamicParticle);
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_cloth.GetParticleCount(); ++i)
|
||||
{
|
||||
b3Particle* p = m_cloth.GetParticle(i);
|
||||
|
||||
b3Vec3 d;
|
||||
d.SetZero();
|
||||
|
||||
if (button == GLFW_KEY_LEFT)
|
||||
{
|
||||
d.x = -1.0f;
|
||||
}
|
||||
|
||||
if (button == GLFW_KEY_RIGHT)
|
||||
{
|
||||
d.x = 1.0f;
|
||||
}
|
||||
|
||||
if (button == GLFW_KEY_UP)
|
||||
{
|
||||
d.y = 1.0f;
|
||||
}
|
||||
|
||||
if (button == GLFW_KEY_DOWN)
|
||||
{
|
||||
d.y = -1.0f;
|
||||
}
|
||||
|
||||
if (button == GLFW_KEY_LEFT ||
|
||||
button == GLFW_KEY_RIGHT ||
|
||||
button == GLFW_KEY_UP ||
|
||||
button == GLFW_KEY_DOWN)
|
||||
{
|
||||
if (p->type == e_staticParticle)
|
||||
{
|
||||
m_cloth.Translate(p, d);
|
||||
}
|
||||
|
||||
if (p->type == e_kinematicParticle)
|
||||
{
|
||||
b3Vec3 v = p->velocity;
|
||||
|
||||
v += 5.0f * d;
|
||||
|
||||
m_cloth.SetVelocity(p, d);
|
||||
}
|
||||
|
||||
if (p->type == e_dynamicParticle)
|
||||
{
|
||||
b3Vec3 f = 100.0f * d;
|
||||
|
||||
m_cloth.ApplyForce(p, f);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static Test* Create()
|
||||
{
|
||||
return new ParticleTypes();
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -19,7 +19,7 @@
|
||||
#ifndef PINNED_CLOTH_H
|
||||
#define PINNED_CLOTH_H
|
||||
|
||||
class PinnedCloth : public SpringClothTest
|
||||
class PinnedCloth : public ClothTest
|
||||
{
|
||||
public:
|
||||
PinnedCloth()
|
||||
@ -42,14 +42,12 @@ public:
|
||||
m_gridClothMesh.sewingLineCount = 0;
|
||||
m_gridClothMesh.sewingLines = nullptr;
|
||||
|
||||
b3SpringClothDef def;
|
||||
def.allocator = &m_clothAllocator;
|
||||
b3ClothDef def;
|
||||
def.mesh = &m_gridClothMesh;
|
||||
def.density = 0.2f;
|
||||
def.ks = 10000.0f;
|
||||
def.kd = 0.0f;
|
||||
def.r = 0.05f;
|
||||
def.gravity.Set(0.0f, -10.0f, 0.0f);
|
||||
|
||||
m_cloth.Initialize(def);
|
||||
|
||||
@ -57,11 +55,12 @@ 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.GetMassCount(); ++i)
|
||||
for (u32 i = 0; i < m_cloth.GetParticleCount(); ++i)
|
||||
{
|
||||
if (aabb.Contains(m_cloth.GetPosition(i)))
|
||||
b3Particle* p = m_cloth.GetParticle(i);
|
||||
if (aabb.Contains(p->position))
|
||||
{
|
||||
m_cloth.SetType(i, b3MassType::e_staticMass);
|
||||
m_cloth.SetType(p, e_staticParticle);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -23,7 +23,7 @@
|
||||
#include <bounce/garment/sewing_pattern.h>
|
||||
#include <bounce/garment/garment_mesh.h>
|
||||
|
||||
class Shirt : public SpringClothTest
|
||||
class Shirt : public ClothTest
|
||||
{
|
||||
public:
|
||||
Shirt()
|
||||
@ -56,13 +56,11 @@ public:
|
||||
}
|
||||
|
||||
// Create cloth
|
||||
b3SpringClothDef def;
|
||||
def.allocator = &m_clothAllocator;
|
||||
b3ClothDef def;
|
||||
def.mesh = &m_shirtClothMesh;
|
||||
def.density = 0.2f;
|
||||
def.ks = 10000.0f;
|
||||
def.r = 0.2f;
|
||||
def.gravity.Set(0.0f, -10.0f, 0.0f);
|
||||
def.ks = 10000.0f;
|
||||
|
||||
m_cloth.Initialize(def);
|
||||
}
|
||||
|
||||
@ -1,356 +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 SPRING_CLOTH_TESH_H
|
||||
#define SPRING_CLOTH_TESH_H
|
||||
|
||||
class ClothDragger
|
||||
{
|
||||
public:
|
||||
ClothDragger(Ray3* ray, b3SpringCloth* cloth)
|
||||
{
|
||||
m_ray = ray;
|
||||
m_cloth = cloth;
|
||||
m_isSelected = false;
|
||||
}
|
||||
|
||||
~ClothDragger()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
bool IsSelected() const
|
||||
{
|
||||
return m_isSelected;
|
||||
}
|
||||
|
||||
b3Vec3 GetPointA() const
|
||||
{
|
||||
B3_ASSERT(m_isSelected);
|
||||
|
||||
b3ClothMesh* m = m_cloth->GetMesh();
|
||||
b3ClothMeshTriangle* t = m->triangles + m_selection;
|
||||
|
||||
b3Vec3 A = m->vertices[t->v1];
|
||||
b3Vec3 B = m->vertices[t->v2];
|
||||
b3Vec3 C = m->vertices[t->v3];
|
||||
|
||||
return m_u * A + m_v * B + (1.0f - m_u - m_v) * C;
|
||||
}
|
||||
|
||||
b3Vec3 GetPointB() const
|
||||
{
|
||||
B3_ASSERT(m_isSelected);
|
||||
return (1.0f - m_x) * m_ray->A() + m_x * m_ray->B();
|
||||
}
|
||||
|
||||
bool StartDragging()
|
||||
{
|
||||
B3_ASSERT(m_isSelected == false);
|
||||
|
||||
if (Select(m_selection, m_x) == false)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
m_isSelected = true;
|
||||
|
||||
b3ClothMesh* m = m_cloth->GetMesh();
|
||||
b3ClothMeshTriangle* t = m->triangles + m_selection;
|
||||
|
||||
m_t1 = m_cloth->GetType(t->v1);
|
||||
m_cloth->SetType(t->v1, b3MassType::e_staticMass);
|
||||
|
||||
m_t2 = m_cloth->GetType(t->v2);
|
||||
m_cloth->SetType(t->v2, b3MassType::e_staticMass);
|
||||
|
||||
m_t3 = m_cloth->GetType(t->v3);
|
||||
m_cloth->SetType(t->v3, b3MassType::e_staticMass);
|
||||
|
||||
b3Vec3 v1 = m_cloth->GetPosition(t->v1);
|
||||
b3Vec3 v2 = m_cloth->GetPosition(t->v2);
|
||||
b3Vec3 v3 = m_cloth->GetPosition(t->v3);
|
||||
|
||||
b3Vec3 B = GetPointB();
|
||||
|
||||
float32 wABC[4];
|
||||
b3BarycentricCoordinates(wABC, v1, v2, v3, B);
|
||||
|
||||
if (wABC[3] > B3_EPSILON)
|
||||
{
|
||||
m_u = wABC[0] / wABC[3];
|
||||
m_v = wABC[1] / wABC[3];
|
||||
}
|
||||
else
|
||||
{
|
||||
m_u = m_v = 0.0f;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Drag()
|
||||
{
|
||||
B3_ASSERT(m_isSelected);
|
||||
|
||||
b3ClothMesh* m = m_cloth->GetMesh();
|
||||
b3ClothMeshTriangle* t = m->triangles + m_selection;
|
||||
|
||||
b3Vec3 A = GetPointA();
|
||||
b3Vec3 B = GetPointB();
|
||||
|
||||
b3Vec3 dx = B - A;
|
||||
|
||||
b3Vec3 v1 = m_cloth->GetPosition(t->v1);
|
||||
v1 += dx;
|
||||
m_cloth->SetPosition(t->v1, v1);
|
||||
|
||||
b3Vec3 v2 = m_cloth->GetPosition(t->v2);
|
||||
v2 += dx;
|
||||
m_cloth->SetPosition(t->v2, v2);
|
||||
|
||||
b3Vec3 v3 = m_cloth->GetPosition(t->v3);
|
||||
v3 += dx;
|
||||
m_cloth->SetPosition(t->v3, v3);
|
||||
}
|
||||
|
||||
void StopDragging()
|
||||
{
|
||||
B3_ASSERT(m_isSelected);
|
||||
|
||||
m_isSelected = false;
|
||||
|
||||
b3ClothMesh* m = m_cloth->GetMesh();
|
||||
b3ClothMeshTriangle* t = m->triangles + m_selection;
|
||||
|
||||
m_cloth->SetType(t->v1, m_t1);
|
||||
m_cloth->SetType(t->v2, m_t2);
|
||||
m_cloth->SetType(t->v3, m_t3);
|
||||
}
|
||||
|
||||
private:
|
||||
bool RayCast(b3RayCastOutput* output, u32 triangleIndex) const
|
||||
{
|
||||
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 = -0.005f;
|
||||
|
||||
// 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;
|
||||
}
|
||||
else
|
||||
{
|
||||
output->normal = n;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Select(u32& selection, float32& fraction) const
|
||||
{
|
||||
b3ClothMesh* m = m_cloth->GetMesh();
|
||||
|
||||
b3RayCastInput input;
|
||||
input.p1 = m_ray->A();
|
||||
input.p2 = m_ray->B();
|
||||
input.maxFraction = m_ray->fraction;
|
||||
|
||||
float32 minFraction = B3_MAX_FLOAT;
|
||||
b3Vec3 minNormal(0.0f, 0.0f, 0.0f);
|
||||
u32 minIndex = ~0;
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (minIndex != ~0)
|
||||
{
|
||||
selection = minIndex;
|
||||
fraction = minFraction;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool m_isSelected;
|
||||
|
||||
Ray3* m_ray;
|
||||
float32 m_x;
|
||||
|
||||
b3SpringCloth * m_cloth;
|
||||
u32 m_selection;
|
||||
float32 m_u, m_v;
|
||||
b3MassType m_t1, m_t2, m_t3;
|
||||
};
|
||||
|
||||
class SpringClothTest : public Test
|
||||
{
|
||||
public:
|
||||
SpringClothTest() : m_clothDragger(&m_clothRay, &m_cloth)
|
||||
{
|
||||
m_clothRay.origin.SetZero();
|
||||
m_clothRay.direction.Set(0.0f, 0.0f, -1.0f);
|
||||
m_clothRay.fraction = g_camera->m_zFar;
|
||||
}
|
||||
|
||||
void Step()
|
||||
{
|
||||
float32 dt = g_testSettings->inv_hertz;
|
||||
|
||||
m_cloth.Step(dt);
|
||||
m_cloth.Apply();
|
||||
|
||||
b3Shape** shapes = m_cloth.GetShapes();
|
||||
for (u32 i = 0; i < m_cloth.GetShapeCount(); ++i)
|
||||
{
|
||||
b3Shape* s = shapes[i];
|
||||
|
||||
b3Transform xf;
|
||||
xf.SetIdentity();
|
||||
|
||||
g_draw->DrawSolidShape(s, b3Color_white, xf);
|
||||
}
|
||||
|
||||
m_cloth.Draw();
|
||||
|
||||
b3SpringClothStep step = m_cloth.GetStep();
|
||||
|
||||
g_draw->DrawString(b3Color_white, "Iterations = %u", step.iterations);
|
||||
|
||||
float32 E = m_cloth.GetEnergy();
|
||||
g_draw->DrawString(b3Color_white, "E = %f", E);
|
||||
|
||||
if (m_clothDragger.IsSelected() == true)
|
||||
{
|
||||
g_draw->DrawSegment(m_clothDragger.GetPointA(), m_clothDragger.GetPointB(), b3Color_white);
|
||||
}
|
||||
}
|
||||
|
||||
void MouseMove(const Ray3& pw)
|
||||
{
|
||||
m_clothRay = pw;
|
||||
|
||||
if (m_clothDragger.IsSelected() == true)
|
||||
{
|
||||
m_clothDragger.Drag();
|
||||
}
|
||||
}
|
||||
|
||||
void MouseLeftDown(const Ray3& pw)
|
||||
{
|
||||
if (m_clothDragger.IsSelected() == false)
|
||||
{
|
||||
m_clothDragger.StartDragging();
|
||||
}
|
||||
}
|
||||
|
||||
void MouseLeftUp(const Ray3& pw)
|
||||
{
|
||||
if (m_clothDragger.IsSelected() == true)
|
||||
{
|
||||
m_clothDragger.StopDragging();
|
||||
}
|
||||
}
|
||||
|
||||
Ray3 m_clothRay;
|
||||
|
||||
b3StackAllocator m_clothAllocator;
|
||||
b3SpringCloth m_cloth;
|
||||
|
||||
ClothDragger m_clothDragger;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -19,7 +19,7 @@
|
||||
#ifndef TABLE_CLOTH_H
|
||||
#define TABLE_CLOTH_H
|
||||
|
||||
class TableCloth : public SpringClothTest
|
||||
class TableCloth : public ClothTest
|
||||
{
|
||||
public:
|
||||
TableCloth()
|
||||
@ -48,14 +48,12 @@ public:
|
||||
m_gridClothMesh.sewingLineCount = 0;
|
||||
m_gridClothMesh.sewingLines = nullptr;
|
||||
|
||||
b3SpringClothDef def;
|
||||
def.allocator = &m_clothAllocator;
|
||||
b3ClothDef def;
|
||||
def.mesh = &m_gridClothMesh;
|
||||
def.density = 0.2f;
|
||||
def.ks = 10000.0f;
|
||||
def.kd = 0.0f;
|
||||
def.r = 0.05f;
|
||||
def.gravity.Set(0.0f, -10.0f, 0.0f);
|
||||
|
||||
m_cloth.Initialize(def);
|
||||
|
||||
|
||||
@ -55,7 +55,7 @@ static inline b3Color Color(float32 x, float32 a, float32 b)
|
||||
return c;
|
||||
}
|
||||
|
||||
class TensionMapping : public SpringClothTest
|
||||
class TensionMapping : public ClothTest
|
||||
{
|
||||
public:
|
||||
TensionMapping()
|
||||
@ -78,14 +78,12 @@ public:
|
||||
m_gridClothMesh.sewingLineCount = 0;
|
||||
m_gridClothMesh.sewingLines = nullptr;
|
||||
|
||||
b3SpringClothDef def;
|
||||
def.allocator = &m_clothAllocator;
|
||||
b3ClothDef def;
|
||||
def.mesh = &m_gridClothMesh;
|
||||
def.density = 0.2f;
|
||||
def.ks = 10000.0f;
|
||||
def.kd = 0.0f;
|
||||
def.r = 0.2f;
|
||||
def.gravity.Set(0.0f, -10.0f, 0.0f);
|
||||
|
||||
m_cloth.Initialize(def);
|
||||
|
||||
@ -93,11 +91,12 @@ 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.GetMassCount(); ++i)
|
||||
for (u32 i = 0; i < m_cloth.GetParticleCount(); ++i)
|
||||
{
|
||||
if (aabb.Contains(m_cloth.GetPosition(i)))
|
||||
b3Particle* p = m_cloth.GetParticle(i);
|
||||
if (aabb.Contains(p->position))
|
||||
{
|
||||
m_cloth.SetType(i, b3MassType::e_staticMass);
|
||||
m_cloth.SetType(p, e_staticParticle);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -108,29 +107,30 @@ public:
|
||||
|
||||
m_cloth.Step(dt);
|
||||
m_cloth.Apply();
|
||||
|
||||
b3StackArray<b3Vec3, 100> T;
|
||||
m_cloth.GetTension(T);
|
||||
|
||||
for (u32 i = 0; i < m_gridClothMesh.triangleCount; ++i)
|
||||
{
|
||||
b3ClothMeshTriangle* t = m_gridClothMesh.triangles + i;
|
||||
|
||||
b3Vec3 v1 = m_gridClothMesh.vertices[t->v1];
|
||||
b3Vec3 v2 = m_gridClothMesh.vertices[t->v2];
|
||||
b3Vec3 v3 = m_gridClothMesh.vertices[t->v3];
|
||||
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;
|
||||
|
||||
b3Draw_draw->DrawSegment(v1, v2, b3Color_black);
|
||||
b3Draw_draw->DrawSegment(v2, v3, b3Color_black);
|
||||
b3Draw_draw->DrawSegment(v3, v1, b3Color_black);
|
||||
|
||||
b3Vec3 f1 = T[t->v1];
|
||||
b3Vec3 f1 = p1->tension;
|
||||
float32 L1 = b3Length(f1);
|
||||
|
||||
b3Vec3 f2 = T[t->v2];
|
||||
b3Vec3 f2 = p2->tension;
|
||||
float32 L2 = b3Length(f2);
|
||||
|
||||
b3Vec3 f3 = T[t->v3];
|
||||
b3Vec3 f3 = p3->tension;
|
||||
float32 L3 = b3Length(f3);
|
||||
|
||||
float32 L = (L1 + L2 + L3) / 3.0f;
|
||||
@ -140,16 +140,14 @@ public:
|
||||
|
||||
b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1);
|
||||
n1.Normalize();
|
||||
g_draw->DrawSolidTriangle(n1, v1, v2, v3, color);
|
||||
|
||||
b3Vec3 n2 = -n1;
|
||||
|
||||
g_draw->DrawSolidTriangle(n1, v1, v2, v3, color);
|
||||
g_draw->DrawSolidTriangle(n2, v1, v3, v2, color);
|
||||
}
|
||||
|
||||
b3SpringClothStep step = m_cloth.GetStep();
|
||||
|
||||
g_draw->DrawString(b3Color_white, "Iterations = %u", step.iterations);
|
||||
extern u32 b3_clothSolverIterations;
|
||||
g_draw->DrawString(b3Color_white, "Iterations = %u", b3_clothSolverIterations);
|
||||
|
||||
if (m_clothDragger.IsSelected() == true)
|
||||
{
|
||||
|
||||
@ -60,7 +60,6 @@
|
||||
|
||||
#include <bounce/dynamics/cloth/cloth_mesh.h>
|
||||
#include <bounce/dynamics/cloth/cloth.h>
|
||||
#include <bounce/dynamics/cloth/spring_cloth.h>
|
||||
|
||||
#include <bounce/dynamics/body.h>
|
||||
|
||||
|
||||
@ -19,117 +19,324 @@
|
||||
#ifndef B3_CLOTH_H
|
||||
#define B3_CLOTH_H
|
||||
|
||||
#include <bounce/common/math/vec3.h>
|
||||
#include <bounce/collision/collision.h>
|
||||
#include <bounce/common/math/mat33.h>
|
||||
#include <bounce/common/template/array.h>
|
||||
#include <bounce/common/memory/stack_allocator.h>
|
||||
|
||||
struct b3Mesh;
|
||||
// Maximum number of shapes per cloth.
|
||||
#define B3_CLOTH_SHAPE_CAPACITY 32
|
||||
|
||||
class b3Shape;
|
||||
|
||||
struct b3ClothMesh;
|
||||
|
||||
// Cloth mesh definition
|
||||
struct b3ClothDef
|
||||
{
|
||||
b3ClothDef()
|
||||
{
|
||||
mesh = NULL;
|
||||
mesh = nullptr;
|
||||
density = 0.0f;
|
||||
gravity.SetZero();
|
||||
k1 = 0.9f;
|
||||
k2 = 0.2f;
|
||||
kd = 0.1f;
|
||||
r = 0.0f;
|
||||
r = 0.05f;
|
||||
ks = 0.0f;
|
||||
kb = 0.0f;
|
||||
kd = 0.0f;
|
||||
}
|
||||
|
||||
// Cloth mesh
|
||||
// Each edge must be shared by at most two triangles (manifold)
|
||||
const b3Mesh* mesh;
|
||||
// Cloth proxy mesh
|
||||
b3ClothMesh* mesh;
|
||||
|
||||
// Cloth density in kilograms per meter squared
|
||||
// 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;
|
||||
|
||||
// Cloth density in kg/m^3
|
||||
float32 density;
|
||||
|
||||
// Gravity force
|
||||
b3Vec3 gravity;
|
||||
|
||||
// Streching stiffness
|
||||
float32 k1;
|
||||
float32 ks;
|
||||
|
||||
// Bending stiffness
|
||||
float32 k2;
|
||||
|
||||
// Damping
|
||||
float32 kd;
|
||||
float32 kb;
|
||||
|
||||
// Cloth thickness
|
||||
float32 r;
|
||||
// Damping stiffness
|
||||
float32 kd;
|
||||
};
|
||||
|
||||
// Static particles have zero mass and velocity, and therefore they can't move.
|
||||
// Kinematic particles are't moved by external and internal forces but can be moved by contact forces.
|
||||
// Dynamic particles have non-zero mass and can move due to internal and external forces.
|
||||
enum b3ParticleType
|
||||
{
|
||||
e_staticParticle,
|
||||
e_kinematicParticle,
|
||||
e_dynamicParticle
|
||||
};
|
||||
|
||||
// Read-only particle
|
||||
struct b3Particle
|
||||
{
|
||||
float32 im;
|
||||
b3Vec3 p0;
|
||||
b3Vec3 p;
|
||||
b3Vec3 v;
|
||||
// Particle type
|
||||
b3ParticleType type;
|
||||
|
||||
// Mass position
|
||||
b3Vec3 position;
|
||||
|
||||
// Mass velocity
|
||||
b3Vec3 velocity;
|
||||
|
||||
// Mass force
|
||||
b3Vec3 force;
|
||||
|
||||
// Mass tension force for visualization
|
||||
b3Vec3 tension;
|
||||
|
||||
// Mass
|
||||
float32 mass;
|
||||
|
||||
// Inverse mass
|
||||
float32 invMass;
|
||||
|
||||
// Radius
|
||||
float32 radius;
|
||||
|
||||
// User data
|
||||
void* userData;
|
||||
|
||||
// Translation used for direct position manipulation
|
||||
b3Vec3 translation;
|
||||
|
||||
// Solver temp
|
||||
|
||||
// Identifier
|
||||
u32 solverId;
|
||||
|
||||
// Solution
|
||||
b3Vec3 x;
|
||||
};
|
||||
|
||||
struct b3C1
|
||||
// Spring types
|
||||
enum b3SpringType
|
||||
{
|
||||
float32 L;
|
||||
u32 i1;
|
||||
u32 i2;
|
||||
e_strechSpring,
|
||||
e_bendSpring,
|
||||
};
|
||||
|
||||
struct b3C2
|
||||
// Read-only spring
|
||||
struct b3Spring
|
||||
{
|
||||
float32 angle;
|
||||
u32 i1;
|
||||
u32 i2;
|
||||
u32 i3;
|
||||
u32 i4;
|
||||
// Spring type
|
||||
b3SpringType type;
|
||||
|
||||
// Particle 1
|
||||
b3Particle* p1;
|
||||
|
||||
// Particle 2
|
||||
b3Particle* p2;
|
||||
|
||||
// Rest length
|
||||
float32 L0;
|
||||
|
||||
// Structural stiffness
|
||||
float32 ks;
|
||||
|
||||
// Damping stiffness
|
||||
float32 kd;
|
||||
};
|
||||
|
||||
// Read-only contact
|
||||
struct b3ParticleContact
|
||||
{
|
||||
b3Particle* p1;
|
||||
b3Shape* s2;
|
||||
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.
|
||||
class b3Cloth
|
||||
{
|
||||
public:
|
||||
b3Cloth();
|
||||
~b3Cloth();
|
||||
|
||||
// Initialize this cloth from a definition.
|
||||
void Initialize(const b3ClothDef& def);
|
||||
|
||||
void Step(float32 dt, u32 iterations);
|
||||
// Return the cloth mesh used to initialize this cloth.
|
||||
b3ClothMesh* GetMesh() const;
|
||||
|
||||
u32 GetVertexCount() const
|
||||
{
|
||||
return m_pCount;
|
||||
}
|
||||
// Set the gravitational acceleration applied to this cloth.
|
||||
// Units are m/s^2.
|
||||
void SetGravity(const b3Vec3& gravity);
|
||||
|
||||
const b3Particle* GetVertices() const
|
||||
{
|
||||
return m_ps;
|
||||
}
|
||||
// Return the gravitational acceleration applied to this cloth.
|
||||
const b3Vec3& GetGravity() const;
|
||||
|
||||
b3Particle* GetVertices()
|
||||
{
|
||||
return m_ps;
|
||||
}
|
||||
// Return the number of particles in this cloth.
|
||||
u32 GetParticleCount() const;
|
||||
|
||||
// Return the particle at a given index in this cloth.
|
||||
b3Particle* GetParticle(u32 i) const;
|
||||
|
||||
// Set the type of a given particle.
|
||||
void SetType(b3Particle* p, b3ParticleType type);
|
||||
|
||||
// Translate a particle in the next time step.
|
||||
void Translate(b3Particle* p, const b3Vec3& translation);
|
||||
|
||||
// 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);
|
||||
|
||||
// Return the kinetic (or dynamic) energy in this system.
|
||||
float32 GetEnergy() const;
|
||||
|
||||
// Add a collision shape to the list of shapes in this cloth.
|
||||
// The cloth will be able to respond to collisions with each shape in the list of shapes.
|
||||
// Current the shape will be treated as a static shape.
|
||||
void AddShape(b3Shape* shape);
|
||||
|
||||
// Return the number of collision shapes in this cloth.
|
||||
u32 GetShapeCount() const;
|
||||
|
||||
// Return the list of collision shapes added to this cloth.
|
||||
b3Shape** GetShapeList();
|
||||
|
||||
// Perform a time step.
|
||||
void Step(float32 dt);
|
||||
|
||||
// Set the positions of the mesh vertices to the positions of their associated particles.
|
||||
void Apply() const;
|
||||
|
||||
// Debug draw the cloth using the associated cloth mesh.
|
||||
void Draw() const;
|
||||
private:
|
||||
void SolveC1();
|
||||
void SolveC2();
|
||||
protected:
|
||||
// Compute mass of each particle.
|
||||
void ResetMass();
|
||||
|
||||
b3Particle* m_ps;
|
||||
u32 m_pCount;
|
||||
|
||||
b3C1* m_c1s;
|
||||
u32 m_c1Count;
|
||||
|
||||
b3C2* m_c2s;
|
||||
u32 m_c2Count;
|
||||
// Update contacts.
|
||||
// This is where some contacts might be initiated or terminated.
|
||||
void UpdateContacts();
|
||||
|
||||
// Solve
|
||||
void Solve(float32 dt);
|
||||
|
||||
b3StackAllocator m_allocator;
|
||||
|
||||
float32 m_k1;
|
||||
float32 m_k2;
|
||||
float32 m_kd;
|
||||
float32 m_r;
|
||||
b3Vec3 m_gravity;
|
||||
|
||||
const b3Mesh* m_mesh;
|
||||
u32 m_particleCount;
|
||||
b3Particle* m_particles;
|
||||
|
||||
b3Spring* m_springs;
|
||||
u32 m_springCount;
|
||||
|
||||
b3ParticleContact* m_contacts;
|
||||
//u32 m_contactCount;
|
||||
|
||||
b3Shape* m_shapes[B3_CLOTH_SHAPE_CAPACITY];
|
||||
u32 m_shapeCount;
|
||||
|
||||
b3ClothMesh* m_mesh;
|
||||
float32 m_density;
|
||||
};
|
||||
|
||||
inline b3ClothMesh* b3Cloth::GetMesh() const
|
||||
{
|
||||
return m_mesh;
|
||||
}
|
||||
|
||||
inline const b3Vec3& b3Cloth::GetGravity() const
|
||||
{
|
||||
return m_gravity;
|
||||
}
|
||||
|
||||
inline void b3Cloth::SetGravity(const b3Vec3& gravity)
|
||||
{
|
||||
m_gravity = gravity;
|
||||
}
|
||||
|
||||
inline u32 b3Cloth::GetParticleCount() const
|
||||
{
|
||||
return m_particleCount;
|
||||
}
|
||||
|
||||
inline b3Particle* b3Cloth::GetParticle(u32 i) const
|
||||
{
|
||||
B3_ASSERT(i < m_particleCount);
|
||||
return m_particles + i;
|
||||
}
|
||||
|
||||
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::Translate(b3Particle* p, const b3Vec3& translation)
|
||||
{
|
||||
p->translation += translation;
|
||||
}
|
||||
|
||||
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 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;
|
||||
}
|
||||
|
||||
inline u32 b3Cloth::GetShapeCount() const
|
||||
{
|
||||
return m_shapeCount;
|
||||
}
|
||||
|
||||
inline b3Shape** b3Cloth::GetShapeList()
|
||||
{
|
||||
return m_shapes;
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -16,48 +16,48 @@
|
||||
* 3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_SPRING_SOLVER_H
|
||||
#define B3_SPRING_SOLVER_H
|
||||
#ifndef B3_CLOTH_SOLVER_H
|
||||
#define B3_CLOTH_SOLVER_H
|
||||
|
||||
#include <bounce/common/math/vec3.h>
|
||||
#include <bounce/common/math/mat33.h>
|
||||
|
||||
class b3Shape;
|
||||
|
||||
class b3SpringCloth;
|
||||
class b3StackAllocator;
|
||||
|
||||
struct b3DenseVec3;
|
||||
struct b3DiagMat33;
|
||||
struct b3SparseMat33;
|
||||
|
||||
struct b3MassContact;
|
||||
struct b3Particle;
|
||||
struct b3Spring;
|
||||
struct b3ParticleContact;
|
||||
|
||||
enum class b3MassType : u32;
|
||||
class b3Shape;
|
||||
class b3StackAllocator;
|
||||
|
||||
struct b3SpringSolverDef
|
||||
struct b3ClothSolverDef
|
||||
{
|
||||
b3SpringCloth* cloth;
|
||||
float32 dt;
|
||||
b3StackAllocator* stack;
|
||||
u32 particleCapacity;
|
||||
u32 springCapacity;
|
||||
u32 contactCapacity;
|
||||
};
|
||||
|
||||
class b3SpringSolver
|
||||
class b3ClothSolver
|
||||
{
|
||||
public:
|
||||
b3SpringSolver(const b3SpringSolverDef& def);
|
||||
b3ClothSolver(const b3ClothSolverDef& def);
|
||||
~b3ClothSolver();
|
||||
|
||||
void Add(b3Particle* p);
|
||||
void Add(b3Spring* s);
|
||||
void Add(b3ParticleContact* c);
|
||||
|
||||
~b3SpringSolver();
|
||||
|
||||
void Solve(b3DenseVec3& extraForces);
|
||||
|
||||
u32 GetIterations() const;
|
||||
void Solve(float32 dt, const b3Vec3& gravity);
|
||||
private:
|
||||
// Apply internal forces and store their unique derivatives.
|
||||
void ApplySpringForces();
|
||||
|
||||
// Compute forces.
|
||||
void Compute_f(b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3Vec3& gravity);
|
||||
|
||||
// Compute A and b in Ax = b
|
||||
void Compute_A_b(b3SparseMat33& A, b3DenseVec3& b) const;
|
||||
void Compute_A_b(b3SparseMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const;
|
||||
|
||||
// Compute S.
|
||||
void Compute_S(b3DiagMat33& S);
|
||||
@ -67,38 +67,25 @@ private:
|
||||
|
||||
// Solve Ax = b.
|
||||
// Output x and the residual error f = Ax - b ~ 0.
|
||||
void Solve(b3DenseVec3& x, b3DenseVec3& f, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const;
|
||||
|
||||
b3SpringCloth * m_cloth;
|
||||
float32 m_h;
|
||||
b3Mat33* m_Jx;
|
||||
b3Mat33* m_Jv;
|
||||
u32 m_iterations;
|
||||
void Solve(b3DenseVec3& x, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const;
|
||||
|
||||
b3StackAllocator* m_allocator;
|
||||
|
||||
b3Vec3* m_x;
|
||||
b3Vec3* m_v;
|
||||
b3Vec3* m_f;
|
||||
float32* m_m;
|
||||
float32* m_inv_m;
|
||||
b3Vec3* m_y;
|
||||
b3Vec3* m_z;
|
||||
b3Vec3* m_x0;
|
||||
b3MassType* m_types;
|
||||
u32 m_massCount;
|
||||
float32 m_h;
|
||||
|
||||
u32 m_particleCapacity;
|
||||
u32 m_particleCount;
|
||||
b3Particle** m_particles;
|
||||
|
||||
b3MassContact* m_contacts;
|
||||
|
||||
b3Spring* m_springs;
|
||||
u32 m_springCapacity;
|
||||
u32 m_springCount;
|
||||
|
||||
b3Shape** m_shapes;
|
||||
b3Spring** m_springs;
|
||||
b3Mat33* m_Jx;
|
||||
b3Mat33* m_Jv;
|
||||
|
||||
u32 m_contactCapacity;
|
||||
u32 m_contactCount;
|
||||
b3ParticleContact** m_contacts;
|
||||
};
|
||||
|
||||
inline u32 b3SpringSolver::GetIterations() const
|
||||
{
|
||||
return m_iterations;
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -1,356 +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_SPRING_CLOTH_H
|
||||
#define B3_SPRING_CLOTH_H
|
||||
|
||||
#include <bounce/common/math/mat33.h>
|
||||
#include <bounce/common/template/array.h>
|
||||
|
||||
// Maximum number of shapes per cloth.
|
||||
#define B3_CLOTH_SHAPE_CAPACITY 32
|
||||
|
||||
class b3StackAllocator;
|
||||
|
||||
class b3Shape;
|
||||
|
||||
struct b3ClothMesh;
|
||||
|
||||
struct b3SpringClothDef
|
||||
{
|
||||
b3SpringClothDef()
|
||||
{
|
||||
allocator = nullptr;
|
||||
mesh = nullptr;
|
||||
density = 0.0f;
|
||||
ks = 0.0f;
|
||||
kb = 0.0f;
|
||||
kd = 0.0f;
|
||||
r = 0.05f;
|
||||
gravity.SetZero();
|
||||
}
|
||||
|
||||
// Stack allocator
|
||||
b3StackAllocator* allocator;
|
||||
|
||||
// Cloth mesh
|
||||
b3ClothMesh* mesh;
|
||||
|
||||
// Cloth density in kg/m^3
|
||||
float32 density;
|
||||
|
||||
// Streching stiffness
|
||||
float32 ks;
|
||||
|
||||
// Bending stiffness
|
||||
float32 kb;
|
||||
|
||||
// Damping stiffness
|
||||
float32 kd;
|
||||
|
||||
// Mass 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;
|
||||
|
||||
// Acceleration due to gravity (m/s^2)
|
||||
b3Vec3 gravity;
|
||||
};
|
||||
|
||||
enum b3SpringType
|
||||
{
|
||||
e_strechSpring,
|
||||
e_bendSpring,
|
||||
// e_sewingSpring
|
||||
};
|
||||
|
||||
struct b3Spring
|
||||
{
|
||||
// Spring type
|
||||
b3SpringType type;
|
||||
|
||||
// Mass 1
|
||||
u32 i1;
|
||||
|
||||
// Mass 2
|
||||
u32 i2;
|
||||
|
||||
// Rest length
|
||||
float32 L0;
|
||||
|
||||
// Structural stiffness
|
||||
float32 ks;
|
||||
|
||||
// Damping stiffness
|
||||
float32 kd;
|
||||
};
|
||||
|
||||
// Static masses have zero mass and velocity, and therefore they can't move.
|
||||
// Kinematic masses are't moved by external and internal forces but can be moved by contact forces.
|
||||
// Dynamic masses have non-zero mass and can move due to internal and external forces.
|
||||
enum class b3MassType : u32
|
||||
{
|
||||
e_staticMass,
|
||||
e_kinematicMass,
|
||||
e_dynamicMass
|
||||
};
|
||||
|
||||
//
|
||||
struct b3MassContact
|
||||
{
|
||||
u32 j;
|
||||
b3Vec3 n, t1, t2;
|
||||
float32 Fn, Ft1, Ft2;
|
||||
bool lockN, lockT1, lockT2;
|
||||
};
|
||||
|
||||
// Time step statistics
|
||||
struct b3SpringClothStep
|
||||
{
|
||||
u32 iterations;
|
||||
};
|
||||
|
||||
// A cloth it treats cloth as a collection of masses connected by springs.
|
||||
// Large time steps can be taken.
|
||||
// If accuracy and stability are required, not performance,
|
||||
// you can use this class instead of using b3Cloth.
|
||||
class b3SpringCloth
|
||||
{
|
||||
public:
|
||||
b3SpringCloth();
|
||||
~b3SpringCloth();
|
||||
|
||||
// Initialize this cloth from a definition.
|
||||
void Initialize(const b3SpringClothDef& def);
|
||||
|
||||
// Return the cloth mesh used to initialize this cloth.
|
||||
b3ClothMesh* GetMesh() const;
|
||||
|
||||
// Set the gravitational acceleration applied to this cloth.
|
||||
// Units are m/s^2.
|
||||
void SetGravity(const b3Vec3& gravity);
|
||||
|
||||
// Return the number of masses in this cloth.
|
||||
u32 GetMassCount() const;
|
||||
|
||||
// Return the gravitational acceleration applied to this cloth.
|
||||
const b3Vec3& GetGravity() const;
|
||||
|
||||
// Set the type of a given point mass.
|
||||
void SetType(u32 i, b3MassType type);
|
||||
|
||||
// Return the type of a given point mass.
|
||||
b3MassType GetType(u32 i) const;
|
||||
|
||||
// Set the position of a given point mass.
|
||||
// This function will have effect on the position of the point mass
|
||||
// after performing a time step.
|
||||
void SetPosition(u32 i, const b3Vec3& position);
|
||||
|
||||
// Return the position of a given point mass.
|
||||
const b3Vec3& GetPosition(u32 i) const;
|
||||
|
||||
// Set the velocity of a given point mass.
|
||||
void SetVelocity(u32 i, const b3Vec3& velocity);
|
||||
|
||||
// Return the velocity of a given point mass.
|
||||
const b3Vec3& GetVelocity(u32 i) const;
|
||||
|
||||
// Apply a force to a given point mass.
|
||||
void ApplyForce(u32 i, const b3Vec3& force);
|
||||
|
||||
// Return the kinetic (or dynamic) energy in this system.
|
||||
float32 GetEnergy() const;
|
||||
|
||||
// Return the tension forces (due to springs) acting at each point mass.
|
||||
// Units are kg * m / s^2
|
||||
void GetTension(b3Array<b3Vec3>& tensions) const;
|
||||
|
||||
// Add a shape to the list of shapes in this cloth.
|
||||
// The cloth will be able to respond to collisions with each shape in the list of shapes.
|
||||
void AddShape(b3Shape* shape);
|
||||
|
||||
// Return the number of shapes added to this cloth.
|
||||
u32 GetShapeCount() const;
|
||||
|
||||
// Return the list of shapes added to this cloth.
|
||||
b3Shape** GetShapes();
|
||||
|
||||
// Return the statistics of the last time step.
|
||||
const b3SpringClothStep& GetStep() const;
|
||||
|
||||
// Perform a time step (marches time forward).
|
||||
void Step(float32 dt);
|
||||
|
||||
// Set the positions of the mesh vertices to the positions of their associated point masses.
|
||||
void Apply() const;
|
||||
|
||||
// Debug draw the cloth mesh.
|
||||
void Draw() const;
|
||||
protected:
|
||||
friend class b3SpringSolver;
|
||||
|
||||
// Update contacts.
|
||||
// This is where some contacts might be initiated or terminated.
|
||||
void UpdateContacts();
|
||||
|
||||
b3StackAllocator* m_allocator;
|
||||
|
||||
b3ClothMesh* m_mesh;
|
||||
float32 m_r;
|
||||
|
||||
b3Vec3 m_gravity;
|
||||
|
||||
b3Vec3* m_x;
|
||||
b3Vec3* m_v;
|
||||
b3Vec3* m_f;
|
||||
float32* m_m;
|
||||
float32* m_inv_m;
|
||||
b3Vec3* m_y;
|
||||
b3Vec3* m_z;
|
||||
b3Vec3* m_x0;
|
||||
b3MassType* m_types;
|
||||
u32 m_massCount;
|
||||
|
||||
b3MassContact* m_contacts;
|
||||
|
||||
b3Spring* m_springs;
|
||||
u32 m_springCount;
|
||||
|
||||
b3Shape* m_shapes[B3_CLOTH_SHAPE_CAPACITY];
|
||||
u32 m_shapeCount;
|
||||
|
||||
b3SpringClothStep m_step;
|
||||
};
|
||||
|
||||
inline b3ClothMesh* b3SpringCloth::GetMesh() const
|
||||
{
|
||||
return m_mesh;
|
||||
}
|
||||
|
||||
inline const b3Vec3& b3SpringCloth::GetGravity() const
|
||||
{
|
||||
return m_gravity;
|
||||
}
|
||||
|
||||
inline void b3SpringCloth::SetGravity(const b3Vec3& gravity)
|
||||
{
|
||||
m_gravity = gravity;
|
||||
}
|
||||
|
||||
inline u32 b3SpringCloth::GetMassCount() const
|
||||
{
|
||||
return m_massCount;
|
||||
}
|
||||
|
||||
inline b3MassType b3SpringCloth::GetType(u32 i) const
|
||||
{
|
||||
B3_ASSERT(i < m_massCount);
|
||||
return m_types[i];
|
||||
}
|
||||
|
||||
inline void b3SpringCloth::SetType(u32 i, b3MassType type)
|
||||
{
|
||||
B3_ASSERT(i < m_massCount);
|
||||
|
||||
if (m_types[i] == type)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
m_types[i] = type;
|
||||
|
||||
m_f[i].SetZero();
|
||||
|
||||
if (type == b3MassType::e_staticMass)
|
||||
{
|
||||
m_v[i].SetZero();
|
||||
m_y[i].SetZero();
|
||||
|
||||
m_contacts[i].lockN = false;
|
||||
}
|
||||
}
|
||||
|
||||
inline void b3SpringCloth::SetPosition(u32 i, const b3Vec3& position)
|
||||
{
|
||||
B3_ASSERT(i < m_massCount);
|
||||
m_y[i] += position - m_x[i];
|
||||
}
|
||||
|
||||
inline const b3Vec3& b3SpringCloth::GetPosition(u32 i) const
|
||||
{
|
||||
B3_ASSERT(i < m_massCount);
|
||||
return m_x[i];
|
||||
}
|
||||
|
||||
inline void b3SpringCloth::SetVelocity(u32 i, const b3Vec3& velocity)
|
||||
{
|
||||
B3_ASSERT(i < m_massCount);
|
||||
|
||||
if (m_types[i] == b3MassType::e_staticMass)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
m_v[i] = velocity;
|
||||
}
|
||||
|
||||
inline const b3Vec3& b3SpringCloth::GetVelocity(u32 i) const
|
||||
{
|
||||
B3_ASSERT(i < m_massCount);
|
||||
return m_v[i];
|
||||
}
|
||||
|
||||
inline void b3SpringCloth::ApplyForce(u32 i, const b3Vec3& force)
|
||||
{
|
||||
B3_ASSERT(i < m_massCount);
|
||||
|
||||
if (m_types[i] != b3MassType::e_dynamicMass)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
m_f[i] += force;
|
||||
}
|
||||
|
||||
inline float32 b3SpringCloth::GetEnergy() const
|
||||
{
|
||||
float32 E = 0.0f;
|
||||
for (u32 i = 0; i < m_massCount; ++i)
|
||||
{
|
||||
E += m_m[i] * b3Dot(m_v[i], m_v[i]);
|
||||
}
|
||||
return 0.5f * E;
|
||||
}
|
||||
|
||||
inline u32 b3SpringCloth::GetShapeCount() const
|
||||
{
|
||||
return m_shapeCount;
|
||||
}
|
||||
|
||||
inline b3Shape** b3SpringCloth::GetShapes()
|
||||
{
|
||||
return m_shapes;
|
||||
}
|
||||
|
||||
inline const b3SpringClothStep& b3SpringCloth::GetStep() const
|
||||
{
|
||||
return m_step;
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user