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

typo, optimization, consistency

Browse Source
This commit is contained in:
Irlan 2018-05-25 22:15:00 -03:00
parent 03be41d05b
commit 8d2affb0b2
4 changed files with 144 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -64,9 +64,9 @@ struct b3ClothDef
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.
// Static particle: Has zero mass, can be moved manually.
// Kinematic particle: Has zero mass, non-zero velocity, can be moved by the solver.
// Dynamic particle: Has non-zero mass, non-zero velocity determined by force, can be moved by the solver.
enum b3ParticleType
{
e_staticParticle,
@ -98,10 +98,10 @@ struct b3Particle
// Radius
float32 radius;
// User data
// User data.
void* userData;
// Translation used for direct position manipulation
// Applied external translation
b3Vec3 translation;
// Solver temp
@ -125,6 +125,8 @@ struct b3ClothSolverData;
// Read-only spring
struct b3Spring
{
// Solver shared
// Spring type
b3SpringType type;
@ -151,8 +153,8 @@ struct b3Spring
// Jacobian (J_ii entry)
b3Mat33 Jx, Jv;
// Initialize solver data.
void Initialize(const b3ClothSolverData* data);
// Apply spring forces.
void ApplyForces(const b3ClothSolverData* data);
};
// Read-only contact
@ -160,6 +162,7 @@ struct b3ParticleContact
{
b3Particle* p1;
b3Shape* s2;
float32 s;
b3Vec3 n, t1, t2;
float32 Fn, Ft1, Ft2;
bool n_active, t1_active, t2_active;
@ -254,8 +257,8 @@ protected:
u32 m_particleCount;
b3Particle* m_particles;
b3Spring* m_springs;
u32 m_springCount;
b3Spring* m_springs;
b3ParticleContact* m_contacts;
//u32 m_contactCount;

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

@ -50,6 +50,13 @@ struct b3ClothSolverData
float32 invdt;
};
struct b3AccelerationConstraint
{
u32 i1;
u32 ndof;
b3Vec3 p, q, z;
};
class b3ClothSolver
{
public:
@ -62,17 +69,16 @@ public:
void Solve(float32 dt, const b3Vec3& gravity);
private:
// 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;
// Compute S.
void Compute_S(b3DiagMat33& S);
// Compute z.
void Compute_z(b3DenseVec3& z);
// Compute S and z.
void Compute_S_z(b3DiagMat33& S, b3DenseVec3& z);
// Solve Ax = b.
// Output x and the residual error f = Ax - b ~ 0.
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;
@ -89,6 +95,10 @@ private:
u32 m_contactCount;
b3ParticleContact** m_contacts;
u32 m_constraintCapacity;
u32 m_constraintCount;
b3AccelerationConstraint* m_constraints;
b3ClothSolverData m_solverData;
};

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

@ -32,7 +32,7 @@
#define B3_CLOTH_FRICTION 0
// b3Spring
void b3Spring::Initialize(const b3ClothSolverData* data)
void b3Spring::ApplyForces(const b3ClothSolverData* data)
{
u32 i1 = p1->solverId;
u32 i2 = p2->solverId;
@ -43,7 +43,7 @@ void b3Spring::Initialize(const b3ClothSolverData* data)
b3Vec3 x2 = data->x[i2];
b3Vec3 v2 = data->v[i2];
const b3Mat33 I = b3Mat33_identity;
b3Mat33 I; I.SetIdentity();
b3Vec3 dx = x1 - x2;
@ -145,7 +145,7 @@ static u32 b3FindUniqueEdges(b3UniqueEdge* uniqueEdges, const b3ClothMesh* m)
unique = false;
break;
}
if (ue->v2 == t1v1 && ue->v1 == t1v2)
{
unique = false;
@ -238,13 +238,14 @@ void b3Cloth::Initialize(const b3ClothDef& def)
const b3ClothMesh* m = m_mesh;
// Create particles
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;
b3Particle* p = m_particles + i;
p->type = e_dynamicParticle;
p->position = m->vertices[i];
p->velocity.SetZero();
@ -266,23 +267,23 @@ void b3Cloth::Initialize(const b3ClothDef& def)
// Compute mass
ResetMass();
// Initialize springs
// Create 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));
@ -340,7 +341,7 @@ void b3Cloth::Initialize(const b3ClothDef& def)
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;
@ -368,7 +369,7 @@ void b3Cloth::ResetMass()
const float32 inv3 = 1.0f / 3.0f;
const float32 rho = m_density;
// Accumulate the mass about the body origin of all triangles.
// Accumulate the mass about the mesh origin of all triangles.
for (u32 i = 0; i < m_mesh->triangleCount; ++i)
{
b3ClothMeshTriangle* triangle = m_mesh->triangles + i;
@ -413,7 +414,7 @@ void b3Cloth::AddShape(b3Shape* shape)
void b3Cloth::UpdateContacts()
{
B3_PROFILE("Update Contacts");
// Clear active flags
for (u32 i = 0; i < m_particleCount; ++i)
{
@ -477,23 +478,21 @@ void b3Cloth::UpdateContacts()
b3Vec3 n = bestNormal;
// Update contact manifold
// Remember the normal orientation is from shape 2 to shape 1 (mass)
// Remember the normal points from shape 2 to shape 1 (mass)
c->n_active = true;
c->p1 = p;
c->s2 = shape;
c->s = s;
c->n = n;
c->t1 = b3Perp(n);
c->t2 = b3Cross(c->t1, n);
// Apply position correction
p->translation -= s * n;
}
// Update contact state
if (c0.n_active == true && c->n_active == true)
{
// The contact persists
// Has the contact constraint been satisfied?
if (c0.Fn <= -B3_FORCE_THRESHOLD)
{
@ -542,7 +541,7 @@ void b3Cloth::UpdateContacts()
{
// Create a dynamic basis
t1.Normalize();
b3Vec3 t2 = b3Cross(t1, n);
t2.Normalize();
@ -589,7 +588,7 @@ void b3Cloth::UpdateContacts()
{
// The contact persists
float32 maxForce = u * normalForce;
if (Ft0[k] * Ft0[k] > maxForce * maxForce)
{
// Unlock particle off surface
@ -686,19 +685,19 @@ void b3Cloth::Draw() const
{
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);
@ -716,7 +715,7 @@ void b3Cloth::Draw() const
b3Draw_draw->DrawSegment(p1->position, p2->position, b3Color_black);
}
}
const b3ClothMesh* m = m_mesh;
for (u32 i = 0; i < m->sewingLineCount; ++i)

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

@ -47,11 +47,16 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
m_contactCapacity = def.contactCapacity;
m_contactCount = 0;
m_contacts = (b3ParticleContact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3ParticleContact*));;
m_contacts = (b3ParticleContact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3ParticleContact*));
m_constraintCapacity = def.particleCapacity;
m_constraintCount = 0;
m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_constraintCapacity * sizeof(b3AccelerationConstraint));
}
b3ClothSolver::~b3ClothSolver()
{
m_allocator->Free(m_constraints);
m_allocator->Free(m_contacts);
m_allocator->Free(m_springs);
m_allocator->Free(m_particles);
@ -73,6 +78,56 @@ void b3ClothSolver::Add(b3Spring* s)
m_springs[m_springCount++] = s;
}
void b3ClothSolver::InitializeConstraints()
{
for (u32 i = 0; i < m_particleCount; ++i)
{
b3Particle* p = m_particles[i];
if (p->type == e_staticParticle)
{
b3AccelerationConstraint* ac = m_constraints + m_constraintCount;
++m_constraintCount;
ac->i1 = i;
ac->ndof = 0;
ac->z.SetZero();
}
}
for (u32 i = 0; i < m_contactCount; ++i)
{
b3ParticleContact* 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;
ac->ndof = 2;
ac->p = pc->n;
ac->z.SetZero();
if (pc->t1_active && pc->t2_active)
{
ac->ndof = 0;
}
else
{
if (pc->t1_active)
{
ac->ndof = 1;
ac->q = pc->t1;
}
if (pc->t2_active)
{
ac->ndof = 1;
ac->q = pc->t2;
}
}
}
}
static B3_FORCE_INLINE b3SparseMat33 b3AllocSparse(b3StackAllocator* allocator, u32 M, u32 N)
{
u32 size = M * N;
@ -124,13 +179,27 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
sx0[i] = p->x;
}
// Initialize spring forces
// Apply contact position corrections
for (u32 i = 0; i < m_contactCount; ++i)
{
b3ParticleContact* c = m_contacts[i];
b3Particle* p = c->p1;
sy[p->solverId] -= c->s * c->n;
}
// Apply spring forces and derivatives
for (u32 i = 0; i < m_springCount; ++i)
{
b3Spring* s = m_springs[i];
s->Initialize(&m_solverData);
m_springs[i]->ApplyForces(&m_solverData);
}
// Initialize constraints
InitializeConstraints();
b3DiagMat33 S(m_particleCount);
b3DenseVec3 z(m_particleCount);
Compute_S_z(S, z);
// Solve Ax = b, where
// A = M - h * dfdv - h * h * dfdx
// b = h * (f0 + h * dfdx * v0 + dfdx * y)
@ -143,14 +212,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
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);
// x
b3DenseVec3 x(m_particleCount);
@ -379,67 +440,47 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense
b = h * (f + h * Jx_v + Jx_y);
}
void b3ClothSolver::Compute_z(b3DenseVec3& z)
void b3ClothSolver::Compute_S_z(b3DiagMat33& S, b3DenseVec3& z)
{
// TODO
S.SetIdentity();
z.SetZero();
}
void b3ClothSolver::Compute_S(b3DiagMat33& out)
{
for (u32 i = 0; i < m_particleCount; ++i)
for (u32 i = 0; i < m_constraintCount; ++i)
{
b3Particle* p = m_particles[i];
b3AccelerationConstraint* ac = m_constraints + i;
u32 ip = ac->i1;
u32 ndof = ac->ndof;
b3Vec3 p = ac->p;
b3Vec3 q = ac->q;
b3Vec3 cz = ac->z;
if (p->type == e_staticParticle)
z[ip] = cz;
if (ndof == 2)
{
out[i].SetZero();
continue;
b3Mat33 I; I.SetIdentity();
S[ip] = I - b3Outer(p, p);
}
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)
if (ndof == 1)
{
S.SetZero();
}
else
{
if (c->t1_active == true)
{
b3Vec3 t1 = c->t1;
b3Mat33 I; I.SetIdentity();
S -= b3Outer(t1, t1);
}
if (c->t2_active == true)
{
b3Vec3 t2 = c->t2;
S -= b3Outer(t2, t2);
}
S[ip] = I - b3Outer(p, p) - b3Outer(q, q);
}
b3Particle* p = c->p1;
out[p->solverId] = S;
if (ndof == 0)
{
S[ip].SetZero();
}
}
}
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");
B3_PROFILE("Solve Ax = b");
// P = diag(A)
b3DiagMat33 inv_P(m_particleCount);