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

through an acceleration constraint, the solver can remove acceleration from kinematic particles; consistency; in effect test update

Browse Source
This commit is contained in:
Irlan 2018-05-27 02:50:40 -03:00
parent 8abb45fd21
commit dba5ffbe06
8 changed files with 146 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -118,13 +118,13 @@ public:
b3Vec3 dx = B - A;
b3Particle* p1 = m_cloth->GetParticle(t->v1);
m_cloth->Translate(p1, dx);
m_cloth->ApplyTranslation(p1, dx);
b3Particle* p2 = m_cloth->GetParticle(t->v2);
m_cloth->Translate(p2, dx);
m_cloth->ApplyTranslation(p2, dx);
b3Particle* p3 = m_cloth->GetParticle(t->v3);
m_cloth->Translate(p3, dx);
m_cloth->ApplyTranslation(p3, dx);
}
void StopDragging()

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

@ -96,7 +96,7 @@ public:
{
if (p->type == e_staticParticle)
{
m_cloth->Translate(p, d);
m_cloth->ApplyTranslation(p, d);
}
if (p->type == e_kinematicParticle)

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

@ -69,6 +69,11 @@ public:
tableShape.m_hull = &m_tableHull;
tableShape.m_radius = 0.2f;
//b3CapsuleShape tableShape;
//tableShape.m_centers[0].Set(0.0f, 0.0f, -1.0f);
//tableShape.m_centers[1].Set(0.0f, 0.0f, 1.0f);
//tableShape.m_radius = 2.0f;
b3ShapeDef sd;
sd.shape = &tableShape;
sd.friction = 1.0f;
@ -82,6 +87,7 @@ public:
return new TableCloth();
}
//b3GridMesh<2, 2> m_gridMesh;
b3GridMesh<10, 10> m_gridMesh;
b3ClothMeshMesh m_gridClothMeshMesh;
b3ClothMesh m_gridClothMesh;

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

@ -124,8 +124,8 @@ public:
u32 i1 = m_cloth->GetParticleIndex(p1);
u32 i2 = m_cloth->GetParticleIndex(p2);
tension[i1] += s->tension;
tension[i2] -= s->tension;
tension[i1] += s->f;
tension[i2] -= s->f;
}
for (u32 i = 0; i < m_gridClothMesh.triangleCount; ++i)

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

@ -145,14 +145,14 @@ struct b3Spring
// Solver temp
// Action tensile force (f_i entry)
b3Vec3 tension;
// Force (f_i entry)
b3Vec3 f;
// Jacobian (J_ii entry)
b3Mat33 Jx, Jv;
// Apply spring forces.
void ApplyForces(const b3ClothSolverData* data);
// Initialize forces and its derivatives.
void InitializeForces(const b3ClothSolverData* data);
};
// Read-only body contact between a particle and a solid
@ -191,15 +191,15 @@ public:
// Set the type of a given particle.
void SetType(b3Particle* p, b3ParticleType type);
// Translate a given 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);
// Apply a translation to a given particle.
void ApplyTranslation(b3Particle* p, const b3Vec3& translation);
// Return the number of springs in this cloth.
u32 GetSpringCount() const;
@ -317,11 +317,6 @@ inline void b3Cloth::SetType(b3Particle* p, b3ParticleType type)
}
}
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)
@ -340,6 +335,11 @@ inline void b3Cloth::ApplyForce(b3Particle* p, const b3Vec3& force)
p->force += force;
}
inline void b3Cloth::ApplyTranslation(b3Particle* p, const b3Vec3& translation)
{
p->translation += translation;
}
inline u32 b3Cloth::GetSpringCount() const
{
return m_springCount;

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

@ -25,6 +25,7 @@
struct b3DenseVec3;
struct b3DiagMat33;
struct b3SparseMat33;
struct b3SolverSparseMat33;
struct b3Particle;
struct b3Spring;
@ -50,6 +51,13 @@ struct b3ClothSolverData
float32 invdt;
};
struct b3SpringForce
{
u32 i1, i2;
b3Vec3 f;
b3Mat33 Jx, Jv;
};
struct b3AccelerationConstraint
{
u32 i1;
@ -69,11 +77,14 @@ public:
void Solve(float32 dt, const b3Vec3& gravity);
private:
// Initialize forces.
void InitializeForces();
// Initialize constraints.
void InitializeConstraints();
// Compute A and b in Ax = b
void Compute_A_b(b3SparseMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const;
void Compute_A_b(b3SolverSparseMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const;
// Compute S and z.
void Compute_S_z(b3DiagMat33& S, b3DenseVec3& z);

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

@ -31,10 +31,10 @@
#define B3_CLOTH_BENDING 0
#define B3_CLOTH_FRICTION 0
#define B3_CLOTH_FRICTION 1
// b3Spring
void b3Spring::ApplyForces(const b3ClothSolverData* data)
void b3Spring::InitializeForces(const b3ClothSolverData* data)
{
u32 i1 = p1->solverId;
u32 i2 = p2->solverId;
@ -51,36 +51,25 @@ void b3Spring::ApplyForces(const b3ClothSolverData* data)
if (b3Dot(dx, dx) >= L0 * L0)
{
// Tension
float32 L = b3Length(dx);
b3Vec3 n = dx / L;
b3Vec3 sf1 = -ks * (L - L0) * n;
b3Vec3 sf2 = -sf1;
tension = sf1;
data->f[i1] += sf1;
data->f[i2] += sf2;
// Tension
f = -ks * (L - L0) * n;
// Jacobian
Jx = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx)));
}
else
{
tension.SetZero();
f.SetZero();
Jx.SetZero();
}
// Damping
b3Vec3 dv = v1 - v2;
b3Vec3 df1 = -kd * dv;
b3Vec3 df2 = -df1;
data->f[i1] += df1;
data->f[i2] += df2;
f += -kd * dv;
Jv = -kd * I;
}
@ -238,6 +227,9 @@ b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world)
c->n_active = false;
c->t1_active = false;
c->t2_active = false;
c->Fn = 0.0f;
c->Ft1 = 0.0f;
c->Ft2 = 0.0f;
}
// Compute mass
@ -284,7 +276,7 @@ b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world)
s->L0 = b3Distance(p1->position, p2->position);
s->ks = def.ks;
s->kd = def.kd;
s->tension.SetZero();
s->f.SetZero();
}
#if B3_CLOTH_BENDING
@ -330,7 +322,7 @@ b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world)
s->L0 = 0.0f;
s->ks = def.ks;
s->kd = def.kd;
s->tension.SetZero();
s->f.SetZero();
}
B3_ASSERT(m_springCount <= springCapacity);
@ -388,21 +380,13 @@ void b3Cloth::UpdateContacts()
{
B3_PROFILE("Update Contacts");
// Clear active flags
for (u32 i = 0; i < m_particleCount; ++i)
{
m_contacts[i].n_active = false;
m_contacts[i].t1_active = false;
m_contacts[i].t2_active = false;
}
// Create contacts
for (u32 i = 0; i < m_particleCount; ++i)
{
b3Particle* p = m_particles + i;
// Static particles can't participate in unilateral collisions.
if (p->type == e_staticParticle)
// Static and kinematic particles can't participate in unilateral collisions.
if (p->type != e_dynamicParticle)
{
continue;
}
@ -412,6 +396,11 @@ void b3Cloth::UpdateContacts()
// Save the old contact
b3BodyContact c0 = *c;
// Create a new contact
c->n_active = false;
c->t1_active = false;
c->t2_active = false;
b3Sphere s1;
s1.vertex = p->position;
s1.radius = p->radius;
@ -456,7 +445,7 @@ void b3Cloth::UpdateContacts()
c->t2 = b3Cross(c->t1, n);
}
// Update contact state
// Update the contact state
if (c0.n_active == true && c->n_active == true)
{
// The contact persists
@ -496,7 +485,7 @@ void b3Cloth::UpdateContacts()
continue;
}
b3Shape* s = c->s;
b3Shape* s = c->s2;
b3Vec3 n = c->n;
float32 u = s->GetFriction();
float32 normalForce = c0.Fn;

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

@ -78,12 +78,20 @@ void b3ClothSolver::Add(b3Spring* s)
m_springs[m_springCount++] = s;
}
void b3ClothSolver::InitializeForces()
{
for (u32 i = 0; i < m_springCount; ++i)
{
m_springs[i]->InitializeForces(&m_solverData);
}
}
void b3ClothSolver::InitializeConstraints()
{
for (u32 i = 0; i < m_particleCount; ++i)
{
b3Particle* p = m_particles[i];
if (p->type == e_staticParticle)
if (p->type != e_dynamicParticle)
{
b3AccelerationConstraint* ac = m_constraints + m_constraintCount;
++m_constraintCount;
@ -98,8 +106,6 @@ void b3ClothSolver::InitializeConstraints()
b3BodyContact* pc = m_contacts[i];
b3Particle* p = pc->p1;
B3_ASSERT(p->type != e_staticParticle);
b3AccelerationConstraint* ac = m_constraints + m_constraintCount;
++m_constraintCount;
ac->i1 = p->solverId;
@ -128,22 +134,28 @@ void b3ClothSolver::InitializeConstraints()
}
}
static B3_FORCE_INLINE b3SparseMat33 b3AllocSparse(b3StackAllocator* allocator, u32 M, u32 N)
struct b3SolverSparseMat33 : public b3SparseMat33
{
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));
b3SolverSparseMat33(b3StackAllocator* a, u32 m, u32 n)
{
allocator = a;
M = m;
N = n;
valueCount = 0;
values = (b3Mat33*)allocator->Allocate(M * N * sizeof(b3Mat33));
cols = (u32*)allocator->Allocate(M * N * sizeof(u32));
row_ptrs = (u32*)allocator->Allocate((M + 1) * sizeof(u32));
}
return b3SparseMat33(M, N, size, elements, row_ptrs, cols);
}
~b3SolverSparseMat33()
{
allocator->Free(row_ptrs);
allocator->Free(cols);
allocator->Free(values);
}
static B3_FORCE_INLINE void b3FreeSparse(b3SparseMat33& matrix, b3StackAllocator* allocator)
{
allocator->Free(matrix.row_ptrs);
allocator->Free(matrix.cols);
allocator->Free(matrix.values);
}
b3StackAllocator* allocator;
};
void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
{
@ -187,15 +199,21 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
sy[p->solverId] -= c->s * c->n;
}
// Apply spring forces and derivatives
// Initialize forces
InitializeForces();
// Apply internal forces
for (u32 i = 0; i < m_springCount; ++i)
{
m_springs[i]->ApplyForces(&m_solverData);
b3Spring* s = m_springs[i];
sf[s->p1->solverId] += s->f;
sf[s->p2->solverId] -= s->f;
}
// Initialize constraints
InitializeConstraints();
// Compute S, z
b3DiagMat33 S(m_particleCount);
b3DenseVec3 z(m_particleCount);
Compute_S_z(S, z);
@ -205,7 +223,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
// b = h * (f0 + h * dfdx * v0 + dfdx * y)
// A
b3SparseMat33 A = b3AllocSparse(m_allocator, m_particleCount, m_particleCount);
b3SolverSparseMat33 A(m_allocator, m_particleCount, m_particleCount);
// b
b3DenseVec3 b(m_particleCount);
@ -220,38 +238,17 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
Solve(x, iterations, A, b, S, z, sx0);
b3_clothSolverIterations = iterations;
// f = A * x - b
b3DenseVec3 f = A * x - b;
// Update state
// Compute the new state
// Clamp large translations?
float32 h = dt;
sv = sv + x;
sx = sx + h * sv + sy;
// Copy state buffers back to the particles
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;
m_particles[i]->position = sx[i];
m_particles[i]->velocity = sv[i];
}
// Cache x to improve convergence
@ -263,6 +260,10 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
// Store the extra contact constraint forces that should have been
// supplied to enforce the contact constraints exactly.
// These forces can be used in contact constraint logic.
// f = A * x - b
b3DenseVec3 f = A * x - b;
for (u32 i = 0; i < m_contactCount; ++i)
{
b3BodyContact* c = m_contacts[i];
@ -277,15 +278,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
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)
@ -329,7 +321,7 @@ static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A)
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
void b3ClothSolver::Compute_A_b(b3SolverSparseMat33& SA, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const
{
float32 h = m_solverData.dt;
@ -390,14 +382,14 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense
}
}
// Assembly sparsity
u32 nzCount = 0;
// Assembly sparsity.
u32 valueCapacity = m_particleCapacity * m_particleCapacity;
SA.row_ptrs[0] = 0;
for (u32 i = 0; i < m_particleCount; ++i)
{
u32 rowNzCount = 0;
u32 rowValueCount = 0;
for (u32 j = 0; j < m_particleCount; ++j)
{
@ -405,21 +397,17 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense
if (b3IsZero(a) == false)
{
B3_ASSERT(nzCount <= SA.valueCount);
SA.values[nzCount] = a;
SA.cols[nzCount] = j;
++nzCount;
++rowNzCount;
SA.values[SA.valueCount] = a;
SA.cols[SA.valueCount] = j;
++SA.valueCount;
++rowValueCount;
}
}
SA.row_ptrs[i + 1] = SA.row_ptrs[(i + 1) - 1] + rowNzCount;
SA.row_ptrs[i + 1] = SA.row_ptrs[(i + 1) - 1] + rowValueCount;
}
B3_ASSERT(nzCount <= SA.valueCount);
SA.valueCount = nzCount;
B3_ASSERT(SA.valueCount <= valueCapacity);
m_allocator->Free(A);
@ -525,26 +513,34 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
// p = S * (P^-1 * r)
b3DenseVec3 p = S * (inv_P * r);
// deltaNew = dot(r, p)
float32 deltaNew = b3Dot(r, p);
// delta_new = dot(r, p)
float32 delta_new = b3Dot(r, p);
// Tolerance.
// This is the main stopping criteria.
// [0, 1]
const float32 tolerance = 10.0f * B3_EPSILON;
// Set the tolerance.
const float32 tolerance = 1.e-4f;
// Maximum number of iterations.
const u32 maxIters = 1000;
// Stop at this iteration if diverged.
const u32 max_iterations = 100;
u32 iteration = 0;
// Main iteration loop.
u32 iter = 0;
while (deltaNew > tolerance * tolerance * b_delta && iter < maxIters)
while (iteration < max_iterations)
{
B3_ASSERT(b3IsValid(delta_new));
// Convergence check.
if (delta_new <= tolerance * tolerance * b_delta)
{
break;
}
// s = S * (A * p)
b3DenseVec3 s = S * (A * p);
// alpha = deltaNew / dot(c, q)
float32 alpha = deltaNew / b3Dot(p, s);
// alpha = delta_new / dot(p, s)
float32 alpha = delta_new / b3Dot(p, s);
// x = x + alpha * p
x = x + alpha * p;
@ -555,21 +551,20 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
// h = inv_P * r
b3DenseVec3 h = inv_P * r;
// deltaOld = deltaNew
float32 deltaOld = deltaNew;
// delta_old = delta_new
float32 delta_old = delta_new;
// deltaNew = dot(r, h)
deltaNew = b3Dot(r, h);
//B3_ASSERT(b3IsValid(deltaNew));
// delta_new = dot(r, h)
delta_new = b3Dot(r, h);
// beta = deltaNew / deltaOld
float32 beta = deltaNew / deltaOld;
// beta = delta_new / delta_old
float32 beta = delta_new / delta_old;
// p = S * (h + beta * p)
p = S * (h + beta * p);
++iter;
++iteration;
}
iterations = iter;
iterations = iteration;
}