remove some inefficient code, add some test code
This commit is contained in:
Irlan
parent
c638a0a52c
commit
756c4d354a
@ -156,9 +156,6 @@ private:
|
||||
|
||||
// Update particle contacts.
|
||||
void UpdateParticleContacts();
|
||||
|
||||
// Update triangle contacts.
|
||||
void UpdateTriangleContacts();
|
||||
|
||||
// Update contacts
|
||||
void UpdateContacts();
|
||||
@ -192,10 +189,7 @@ private:
|
||||
|
||||
// Pool of particle contacts
|
||||
b3BlockPool m_particleContactBlocks;
|
||||
|
||||
// Pool of triangle contacts
|
||||
b3BlockPool m_triangleContactBlocks;
|
||||
|
||||
|
||||
// List of particles
|
||||
b3List2<b3Particle> m_particleList;
|
||||
|
||||
|
||||
@ -149,8 +149,7 @@ static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m)
|
||||
b3Cloth::b3Cloth(const b3ClothDef& def) :
|
||||
m_particleBlocks(sizeof(b3Particle)),
|
||||
m_bodyContactBlocks(sizeof(b3BodyContact)),
|
||||
m_particleContactBlocks(sizeof(b3ParticleContact)),
|
||||
m_triangleContactBlocks(sizeof(b3TriangleContact))
|
||||
m_particleContactBlocks(sizeof(b3ParticleContact))
|
||||
{
|
||||
B3_ASSERT(def.mesh);
|
||||
B3_ASSERT(def.density > 0.0f);
|
||||
@ -624,241 +623,6 @@ void b3Cloth::UpdateParticleContacts()
|
||||
}
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3Barycentric(float32 out[3],
|
||||
const b3Vec3& A, const b3Vec3& B,
|
||||
const b3Vec3& Q)
|
||||
{
|
||||
b3Vec3 AB = B - A;
|
||||
b3Vec3 QA = A - Q;
|
||||
b3Vec3 QB = B - Q;
|
||||
|
||||
//float32 divisor = b3Dot(AB, AB);
|
||||
|
||||
out[0] = b3Dot(QB, AB);
|
||||
out[1] = -b3Dot(QA, AB);
|
||||
out[2] = out[0] + out[1];
|
||||
}
|
||||
|
||||
// Convert a point Q from Cartesian coordinates to Barycentric coordinates (u, v, w)
|
||||
// with respect to a triangle ABC.
|
||||
// The last output value is the divisor.
|
||||
static B3_FORCE_INLINE void b3Barycentric(float32 out[4],
|
||||
const b3Vec3& A, const b3Vec3& B, const b3Vec3& C,
|
||||
const b3Vec3& Q)
|
||||
{
|
||||
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 divisor = b3Dot(AB_x_AC, AB_x_AC);
|
||||
|
||||
out[0] = b3Dot(QB_x_QC, AB_x_AC);
|
||||
out[1] = b3Dot(QC_x_QA, AB_x_AC);
|
||||
out[2] = b3Dot(QA_x_QB, AB_x_AC);
|
||||
out[3] = out[0] + out[1] + out[2];
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3Solve3(float32 out[3],
|
||||
const b3Vec3& A, const b3Vec3& B, const b3Vec3& C,
|
||||
const b3Vec3& Q)
|
||||
{
|
||||
// Test vertex regions
|
||||
float32 wAB[3], wBC[3], wCA[3];
|
||||
b3Barycentric(wAB, A, B, Q);
|
||||
b3Barycentric(wBC, B, C, Q);
|
||||
b3Barycentric(wCA, C, A, Q);
|
||||
|
||||
// R A
|
||||
if (wAB[1] <= 0.0f && wCA[0] <= 0.0f)
|
||||
{
|
||||
out[0] = 1.0f;
|
||||
out[1] = 0.0f;
|
||||
out[2] = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
// R B
|
||||
if (wAB[0] <= 0.0f && wBC[1] <= 0.0f)
|
||||
{
|
||||
out[0] = 0.0f;
|
||||
out[1] = 1.0f;
|
||||
out[2] = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
// R C
|
||||
if (wBC[0] <= 0.0f && wCA[1] <= 0.0f)
|
||||
{
|
||||
out[0] = 0.0f;
|
||||
out[1] = 0.0f;
|
||||
out[2] = 1.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
// Test edge regions
|
||||
float32 wABC[4];
|
||||
b3Barycentric(wABC, A, B, C, Q);
|
||||
|
||||
// This is used to help testing if the face degenerates
|
||||
// into an edge.
|
||||
float32 area = wABC[3];
|
||||
|
||||
// R AB
|
||||
if (wAB[0] > 0.0f && wAB[1] > 0.0f && area * wABC[2] <= 0.0f)
|
||||
{
|
||||
float32 divisor = wAB[2];
|
||||
B3_ASSERT(divisor > 0.0f);
|
||||
float32 s = 1.0f / divisor;
|
||||
out[0] = s * wAB[0];
|
||||
out[1] = s * wAB[1];
|
||||
out[2] = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
// R BC
|
||||
if (wBC[0] > 0.0f && wBC[1] > 0.0f && area * wABC[0] <= 0.0f)
|
||||
{
|
||||
float32 divisor = wBC[2];
|
||||
B3_ASSERT(divisor > 0.0f);
|
||||
float32 s = 1.0f / divisor;
|
||||
out[0] = 0.0f;
|
||||
out[1] = s * wBC[0];
|
||||
out[2] = s * wBC[1];
|
||||
return;
|
||||
}
|
||||
|
||||
// R CA
|
||||
if (wCA[0] > 0.0f && wCA[1] > 0.0f && area * wABC[1] <= 0.0f)
|
||||
{
|
||||
float32 divisor = wCA[2];
|
||||
B3_ASSERT(divisor > 0.0f);
|
||||
float32 s = 1.0f / divisor;
|
||||
out[0] = 0.0f;
|
||||
out[2] = s * wCA[0];
|
||||
out[1] = s * wCA[1];
|
||||
return;
|
||||
}
|
||||
|
||||
// R ABC/ACB
|
||||
float32 divisor = wABC[3];
|
||||
if (divisor <= 0.0f)
|
||||
{
|
||||
float32 s = 1.0f / 3.0f;
|
||||
out[0] = s;
|
||||
out[2] = s;
|
||||
out[1] = s;
|
||||
return;
|
||||
}
|
||||
|
||||
B3_ASSERT(divisor > 0.0f);
|
||||
float32 s = 1.0f / divisor;
|
||||
out[0] = s * wABC[0];
|
||||
out[1] = s * wABC[1];
|
||||
out[2] = s * wABC[2];
|
||||
}
|
||||
|
||||
void b3Cloth::UpdateTriangleContacts()
|
||||
{
|
||||
B3_PROFILE("Cloth Update Triangle Contacts");
|
||||
|
||||
// Clear buffer
|
||||
b3TriangleContact* c = m_triangleContactList.m_head;
|
||||
while (c)
|
||||
{
|
||||
b3TriangleContact* c0 = c;
|
||||
c = c->m_next;
|
||||
m_triangleContactList.Remove(c0);
|
||||
c0->~b3TriangleContact();
|
||||
m_triangleContactBlocks.Free(c0);
|
||||
}
|
||||
|
||||
// Create triangle contacts
|
||||
for (b3Particle* p1 = m_particleList.m_head; p1; p1 = p1->m_next)
|
||||
{
|
||||
for (u32 i = 0; i < m_mesh->triangleCount; ++i)
|
||||
{
|
||||
b3ClothMeshTriangle* triangle = m_mesh->triangles + i;
|
||||
|
||||
b3Particle* p2 = m_vertexParticles[triangle->v1];
|
||||
b3Particle* p3 = m_vertexParticles[triangle->v2];
|
||||
b3Particle* p4 = m_vertexParticles[triangle->v3];
|
||||
|
||||
if (p1 == p2 || p1 == p3 || p1 == p4)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
float32 r1 = p1->m_radius;
|
||||
float32 r2 = 0.0f;
|
||||
|
||||
float32 totalRadius = r1 + r2;
|
||||
|
||||
b3Vec3 A = p2->m_position;
|
||||
b3Vec3 B = p3->m_position;
|
||||
b3Vec3 C = p4->m_position;
|
||||
|
||||
b3Vec3 P = p1->m_position;
|
||||
|
||||
b3Vec3 n = b3Cross(B - A, C - A);
|
||||
float32 n_len = n.Normalize();
|
||||
|
||||
float32 distance = b3Dot(n, P - A);
|
||||
|
||||
if (distance < -totalRadius)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (distance > totalRadius)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// Project particle on triangle plane
|
||||
b3Vec3 Q = P - distance * n;
|
||||
|
||||
// Barycentric coordinates for Q
|
||||
float32 wABC[3];
|
||||
b3Solve3(wABC, A, B, C, Q);
|
||||
|
||||
b3Vec3 c1 = p1->m_position;
|
||||
b3Vec3 c2 = wABC[0] * A + wABC[1] * B + wABC[2] * C;
|
||||
|
||||
if (b3DistanceSquared(c1, c2) > totalRadius * totalRadius)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
bool front = false;
|
||||
|
||||
// Is the the other point in front of the triangle plane?
|
||||
if (distance >= 0.0f)
|
||||
{
|
||||
front = true;
|
||||
}
|
||||
|
||||
b3TriangleContact* c = (b3TriangleContact*)m_triangleContactBlocks.Allocate();
|
||||
c->p1 = p1;
|
||||
c->p2 = p2;
|
||||
c->p3 = p3;
|
||||
c->p4 = p4;
|
||||
c->front = front;
|
||||
c->normalImpulse = 0.0f;
|
||||
|
||||
m_triangleContactList.PushFront(c);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
|
||||
{
|
||||
B3_PROFILE("Cloth Solve");
|
||||
@ -918,9 +682,6 @@ void b3Cloth::UpdateContacts()
|
||||
#if 0
|
||||
// Update particle contacts
|
||||
UpdateParticleContacts();
|
||||
|
||||
// Update triangle contacts
|
||||
UpdateTriangleContacts();
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@ -155,6 +155,7 @@ void b3ClothSolver::ApplyConstraints()
|
||||
{
|
||||
b3DiagMat33& S = *m_solverData.S;
|
||||
b3DenseVec3& z = *m_solverData.z;
|
||||
b3DenseVec3& x = *m_solverData.x;
|
||||
|
||||
S.SetIdentity();
|
||||
z.SetZero();
|
||||
@ -172,6 +173,35 @@ void b3ClothSolver::ApplyConstraints()
|
||||
}
|
||||
}
|
||||
|
||||
#if 0
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3BodyContact* bc = m_bodyContacts[i];
|
||||
|
||||
b3Particle* p1 = bc->p1;
|
||||
|
||||
B3_ASSERT(p1->m_type == e_dynamicParticle);
|
||||
|
||||
b3Transform xf1;
|
||||
xf1.position = x[p1->m_solverId];
|
||||
xf1.rotation.SetIdentity();
|
||||
|
||||
b3Shape* s2 = bc->s2;
|
||||
b3Body* b2 = s2->GetBody();
|
||||
b3Transform xf2 = b2->GetTransform();
|
||||
|
||||
b3BodyContactWorldPoint bcwp;
|
||||
bcwp.Initialize(bc, p1->m_radius, xf1, s2->m_radius, xf2);
|
||||
|
||||
b3AccelerationConstraint* ac = m_constraints + m_constraintCount;
|
||||
++m_constraintCount;
|
||||
ac->i1 = p1->m_solverId;
|
||||
ac->ndof = 2;
|
||||
ac->p = bcwp.normal;
|
||||
ac->z.SetZero();
|
||||
}
|
||||
#endif
|
||||
|
||||
for (u32 i = 0; i < m_constraintCount; ++i)
|
||||
{
|
||||
m_constraints[i].Apply(&m_solverData);
|
||||
@ -223,6 +253,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
|
||||
m_solverData.S = &S;
|
||||
m_solverData.z = &z;
|
||||
|
||||
// Apply position correction
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_particles[i];
|
||||
@ -230,7 +261,27 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
|
||||
sy[i] = p->m_translation;
|
||||
sx0[i] = p->m_x;
|
||||
}
|
||||
|
||||
#if 0
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3BodyContact* bc = m_bodyContacts[i];
|
||||
|
||||
b3Particle* p1 = bc->p1;
|
||||
b3Transform xf1;
|
||||
xf1.position = sx[p1->m_solverId];
|
||||
xf1.rotation.SetIdentity();
|
||||
|
||||
b3Shape* s2 = bc->s2;
|
||||
b3Body* b2 = s2->GetBody();
|
||||
b3Transform xf2 = b2->GetTransform();
|
||||
|
||||
b3BodyContactWorldPoint bcwp;
|
||||
bcwp.Initialize(bc, p1->m_radius, xf1, s2->m_radius, xf2);
|
||||
|
||||
sy[p1->m_solverId] += bcwp.separation * bcwp.normal;
|
||||
}
|
||||
#endif
|
||||
// Apply internal forces
|
||||
ApplyForces();
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user