Keep particles and triangles in a tree
This commit is contained in:
Irlan
parent
99270a70b9
commit
d3b6292afd
@ -19,11 +19,11 @@
|
||||
#ifndef B3_CLOTH_H
|
||||
#define B3_CLOTH_H
|
||||
|
||||
#include <bounce/common/math/transform.h>
|
||||
#include <bounce/common/template/list.h>
|
||||
#include <bounce/common/memory/stack_allocator.h>
|
||||
#include <bounce/common/memory/block_pool.h>
|
||||
#include <bounce/collision/trees/dynamic_tree.h>
|
||||
#include <bounce/common/math/transform.h>
|
||||
#include <bounce/cloth/cloth_contact_manager.h>
|
||||
|
||||
class b3World;
|
||||
class b3Shape;
|
||||
@ -42,6 +42,8 @@ class b3RayCastListener;
|
||||
struct b3RayCastInput;
|
||||
struct b3RayCastOutput;
|
||||
|
||||
struct b3ClothAABBProxy;
|
||||
|
||||
struct b3ClothRayCastSingleOutput
|
||||
{
|
||||
u32 triangle;
|
||||
@ -140,16 +142,23 @@ public:
|
||||
void Draw() const;
|
||||
private:
|
||||
friend class b3Particle;
|
||||
friend class b3ClothContactManager;
|
||||
|
||||
// Compute mass of each particle.
|
||||
void ComputeMass();
|
||||
|
||||
// Update contacts
|
||||
void UpdateContacts();
|
||||
// Update particle-body contacts
|
||||
void UpdateParticleBodyContacts();
|
||||
|
||||
// Solve
|
||||
void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations);
|
||||
|
||||
// Synchronize triangle AABB.
|
||||
void SynchronizeTriangle(u32 triangleIndex);
|
||||
|
||||
// Time-step
|
||||
float32 m_dt;
|
||||
|
||||
// Stack allocator
|
||||
b3StackAllocator m_stackAllocator;
|
||||
|
||||
@ -165,6 +174,9 @@ private:
|
||||
// Vertex particles
|
||||
b3Particle** m_vertexParticles;
|
||||
|
||||
// Triangle proxies
|
||||
b3ClothAABBProxy* m_triangleProxies;
|
||||
|
||||
// Cloth density
|
||||
float32 m_density;
|
||||
|
||||
@ -174,11 +186,11 @@ private:
|
||||
// List of particles
|
||||
b3List2<b3Particle> m_particleList;
|
||||
|
||||
// Particle tree
|
||||
b3DynamicTree m_particleTree;
|
||||
|
||||
// List of forces
|
||||
b3List2<b3Force> m_forceList;
|
||||
|
||||
// Contact manager
|
||||
b3ClothContactManager m_contactManager;
|
||||
};
|
||||
|
||||
inline void b3Cloth::SetGravity(const b3Vec3& gravity)
|
||||
|
||||
81
include/bounce/cloth/cloth_contact_manager.h
Normal file
81
include/bounce/cloth/cloth_contact_manager.h
Normal file
@ -0,0 +1,81 @@
|
||||
/*
|
||||
* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
|
||||
*
|
||||
* 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_CLOTH_CONTACT_MANAGER_H
|
||||
#define B3_CLOTH_CONTACT_MANAGER_H
|
||||
|
||||
#include <bounce/common/memory/block_pool.h>
|
||||
#include <bounce/common/template/list.h>
|
||||
#include <bounce/collision/broad_phase.h>
|
||||
|
||||
class b3Cloth;
|
||||
class b3Particle;
|
||||
struct b3ClothMeshTriangle;
|
||||
struct b3ClothAABBProxy;
|
||||
|
||||
// Contact between particle and a triangle
|
||||
class b3ParticleTriangleContact
|
||||
{
|
||||
public:
|
||||
b3Particle* m_p1;
|
||||
|
||||
b3ClothMeshTriangle* m_triangle;
|
||||
b3ClothAABBProxy* m_triangleProxy;
|
||||
b3Particle* m_p2;
|
||||
b3Particle* m_p3;
|
||||
b3Particle* m_p4;
|
||||
|
||||
bool m_front;
|
||||
|
||||
bool m_active;
|
||||
|
||||
float32 m_normalImpulse;
|
||||
|
||||
b3ParticleTriangleContact* m_prev;
|
||||
b3ParticleTriangleContact* m_next;
|
||||
};
|
||||
|
||||
// Contact delegator for b3Cloth.
|
||||
class b3ClothContactManager
|
||||
{
|
||||
public:
|
||||
b3ClothContactManager();
|
||||
|
||||
// The broad-phase callback.
|
||||
void AddPair(void* data1, void* data2);
|
||||
|
||||
void FindNewContacts();
|
||||
|
||||
void UpdateContacts();
|
||||
|
||||
b3ParticleTriangleContact* Create();
|
||||
|
||||
void Destroy(b3ParticleTriangleContact* c);
|
||||
|
||||
void Update(b3ParticleTriangleContact* c);
|
||||
|
||||
b3Cloth* m_cloth;
|
||||
|
||||
b3BlockPool m_particleTriangleContactBlocks;
|
||||
|
||||
b3BroadPhase m_broadPhase;
|
||||
|
||||
b3List2<b3ParticleTriangleContact> m_particleTriangleContactList;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -28,6 +28,7 @@ class b3Particle;
|
||||
class b3Body;
|
||||
|
||||
struct b3ParticleBodyContact;
|
||||
class b3ParticleTriangleContact;
|
||||
|
||||
struct b3DenseVec3;
|
||||
|
||||
@ -80,6 +81,44 @@ struct b3ClothSolverBodyContactPositionConstraint
|
||||
b3Vec3 localPointB;
|
||||
};
|
||||
|
||||
struct b3ClothSolverTriangleContactVelocityConstraint
|
||||
{
|
||||
u32 indexA;
|
||||
float32 invMassA;
|
||||
|
||||
u32 indexB;
|
||||
float32 invMassB;
|
||||
u32 indexC;
|
||||
float32 invMassC;
|
||||
u32 indexD;
|
||||
float32 invMassD;
|
||||
|
||||
b3Vec3 JA;
|
||||
b3Vec3 JB;
|
||||
b3Vec3 JC;
|
||||
b3Vec3 JD;
|
||||
|
||||
float32 normalMass;
|
||||
float32 normalImpulse;
|
||||
};
|
||||
|
||||
struct b3ClothSolverTriangleContactPositionConstraint
|
||||
{
|
||||
u32 indexA;
|
||||
float32 invMassA;
|
||||
float32 radiusA;
|
||||
|
||||
u32 indexB;
|
||||
float32 invMassB;
|
||||
u32 indexC;
|
||||
float32 invMassC;
|
||||
u32 indexD;
|
||||
float32 invMassD;
|
||||
float32 triangleRadius;
|
||||
|
||||
bool front;
|
||||
};
|
||||
|
||||
struct b3ClothContactSolverDef
|
||||
{
|
||||
b3StackAllocator* allocator;
|
||||
@ -89,6 +128,9 @@ struct b3ClothContactSolverDef
|
||||
|
||||
u32 bodyContactCount;
|
||||
b3ParticleBodyContact** bodyContacts;
|
||||
|
||||
u32 triangleContactCount;
|
||||
b3ParticleTriangleContact** triangleContacts;
|
||||
};
|
||||
|
||||
inline float32 b3MixFriction(float32 u1, float32 u2)
|
||||
@ -103,14 +145,18 @@ public:
|
||||
~b3ClothContactSolver();
|
||||
|
||||
void InitializeBodyContactConstraints();
|
||||
void InitializeTriangleContactConstraints();
|
||||
|
||||
void WarmStart();
|
||||
void WarmStartBodyContactConstraints();
|
||||
void WarmStartTriangleContactConstraints();
|
||||
|
||||
void SolveBodyContactVelocityConstraints();
|
||||
void SolveTriangleContactVelocityConstraints();
|
||||
|
||||
void StoreImpulses();
|
||||
|
||||
bool SolveBodyContactPositionConstraints();
|
||||
bool SolveTriangleContactPositionConstraints();
|
||||
protected:
|
||||
b3StackAllocator* m_allocator;
|
||||
|
||||
@ -121,6 +167,11 @@ protected:
|
||||
b3ParticleBodyContact** m_bodyContacts;
|
||||
b3ClothSolverBodyContactVelocityConstraint* m_bodyVelocityConstraints;
|
||||
b3ClothSolverBodyContactPositionConstraint* m_bodyPositionConstraints;
|
||||
|
||||
u32 m_triangleContactCount;
|
||||
b3ParticleTriangleContact** m_triangleContacts;
|
||||
b3ClothSolverTriangleContactVelocityConstraint* m_triangleVelocityConstraints;
|
||||
b3ClothSolverTriangleContactPositionConstraint* m_trianglePositionConstraints;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -34,6 +34,7 @@ struct b3SparseSymMat33;
|
||||
struct b3SparseSymMat33View;
|
||||
|
||||
struct b3ParticleBodyContact;
|
||||
class b3ParticleTriangleContact;
|
||||
|
||||
struct b3ClothSolverDef
|
||||
{
|
||||
@ -41,6 +42,7 @@ struct b3ClothSolverDef
|
||||
u32 particleCapacity;
|
||||
u32 forceCapacity;
|
||||
u32 bodyContactCapacity;
|
||||
u32 triangleContactCapacity;
|
||||
};
|
||||
|
||||
struct b3ClothSolverData
|
||||
@ -75,6 +77,7 @@ public:
|
||||
void Add(b3Particle* p);
|
||||
void Add(b3Force* f);
|
||||
void Add(b3ParticleBodyContact* c);
|
||||
void Add(b3ParticleTriangleContact* c);
|
||||
|
||||
void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations);
|
||||
private:
|
||||
@ -107,6 +110,10 @@ private:
|
||||
u32 m_bodyContactCount;
|
||||
b3ParticleBodyContact** m_bodyContacts;
|
||||
|
||||
u32 m_triangleContactCapacity;
|
||||
u32 m_triangleContactCount;
|
||||
b3ParticleTriangleContact** m_triangleContacts;
|
||||
|
||||
b3ClothSolverData m_solverData;
|
||||
};
|
||||
|
||||
|
||||
@ -44,6 +44,7 @@ struct b3ParticleDef
|
||||
b3ParticleDef()
|
||||
{
|
||||
type = e_staticParticle;
|
||||
mass = 0.0f;
|
||||
position.SetZero();
|
||||
velocity.SetZero();
|
||||
force.SetZero();
|
||||
@ -53,6 +54,7 @@ struct b3ParticleDef
|
||||
}
|
||||
|
||||
b3ParticleType type;
|
||||
float32 mass;
|
||||
b3Vec3 position;
|
||||
b3Vec3 velocity;
|
||||
b3Vec3 force;
|
||||
@ -89,6 +91,20 @@ struct b3ParticleBodyContactWorldPoint
|
||||
float32 separation;
|
||||
};
|
||||
|
||||
enum b3ClothAABBProxyType
|
||||
{
|
||||
e_particleProxy,
|
||||
e_triangleProxy
|
||||
};
|
||||
|
||||
struct b3ClothAABBProxy
|
||||
{
|
||||
b3ClothAABBProxyType type;
|
||||
void* data;
|
||||
u32 index;
|
||||
u32 broadPhaseId;
|
||||
};
|
||||
|
||||
// A cloth particle.
|
||||
class b3Particle
|
||||
{
|
||||
@ -143,18 +159,23 @@ private:
|
||||
friend class b3List2<b3Particle>;
|
||||
friend class b3Cloth;
|
||||
friend class b3ClothSolver;
|
||||
friend class b3ClothContactManager;
|
||||
friend class b3ClothContactSolver;
|
||||
friend class b3Force;
|
||||
friend class b3SpringForce;
|
||||
friend class b3BendForce;
|
||||
friend class b3FrictionForce;
|
||||
|
||||
b3Particle(const b3ParticleDef& def, b3Cloth* cloth);
|
||||
~b3Particle();
|
||||
|
||||
// Synchronize AABB
|
||||
// Synchronize particle AABB
|
||||
void Synchronize();
|
||||
|
||||
// Synchronize triangles AABB
|
||||
void SynchronizeTriangles();
|
||||
|
||||
// Destroy contacts.
|
||||
void DestroyContacts();
|
||||
|
||||
// Type
|
||||
b3ParticleType m_type;
|
||||
|
||||
@ -202,8 +223,8 @@ private:
|
||||
// Contact
|
||||
b3ParticleBodyContact m_bodyContact;
|
||||
|
||||
// Particle tree identifier
|
||||
u32 m_treeId;
|
||||
// AABB Proxy
|
||||
b3ClothAABBProxy m_aabbProxy;
|
||||
|
||||
//
|
||||
b3Particle* m_prev;
|
||||
@ -228,6 +249,7 @@ inline void b3Particle::SetPosition(const b3Vec3& position)
|
||||
m_translation.SetZero();
|
||||
|
||||
Synchronize();
|
||||
SynchronizeTriangles();
|
||||
}
|
||||
|
||||
inline const b3Vec3& b3Particle::GetPosition() const
|
||||
|
||||
@ -32,6 +32,8 @@
|
||||
|
||||
#include <bounce/common/draw.h>
|
||||
|
||||
#define B3_ENABLE_SELF_COLLISION 0
|
||||
|
||||
static B3_FORCE_INLINE u32 b3NextIndex(u32 i)
|
||||
{
|
||||
return i + 1 < 3 ? i + 1 : 0;
|
||||
@ -158,6 +160,8 @@ b3Cloth::b3Cloth(const b3ClothDef& def) :
|
||||
|
||||
m_mesh = def.mesh;
|
||||
m_density = def.density;
|
||||
m_contactManager.m_cloth = this;
|
||||
m_dt = 0.0f;
|
||||
|
||||
const b3ClothMesh* m = m_mesh;
|
||||
|
||||
@ -167,10 +171,12 @@ b3Cloth::b3Cloth(const b3ClothDef& def) :
|
||||
{
|
||||
b3ParticleDef pd;
|
||||
pd.type = e_dynamicParticle;
|
||||
pd.mass = 1.0f;
|
||||
pd.position = m->vertices[i];
|
||||
|
||||
b3Particle* p = CreateParticle(pd);
|
||||
|
||||
p->m_aabbProxy.index = i;
|
||||
p->m_vertex = i;
|
||||
m_vertexParticles[i] = p;
|
||||
}
|
||||
@ -200,7 +206,10 @@ b3Cloth::b3Cloth(const b3ClothDef& def) :
|
||||
b3SpringForceDef fd;
|
||||
fd.Initialize(p1, p2, def.structural, def.damping);
|
||||
|
||||
CreateForce(fd);
|
||||
if (def.structural > 0.0f)
|
||||
{
|
||||
CreateForce(fd);
|
||||
}
|
||||
}
|
||||
|
||||
// Bending
|
||||
@ -216,9 +225,12 @@ b3Cloth::b3Cloth(const b3ClothDef& def) :
|
||||
b3SpringForceDef fd;
|
||||
fd.Initialize(p3, p4, def.bending, def.damping);
|
||||
|
||||
CreateForce(fd);
|
||||
if (def.bending > 0.0f)
|
||||
{
|
||||
CreateForce(fd);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
allocator->Free(sharedEdges);
|
||||
allocator->Free(uniqueEdges);
|
||||
|
||||
@ -233,7 +245,30 @@ b3Cloth::b3Cloth(const b3ClothDef& def) :
|
||||
b3SpringForceDef fd;
|
||||
fd.Initialize(p1, p2, def.structural, def.damping);
|
||||
|
||||
CreateForce(fd);
|
||||
if (def.structural > 0.0f)
|
||||
{
|
||||
CreateForce(fd);
|
||||
}
|
||||
}
|
||||
|
||||
// Initialize triangle proxies
|
||||
m_triangleProxies = (b3ClothAABBProxy*)b3Alloc(m_mesh->triangleCount * sizeof(b3ClothAABBProxy));
|
||||
for (u32 i = 0; i < m_mesh->triangleCount; ++i)
|
||||
{
|
||||
b3ClothMeshTriangle* triangle = m_mesh->triangles + i;
|
||||
b3ClothAABBProxy* triangleProxy = m_triangleProxies + i;
|
||||
|
||||
b3Vec3 v1 = m_mesh->vertices[triangle->v1];
|
||||
b3Vec3 v2 = m_mesh->vertices[triangle->v2];
|
||||
b3Vec3 v3 = m_mesh->vertices[triangle->v3];
|
||||
|
||||
b3AABB3 aabb;
|
||||
aabb.Set(v1, v2, v3);
|
||||
|
||||
triangleProxy->type = e_triangleProxy;
|
||||
triangleProxy->index = i;
|
||||
triangleProxy->data = triangle;
|
||||
triangleProxy->broadPhaseId = m_contactManager.m_broadPhase.CreateProxy(aabb, triangleProxy);
|
||||
}
|
||||
|
||||
m_gravity.SetZero();
|
||||
@ -251,6 +286,7 @@ b3Cloth::~b3Cloth()
|
||||
}
|
||||
|
||||
b3Free(m_vertexParticles);
|
||||
b3Free(m_triangleProxies);
|
||||
|
||||
b3Force* f = m_forceList.m_head;
|
||||
while (f)
|
||||
@ -265,11 +301,14 @@ b3Particle* b3Cloth::CreateParticle(const b3ParticleDef& def)
|
||||
{
|
||||
void* mem = m_particleBlocks.Allocate();
|
||||
b3Particle* p = new(mem) b3Particle(def, this);
|
||||
|
||||
|
||||
b3AABB3 aabb;
|
||||
aabb.Set(p->m_position, p->m_radius);
|
||||
|
||||
p->m_treeId = m_particleTree.InsertNode(aabb, p);
|
||||
|
||||
p->m_aabbProxy.type = e_particleProxy;
|
||||
p->m_aabbProxy.data = p;
|
||||
p->m_aabbProxy.index = p->m_vertex;
|
||||
p->m_aabbProxy.broadPhaseId = m_contactManager.m_broadPhase.CreateProxy(aabb, &p->m_aabbProxy);
|
||||
|
||||
m_particleList.PushFront(p);
|
||||
|
||||
@ -280,6 +319,7 @@ void b3Cloth::DestroyParticle(b3Particle* particle)
|
||||
{
|
||||
B3_ASSERT(particle->m_vertex == ~0);
|
||||
|
||||
// Destroy particle forces
|
||||
b3Force* f = m_forceList.m_head;
|
||||
while (f)
|
||||
{
|
||||
@ -293,7 +333,11 @@ void b3Cloth::DestroyParticle(b3Particle* particle)
|
||||
}
|
||||
}
|
||||
|
||||
m_particleTree.RemoveNode(particle->m_treeId);
|
||||
// Destroy particle contacts
|
||||
particle->DestroyContacts();
|
||||
|
||||
// Destroy AABB proxy
|
||||
m_contactManager.m_broadPhase.DestroyProxy(particle->m_aabbProxy.broadPhaseId);
|
||||
|
||||
m_particleList.Remove(particle);
|
||||
particle->~b3Particle();
|
||||
@ -370,6 +414,44 @@ void b3Cloth::ComputeMass()
|
||||
}
|
||||
}
|
||||
|
||||
struct b3ClothRayCastSingleCallback
|
||||
{
|
||||
float32 Report(const b3RayCastInput& input, u32 proxyId)
|
||||
{
|
||||
// Get primitive associated with the proxy.
|
||||
void* userData = broadPhase->GetUserData(proxyId);
|
||||
b3ClothAABBProxy* proxy = (b3ClothAABBProxy*)userData;
|
||||
|
||||
if (proxy->type != e_triangleProxy)
|
||||
{
|
||||
// Continue search from where we stopped.
|
||||
return input.maxFraction;
|
||||
}
|
||||
|
||||
u32 triangleIndex = proxy->index;
|
||||
|
||||
b3RayCastOutput subOutput;
|
||||
if (cloth->RayCast(&subOutput, &input, triangleIndex))
|
||||
{
|
||||
// Ray hits triangle.
|
||||
if (subOutput.fraction < output0.fraction)
|
||||
{
|
||||
triangle0 = proxy->index;
|
||||
output0.fraction = subOutput.fraction;
|
||||
output0.normal = subOutput.normal;
|
||||
}
|
||||
}
|
||||
|
||||
// Continue search from where we stopped.
|
||||
return input.maxFraction;
|
||||
}
|
||||
|
||||
const b3Cloth* cloth;
|
||||
const b3BroadPhase* broadPhase;
|
||||
u32 triangle0;
|
||||
b3RayCastOutput output0;
|
||||
};
|
||||
|
||||
bool b3Cloth::RayCastSingle(b3ClothRayCastSingleOutput* output, const b3Vec3& p1, const b3Vec3& p2) const
|
||||
{
|
||||
b3RayCastInput input;
|
||||
@ -377,29 +459,19 @@ bool b3Cloth::RayCastSingle(b3ClothRayCastSingleOutput* output, const b3Vec3& p1
|
||||
input.p2 = p2;
|
||||
input.maxFraction = 1.0f;
|
||||
|
||||
u32 triangle0 = ~0;
|
||||
b3RayCastOutput output0;
|
||||
output0.fraction = B3_MAX_FLOAT;
|
||||
b3ClothRayCastSingleCallback callback;
|
||||
callback.cloth = this;
|
||||
callback.broadPhase = &m_contactManager.m_broadPhase;
|
||||
callback.triangle0 = ~0;
|
||||
callback.output0.fraction = B3_MAX_FLOAT;
|
||||
|
||||
for (u32 i = 0; i < m_mesh->triangleCount; ++i)
|
||||
{
|
||||
b3RayCastOutput subOutput;
|
||||
if (RayCast(&subOutput, &input, i))
|
||||
{
|
||||
if (subOutput.fraction < output0.fraction)
|
||||
{
|
||||
triangle0 = i;
|
||||
output0.fraction = subOutput.fraction;
|
||||
output0.normal = subOutput.normal;
|
||||
}
|
||||
}
|
||||
}
|
||||
m_contactManager.m_broadPhase.RayCast(&callback, input);
|
||||
|
||||
if (triangle0 != ~0)
|
||||
if (callback.triangle0 != ~0)
|
||||
{
|
||||
output->triangle = triangle0;
|
||||
output->fraction = output0.fraction;
|
||||
output->normal = output0.normal;
|
||||
output->triangle = callback.triangle0;
|
||||
output->fraction = callback.output0.fraction;
|
||||
output->normal = callback.output0.normal;
|
||||
|
||||
return true;
|
||||
}
|
||||
@ -455,9 +527,9 @@ public:
|
||||
b3Vec3 bestNormal;
|
||||
};
|
||||
|
||||
void b3Cloth::UpdateContacts()
|
||||
void b3Cloth::UpdateParticleBodyContacts()
|
||||
{
|
||||
B3_PROFILE("Cloth Update Contacts");
|
||||
B3_PROFILE("Cloth Update Particle Body Contacts");
|
||||
|
||||
// Is there a world attached to this cloth?
|
||||
if (m_world == nullptr)
|
||||
@ -473,7 +545,7 @@ void b3Cloth::UpdateContacts()
|
||||
continue;
|
||||
}
|
||||
|
||||
b3AABB3 aabb = m_particleTree.GetAABB(p->m_treeId);
|
||||
b3AABB3 aabb = m_contactManager.m_broadPhase.GetAABB(p->m_aabbProxy.broadPhaseId);
|
||||
|
||||
b3ClothUpdateContactsQueryListener listener;
|
||||
listener.sphere.vertex = p->m_position;
|
||||
@ -528,6 +600,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u
|
||||
solverDef.particleCapacity = m_particleList.m_count;
|
||||
solverDef.forceCapacity = m_forceList.m_count;
|
||||
solverDef.bodyContactCapacity = m_particleList.m_count;
|
||||
solverDef.triangleContactCapacity = m_contactManager.m_particleTriangleContactList.m_count;
|
||||
|
||||
b3ClothSolver solver(solverDef);
|
||||
|
||||
@ -549,6 +622,14 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u
|
||||
}
|
||||
}
|
||||
|
||||
for (b3ParticleTriangleContact* c = m_contactManager.m_particleTriangleContactList.m_head; c; c = c->m_next)
|
||||
{
|
||||
if (c->m_active)
|
||||
{
|
||||
solver.Add(c);
|
||||
}
|
||||
}
|
||||
|
||||
// Solve
|
||||
solver.Solve(dt, gravity, velocityIterations, positionIterations);
|
||||
}
|
||||
@ -557,8 +638,13 @@ void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations)
|
||||
{
|
||||
B3_PROFILE("Cloth Step");
|
||||
|
||||
// Update contacts
|
||||
UpdateContacts();
|
||||
m_dt = dt;
|
||||
|
||||
// Update particle-body contacts
|
||||
UpdateParticleBodyContacts();
|
||||
|
||||
// Update self-contacts
|
||||
m_contactManager.UpdateContacts();
|
||||
|
||||
// Integrate state, solve constraints.
|
||||
if (dt > 0.0f)
|
||||
@ -576,13 +662,49 @@ void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations)
|
||||
// Synchronize particles
|
||||
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
|
||||
{
|
||||
if (p->m_type == e_staticParticle)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
p->Synchronize();
|
||||
}
|
||||
|
||||
// Synchronize triangles
|
||||
for (u32 i = 0; i < m_mesh->triangleCount; ++i)
|
||||
{
|
||||
SynchronizeTriangle(i);
|
||||
}
|
||||
|
||||
#if B3_ENABLE_SELF_COLLISION
|
||||
|
||||
// Find new self-contacts
|
||||
m_contactManager.FindNewContacts();
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
void b3Cloth::SynchronizeTriangle(u32 triangleIndex)
|
||||
{
|
||||
b3ClothMeshTriangle* triangle = m_mesh->triangles + triangleIndex;
|
||||
b3ClothAABBProxy* triangleProxy = m_triangleProxies + triangleIndex;
|
||||
|
||||
b3Particle* p1 = m_vertexParticles[triangle->v1];
|
||||
b3Particle* p2 = m_vertexParticles[triangle->v2];
|
||||
b3Particle* p3 = m_vertexParticles[triangle->v3];
|
||||
|
||||
b3Vec3 x1 = p1->m_position;
|
||||
b3Vec3 x2 = p2->m_position;
|
||||
b3Vec3 x3 = p3->m_position;
|
||||
|
||||
b3Vec3 v1 = p1->m_velocity;
|
||||
b3Vec3 v2 = p2->m_velocity;
|
||||
b3Vec3 v3 = p3->m_velocity;
|
||||
|
||||
b3AABB3 aabb;
|
||||
aabb.Set(x1, x2, x3);
|
||||
|
||||
const float32 kInv3 = 1.0f / 3.0f;
|
||||
|
||||
b3Vec3 center_velocity = kInv3 * (v1 + v2 + v3);
|
||||
b3Vec3 center_displacement = m_dt * center_velocity;
|
||||
|
||||
m_contactManager.m_broadPhase.MoveProxy(triangleProxy->broadPhaseId, aabb, center_displacement);
|
||||
}
|
||||
|
||||
void b3Cloth::Draw() const
|
||||
|
||||
336
src/bounce/cloth/cloth_contact_manager.cpp
Normal file
336
src/bounce/cloth/cloth_contact_manager.cpp
Normal file
@ -0,0 +1,336 @@
|
||||
/*
|
||||
* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
|
||||
*
|
||||
* 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/cloth/cloth_contact_manager.h>
|
||||
#include <bounce/cloth/cloth.h>
|
||||
#include <bounce/cloth/cloth_mesh.h>
|
||||
#include <bounce/cloth/particle.h>
|
||||
|
||||
#include <bounce/common/draw.h>
|
||||
|
||||
b3ClothContactManager::b3ClothContactManager() :
|
||||
m_particleTriangleContactBlocks(sizeof(b3ParticleTriangleContact))
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void b3ClothContactManager::FindNewContacts()
|
||||
{
|
||||
B3_PROFILE("Cloth Find New Contacts");
|
||||
|
||||
m_broadPhase.FindPairs(this);
|
||||
}
|
||||
|
||||
void b3ClothContactManager::AddPair(void* data1, void* data2)
|
||||
{
|
||||
b3ClothAABBProxy* proxy1 = (b3ClothAABBProxy*)data1;
|
||||
b3ClothAABBProxy* proxy2 = (b3ClothAABBProxy*)data2;
|
||||
|
||||
if (proxy1->type == e_particleProxy && proxy2->type == e_particleProxy)
|
||||
{
|
||||
// Particle-particle contacts are not supported.
|
||||
return;
|
||||
}
|
||||
|
||||
if (proxy1->type == e_triangleProxy && proxy2->type == e_triangleProxy)
|
||||
{
|
||||
// Triangle-triangle contacts are not supported.
|
||||
return;
|
||||
}
|
||||
|
||||
if (proxy1->type == e_triangleProxy)
|
||||
{
|
||||
// Ensure proxy1 is a particle and proxy 2 a triangle.
|
||||
b3Swap(proxy1, proxy2);
|
||||
}
|
||||
|
||||
B3_ASSERT(proxy1->type == e_particleProxy);
|
||||
B3_ASSERT(proxy2->type == e_triangleProxy);
|
||||
|
||||
b3Particle* p1 = (b3Particle*)proxy1->data;
|
||||
|
||||
b3ClothMeshTriangle* triangle = (b3ClothMeshTriangle*)proxy2->data;
|
||||
b3Particle* p2 = m_cloth->m_vertexParticles[triangle->v1];
|
||||
b3Particle* p3 = m_cloth->m_vertexParticles[triangle->v2];
|
||||
b3Particle* p4 = m_cloth->m_vertexParticles[triangle->v3];
|
||||
|
||||
// Check if there is a contact between the two entities.
|
||||
for (b3ParticleTriangleContact* c = m_particleTriangleContactList.m_head; c; c = c->m_next)
|
||||
{
|
||||
if (c->m_p1 == p1 && c->m_triangle == triangle)
|
||||
{
|
||||
// A contact already exists.
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
bool isntDynamic1 = p1->m_type != e_dynamicParticle;
|
||||
bool isntDynamic2 = p2->m_type != e_dynamicParticle && p3->m_type != e_dynamicParticle && p4->m_type != e_dynamicParticle;
|
||||
|
||||
if (isntDynamic1 && isntDynamic2)
|
||||
{
|
||||
// The entities must not collide with each other.
|
||||
return;
|
||||
}
|
||||
|
||||
if (triangle->v1 == p1->m_vertex || triangle->v2 == p1->m_vertex || triangle->v3 == p1->m_vertex)
|
||||
{
|
||||
// The entities must not collide with each other.
|
||||
return;
|
||||
}
|
||||
|
||||
// Create a new contact.
|
||||
b3ParticleTriangleContact* c = Create();
|
||||
|
||||
c->m_p1 = p1;
|
||||
|
||||
c->m_p2 = p2;
|
||||
c->m_p3 = p3;
|
||||
c->m_p4 = p4;
|
||||
c->m_triangle = triangle;
|
||||
c->m_triangleProxy = proxy2;
|
||||
|
||||
c->m_front = false;
|
||||
c->m_active = false;
|
||||
|
||||
c->m_normalImpulse = 0.0f;
|
||||
|
||||
// Add the contact to the cloth contact list.
|
||||
m_particleTriangleContactList.PushFront(c);
|
||||
}
|
||||
|
||||
b3ParticleTriangleContact* b3ClothContactManager::Create()
|
||||
{
|
||||
void* block = m_particleTriangleContactBlocks.Allocate();
|
||||
return new(block) b3ParticleTriangleContact();
|
||||
}
|
||||
|
||||
void b3ClothContactManager::Destroy(b3ParticleTriangleContact* c)
|
||||
{
|
||||
m_particleTriangleContactList.Remove(c);
|
||||
|
||||
c->~b3ParticleTriangleContact();
|
||||
|
||||
m_particleTriangleContactBlocks.Free(c);
|
||||
}
|
||||
|
||||
static 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];
|
||||
b3BarycentricCoordinates(wAB, A, B, Q);
|
||||
b3BarycentricCoordinates(wBC, B, C, Q);
|
||||
b3BarycentricCoordinates(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];
|
||||
b3BarycentricCoordinates(wABC, A, B, C, Q);
|
||||
|
||||
// R AB
|
||||
if (wAB[0] > 0.0f && wAB[1] > 0.0f && wABC[3] * 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 && wABC[3] * 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 && wABC[3] * wABC[1] <= 0.0f)
|
||||
{
|
||||
float32 divisor = wCA[2];
|
||||
B3_ASSERT(divisor > 0.0f);
|
||||
float32 s = 1.0f / divisor;
|
||||
out[0] = s * wCA[1];
|
||||
out[1] = 0.0f;
|
||||
out[2] = s * wCA[0];
|
||||
return;
|
||||
}
|
||||
|
||||
// R ABC/ACB
|
||||
float32 divisor = wABC[3];
|
||||
if (divisor == 0.0f)
|
||||
{
|
||||
float32 s = 1.0f / 3.0f;
|
||||
out[0] = s;
|
||||
out[1] = s;
|
||||
out[2] = 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 b3ClothContactManager::Update(b3ParticleTriangleContact* c)
|
||||
{
|
||||
b3Particle* p1 = c->m_p1;
|
||||
|
||||
b3Particle* p2 = c->m_p2;
|
||||
b3Particle* p3 = c->m_p3;
|
||||
b3Particle* p4 = c->m_p4;
|
||||
|
||||
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 n = b3Cross(B - A, C - A);
|
||||
float32 len = n.Normalize();
|
||||
|
||||
// Is ABC degenerate?
|
||||
if (len == 0.0f)
|
||||
{
|
||||
c->m_active = false;
|
||||
return;
|
||||
}
|
||||
|
||||
b3Vec3 P1 = p1->m_position;
|
||||
|
||||
float32 distance = b3Dot(n, P1 - A);
|
||||
|
||||
// Is P1 below the plane?
|
||||
if (distance < -totalRadius)
|
||||
{
|
||||
c->m_active = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// Is P1 above the plane?
|
||||
if (distance > totalRadius)
|
||||
{
|
||||
c->m_active = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// Closest point on ABC to P1
|
||||
float32 wABC[3];
|
||||
b3Solve3(wABC, A, B, C, P1);
|
||||
|
||||
b3Vec3 P2 = wABC[0] * A + wABC[1] * B + wABC[2] * C;
|
||||
|
||||
if (b3DistanceSquared(P1, P2) > totalRadius * totalRadius)
|
||||
{
|
||||
c->m_active = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// Activate the contact
|
||||
c->m_active = true;
|
||||
|
||||
// Is the the other point in front of the plane?
|
||||
if (distance >= 0.0f)
|
||||
{
|
||||
c->m_front = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
c->m_front = false;
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothContactManager::UpdateContacts()
|
||||
{
|
||||
B3_PROFILE("Cloth Update Contacts");
|
||||
|
||||
// Update the state of all triangle contacts.
|
||||
b3ParticleTriangleContact* c = m_particleTriangleContactList.m_head;
|
||||
while (c)
|
||||
{
|
||||
bool isntDynamic1 = c->m_p1->m_type != e_dynamicParticle;
|
||||
bool isntDynamic2 = c->m_p2->m_type != e_dynamicParticle && c->m_p3->m_type != e_dynamicParticle && c->m_p4->m_type != e_dynamicParticle;
|
||||
|
||||
// Destroy the contact if primitives must not collide with each other.
|
||||
if (isntDynamic1 && isntDynamic2)
|
||||
{
|
||||
b3ParticleTriangleContact* quack = c;
|
||||
c = c->m_next;
|
||||
Destroy(quack);
|
||||
continue;
|
||||
}
|
||||
|
||||
u32 proxy1 = c->m_p1->m_aabbProxy.broadPhaseId;
|
||||
u32 proxy2 = c->m_triangleProxy->broadPhaseId;
|
||||
|
||||
// Destroy the contact if primitive AABBs are not overlapping.
|
||||
bool overlap = m_broadPhase.TestOverlap(proxy1, proxy2);
|
||||
if (overlap == false)
|
||||
{
|
||||
b3ParticleTriangleContact* quack = c;
|
||||
c = c->m_next;
|
||||
Destroy(quack);
|
||||
continue;
|
||||
}
|
||||
|
||||
// The contact persists.
|
||||
Update(c);
|
||||
|
||||
c = c->m_next;
|
||||
}
|
||||
}
|
||||
@ -17,6 +17,7 @@
|
||||
*/
|
||||
|
||||
#include <bounce/cloth/cloth_contact_solver.h>
|
||||
#include <bounce/cloth/cloth_contact_manager.h>
|
||||
#include <bounce/cloth/particle.h>
|
||||
#include <bounce/cloth/dense_vec3.h>
|
||||
#include <bounce/common/memory/stack_allocator.h>
|
||||
@ -34,10 +35,18 @@ b3ClothContactSolver::b3ClothContactSolver(const b3ClothContactSolverDef& def)
|
||||
m_bodyContacts = def.bodyContacts;
|
||||
m_bodyVelocityConstraints = (b3ClothSolverBodyContactVelocityConstraint*)m_allocator->Allocate(m_bodyContactCount * sizeof(b3ClothSolverBodyContactVelocityConstraint));
|
||||
m_bodyPositionConstraints = (b3ClothSolverBodyContactPositionConstraint*)m_allocator->Allocate(m_bodyContactCount * sizeof(b3ClothSolverBodyContactPositionConstraint));
|
||||
|
||||
m_triangleContactCount = def.triangleContactCount;
|
||||
m_triangleContacts = def.triangleContacts;
|
||||
m_triangleVelocityConstraints = (b3ClothSolverTriangleContactVelocityConstraint*)m_allocator->Allocate(m_triangleContactCount * sizeof(b3ClothSolverTriangleContactVelocityConstraint));
|
||||
m_trianglePositionConstraints = (b3ClothSolverTriangleContactPositionConstraint*)m_allocator->Allocate(m_triangleContactCount * sizeof(b3ClothSolverTriangleContactPositionConstraint));
|
||||
}
|
||||
|
||||
b3ClothContactSolver::~b3ClothContactSolver()
|
||||
{
|
||||
m_allocator->Free(m_trianglePositionConstraints);
|
||||
m_allocator->Free(m_triangleVelocityConstraints);
|
||||
|
||||
m_allocator->Free(m_bodyPositionConstraints);
|
||||
m_allocator->Free(m_bodyVelocityConstraints);
|
||||
}
|
||||
@ -170,8 +179,93 @@ void b3ClothContactSolver::InitializeBodyContactConstraints()
|
||||
}
|
||||
}
|
||||
}
|
||||
void b3ClothContactSolver::InitializeTriangleContactConstraints()
|
||||
{
|
||||
b3DenseVec3& x = *m_positions;
|
||||
|
||||
void b3ClothContactSolver::WarmStart()
|
||||
for (u32 i = 0; i < m_triangleContactCount; ++i)
|
||||
{
|
||||
b3ParticleTriangleContact* c = m_triangleContacts[i];
|
||||
b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i;
|
||||
b3ClothSolverTriangleContactPositionConstraint* pc = m_trianglePositionConstraints + i;
|
||||
|
||||
vc->indexA = c->m_p1->m_solverId;
|
||||
vc->invMassA = c->m_p1->m_type == e_staticParticle ? 0.0f : c->m_p1->m_invMass;
|
||||
|
||||
vc->indexB = c->m_p2->m_solverId;
|
||||
vc->invMassB = c->m_p2->m_type == e_staticParticle ? 0.0f : c->m_p2->m_invMass;
|
||||
|
||||
vc->indexC = c->m_p3->m_solverId;
|
||||
vc->invMassC = c->m_p3->m_type == e_staticParticle ? 0.0f : c->m_p3->m_invMass;
|
||||
|
||||
vc->indexD = c->m_p4->m_solverId;
|
||||
vc->invMassD = c->m_p4->m_type == e_staticParticle ? 0.0f : c->m_p4->m_invMass;
|
||||
|
||||
pc->indexA = c->m_p1->m_solverId;
|
||||
pc->invMassA = c->m_p1->m_type == e_staticParticle ? 0.0f : c->m_p1->m_invMass;
|
||||
pc->radiusA = c->m_p1->m_radius;
|
||||
|
||||
pc->indexB = c->m_p2->m_solverId;
|
||||
pc->invMassB = c->m_p2->m_type == e_staticParticle ? 0.0f : c->m_p2->m_invMass;
|
||||
|
||||
pc->indexC = c->m_p3->m_solverId;
|
||||
pc->invMassC = c->m_p3->m_type == e_staticParticle ? 0.0f : c->m_p3->m_invMass;
|
||||
|
||||
pc->indexD = c->m_p4->m_solverId;
|
||||
pc->invMassD = c->m_p4->m_type == e_staticParticle ? 0.0f : c->m_p4->m_invMass;
|
||||
|
||||
pc->triangleRadius = 0.0f;
|
||||
pc->front = c->m_front;
|
||||
|
||||
u32 indexA = pc->indexA;
|
||||
float32 mA = pc->invMassA;
|
||||
|
||||
u32 indexB = pc->indexB;
|
||||
float32 mB = pc->invMassB;
|
||||
|
||||
u32 indexC = pc->indexC;
|
||||
float32 mC = pc->invMassC;
|
||||
|
||||
u32 indexD = pc->indexD;
|
||||
float32 mD = pc->invMassD;
|
||||
|
||||
b3Vec3 xA = x[indexA];
|
||||
b3Vec3 xB = x[indexB];
|
||||
b3Vec3 xC = x[indexC];
|
||||
b3Vec3 xD = x[indexD];
|
||||
|
||||
b3Vec3 n = b3Cross(xC - xB, xD - xB);
|
||||
|
||||
if (pc->front == false)
|
||||
{
|
||||
n = -n;
|
||||
}
|
||||
|
||||
float32 n_len = n.Normalize();
|
||||
|
||||
b3Mat33 I; I.SetIdentity();
|
||||
|
||||
b3Mat33 N = I - b3Outer(n, n);
|
||||
if (n_len > B3_EPSILON)
|
||||
{
|
||||
N = (1.0f / n_len) * N;
|
||||
}
|
||||
|
||||
b3Vec3 N_n = N * n;
|
||||
|
||||
vc->JA = n;
|
||||
vc->JB = b3Cross(xC - xD, N_n) - n;
|
||||
vc->JC = b3Cross(xD - xB, N_n);
|
||||
vc->JD = b3Cross(xC - xB, N_n);
|
||||
|
||||
float32 K = mA + mB * b3Dot(vc->JB, vc->JB) + mC * b3Dot(vc->JC, vc->JC) + mD * b3Dot(vc->JD, vc->JD);
|
||||
|
||||
vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f;
|
||||
vc->normalImpulse = c->m_normalImpulse;
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothContactSolver::WarmStartBodyContactConstraints()
|
||||
{
|
||||
b3DenseVec3& v = *m_velocities;
|
||||
|
||||
@ -218,6 +312,46 @@ void b3ClothContactSolver::WarmStart()
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothContactSolver::WarmStartTriangleContactConstraints()
|
||||
{
|
||||
b3DenseVec3& v = *m_velocities;
|
||||
|
||||
for (u32 i = 0; i < m_triangleContactCount; ++i)
|
||||
{
|
||||
b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i;
|
||||
|
||||
u32 indexA = vc->indexA;
|
||||
u32 indexB = vc->indexB;
|
||||
u32 indexC = vc->indexC;
|
||||
u32 indexD = vc->indexD;
|
||||
|
||||
b3Vec3 vA = v[indexA];
|
||||
b3Vec3 vB = v[indexB];
|
||||
b3Vec3 vC = v[indexC];
|
||||
b3Vec3 vD = v[indexD];
|
||||
|
||||
float32 mA = vc->invMassA;
|
||||
float32 mB = vc->invMassB;
|
||||
float32 mC = vc->invMassC;
|
||||
float32 mD = vc->invMassD;
|
||||
|
||||
b3Vec3 PA = vc->normalImpulse * vc->JA;
|
||||
b3Vec3 PB = vc->normalImpulse * vc->JB;
|
||||
b3Vec3 PC = vc->normalImpulse * vc->JC;
|
||||
b3Vec3 PD = vc->normalImpulse * vc->JD;
|
||||
|
||||
vA += mA * PA;
|
||||
vB += mB * PB;
|
||||
vC += mC * PC;
|
||||
vD += mD * PD;
|
||||
|
||||
v[indexA] = vA;
|
||||
v[indexB] = vB;
|
||||
v[indexC] = vC;
|
||||
v[indexD] = vD;
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothContactSolver::SolveBodyContactVelocityConstraints()
|
||||
{
|
||||
b3DenseVec3& v = *m_velocities;
|
||||
@ -306,6 +440,56 @@ void b3ClothContactSolver::SolveBodyContactVelocityConstraints()
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothContactSolver::SolveTriangleContactVelocityConstraints()
|
||||
{
|
||||
b3DenseVec3& v = *m_velocities;
|
||||
|
||||
for (u32 i = 0; i < m_triangleContactCount; ++i)
|
||||
{
|
||||
b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i;
|
||||
|
||||
u32 indexA = vc->indexA;
|
||||
float32 mA = vc->invMassA;
|
||||
|
||||
u32 indexB = vc->indexB;
|
||||
float32 mB = vc->invMassB;
|
||||
|
||||
u32 indexC = vc->indexC;
|
||||
float32 mC = vc->invMassC;
|
||||
|
||||
u32 indexD = vc->indexD;
|
||||
float32 mD = vc->invMassD;
|
||||
|
||||
b3Vec3 vA = v[indexA];
|
||||
b3Vec3 vB = v[indexB];
|
||||
b3Vec3 vC = v[indexC];
|
||||
b3Vec3 vD = v[indexD];
|
||||
|
||||
float32 Cdot = b3Dot(vc->JA, vA) + b3Dot(vc->JB, vB) + b3Dot(vc->JC, vC) + b3Dot(vc->JD, vD);
|
||||
|
||||
float32 impulse = -vc->normalMass * Cdot;
|
||||
|
||||
float32 oldImpulse = vc->normalImpulse;
|
||||
vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f);
|
||||
impulse = vc->normalImpulse - oldImpulse;
|
||||
|
||||
b3Vec3 PA = impulse * vc->JA;
|
||||
b3Vec3 PB = impulse * vc->JB;
|
||||
b3Vec3 PC = impulse * vc->JC;
|
||||
b3Vec3 PD = impulse * vc->JD;
|
||||
|
||||
vA += mA * PA;
|
||||
vB += mB * PB;
|
||||
vC += mC * PC;
|
||||
vD += mD * PD;
|
||||
|
||||
v[indexA] = vA;
|
||||
v[indexB] = vB;
|
||||
v[indexC] = vC;
|
||||
v[indexD] = vD;
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothContactSolver::StoreImpulses()
|
||||
{
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
@ -316,6 +500,14 @@ void b3ClothContactSolver::StoreImpulses()
|
||||
c->normalImpulse = vc->normalImpulse;
|
||||
c->tangentImpulse = vc->tangentImpulse;
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_triangleContactCount; ++i)
|
||||
{
|
||||
b3ParticleTriangleContact* c = m_triangleContacts[i];
|
||||
b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i;
|
||||
|
||||
c->m_normalImpulse = vc->normalImpulse;
|
||||
}
|
||||
}
|
||||
|
||||
struct b3ClothSolverBodyContactSolverPoint
|
||||
@ -417,5 +609,92 @@ bool b3ClothContactSolver::SolveBodyContactPositionConstraints()
|
||||
bodyB->m_sweep.orientation = qB;
|
||||
}
|
||||
|
||||
return minSeparation >= -3.0f * B3_LINEAR_SLOP;
|
||||
}
|
||||
|
||||
bool b3ClothContactSolver::SolveTriangleContactPositionConstraints()
|
||||
{
|
||||
b3DenseVec3& x = *m_positions;
|
||||
|
||||
float32 minSeparation = 0.0f;
|
||||
|
||||
for (u32 i = 0; i < m_triangleContactCount; ++i)
|
||||
{
|
||||
b3ClothSolverTriangleContactPositionConstraint* pc = m_trianglePositionConstraints + i;
|
||||
|
||||
u32 indexA = pc->indexA;
|
||||
float32 mA = pc->invMassA;
|
||||
float32 radiusA = pc->radiusA;
|
||||
|
||||
u32 indexB = pc->indexB;
|
||||
float32 mB = pc->invMassB;
|
||||
|
||||
u32 indexC = pc->indexC;
|
||||
float32 mC = pc->invMassC;
|
||||
|
||||
u32 indexD = pc->indexD;
|
||||
float32 mD = pc->invMassD;
|
||||
|
||||
float32 triangleRadius = pc->triangleRadius;
|
||||
|
||||
b3Vec3 xA = x[indexA];
|
||||
b3Vec3 xB = x[indexB];
|
||||
b3Vec3 xC = x[indexC];
|
||||
b3Vec3 xD = x[indexD];
|
||||
|
||||
b3Vec3 n = b3Cross(xC - xB, xD - xB);
|
||||
if (pc->front == false)
|
||||
{
|
||||
n = -n;
|
||||
}
|
||||
|
||||
float32 n_len = n.Normalize();
|
||||
|
||||
float32 distance = b3Dot(n, xA - xB);
|
||||
float32 separation = distance - radiusA - triangleRadius;
|
||||
|
||||
// Update max constraint error.
|
||||
minSeparation = b3Min(minSeparation, separation);
|
||||
|
||||
// Allow some slop and prevent large corrections.
|
||||
float32 C = b3Clamp(B3_BAUMGARTE * (separation + B3_LINEAR_SLOP), -B3_MAX_LINEAR_CORRECTION, 0.0f);
|
||||
|
||||
b3Mat33 I; I.SetIdentity();
|
||||
|
||||
b3Mat33 N = I - b3Outer(n, n);
|
||||
if (n_len > B3_EPSILON)
|
||||
{
|
||||
N = (1.0f / n_len) * N;
|
||||
}
|
||||
|
||||
b3Vec3 N_n = N * n;
|
||||
|
||||
b3Vec3 JA = n;
|
||||
b3Vec3 JB = b3Cross(xC - xD, N_n) - n;
|
||||
b3Vec3 JC = b3Cross(xD - xB, N_n);
|
||||
b3Vec3 JD = b3Cross(xC - xB, N_n);
|
||||
|
||||
// Compute effective mass.
|
||||
float32 K = mA + mB * b3Dot(JB, JB) + mC * b3Dot(JC, JC) + mD * b3Dot(JD, JD);
|
||||
|
||||
// Compute normal impulse.
|
||||
float32 impulse = K > 0.0f ? -C / K : 0.0f;
|
||||
|
||||
b3Vec3 PA = impulse * JA;
|
||||
b3Vec3 PB = impulse * JB;
|
||||
b3Vec3 PC = impulse * JC;
|
||||
b3Vec3 PD = impulse * JD;
|
||||
|
||||
xA += mA * PA;
|
||||
xB += mB * PB;
|
||||
xC += mC * PC;
|
||||
xD += mD * PD;
|
||||
|
||||
x[indexA] = xA;
|
||||
x[indexB] = xB;
|
||||
x[indexC] = xC;
|
||||
x[indexD] = xD;
|
||||
}
|
||||
|
||||
return minSeparation >= -3.0f * B3_LINEAR_SLOP;
|
||||
}
|
||||
@ -58,10 +58,15 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
|
||||
m_bodyContactCapacity = def.bodyContactCapacity;
|
||||
m_bodyContactCount = 0;
|
||||
m_bodyContacts = (b3ParticleBodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3ParticleBodyContact*));;
|
||||
|
||||
m_triangleContactCapacity = def.triangleContactCapacity;
|
||||
m_triangleContactCount = 0;
|
||||
m_triangleContacts = (b3ParticleTriangleContact**)m_allocator->Allocate(m_triangleContactCapacity * sizeof(b3ParticleTriangleContact*));;
|
||||
}
|
||||
|
||||
b3ClothSolver::~b3ClothSolver()
|
||||
{
|
||||
m_allocator->Free(m_triangleContacts);
|
||||
m_allocator->Free(m_bodyContacts);
|
||||
|
||||
m_allocator->Free(m_constraints);
|
||||
@ -85,6 +90,11 @@ void b3ClothSolver::Add(b3ParticleBodyContact* c)
|
||||
m_bodyContacts[m_bodyContactCount++] = c;
|
||||
}
|
||||
|
||||
void b3ClothSolver::Add(b3ParticleTriangleContact* c)
|
||||
{
|
||||
m_triangleContacts[m_triangleContactCount++] = c;
|
||||
}
|
||||
|
||||
void b3ClothSolver::ApplyForces()
|
||||
{
|
||||
for (u32 i = 0; i < m_forceCount; ++i)
|
||||
@ -266,21 +276,26 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterati
|
||||
contactSolverDef.velocities = m_solverData.v;
|
||||
contactSolverDef.bodyContactCount = m_bodyContactCount;
|
||||
contactSolverDef.bodyContacts = m_bodyContacts;
|
||||
contactSolverDef.triangleContactCount = m_triangleContactCount;
|
||||
contactSolverDef.triangleContacts = m_triangleContacts;
|
||||
|
||||
b3ClothContactSolver contactSolver(contactSolverDef);
|
||||
|
||||
{
|
||||
contactSolver.InitializeBodyContactConstraints();
|
||||
contactSolver.InitializeTriangleContactConstraints();
|
||||
}
|
||||
|
||||
{
|
||||
contactSolver.WarmStart();
|
||||
contactSolver.WarmStartBodyContactConstraints();
|
||||
contactSolver.WarmStartTriangleContactConstraints();
|
||||
}
|
||||
|
||||
{
|
||||
for (u32 i = 0; i < velocityIterations; ++i)
|
||||
{
|
||||
contactSolver.SolveBodyContactVelocityConstraints();
|
||||
contactSolver.SolveTriangleContactVelocityConstraints();
|
||||
}
|
||||
}
|
||||
|
||||
@ -297,9 +312,11 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterati
|
||||
for (u32 i = 0; i < positionIterations; ++i)
|
||||
{
|
||||
bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints();
|
||||
bool triangleContactsSolved = contactSolver.SolveTriangleContactPositionConstraints();
|
||||
|
||||
if (bodyContactsSolved)
|
||||
if (bodyContactsSolved && triangleContactsSolved)
|
||||
{
|
||||
// Early out if the position errors are small.
|
||||
positionSolved = true;
|
||||
break;
|
||||
}
|
||||
|
||||
@ -18,6 +18,7 @@
|
||||
|
||||
#include <bounce/cloth/particle.h>
|
||||
#include <bounce/cloth/cloth.h>
|
||||
#include <bounce/cloth/cloth_mesh.h>
|
||||
|
||||
void b3ParticleBodyContactWorldPoint::Initialize(const b3ParticleBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB)
|
||||
{
|
||||
@ -42,8 +43,20 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)
|
||||
m_velocity = def.velocity;
|
||||
m_force = def.force;
|
||||
m_translation.SetZero();
|
||||
m_mass = 0.0f;
|
||||
m_invMass = 0.0f;
|
||||
m_mass = def.mass;
|
||||
|
||||
if (m_mass == 0.0f)
|
||||
{
|
||||
m_type = e_staticParticle;
|
||||
m_mass = 1.0f;
|
||||
m_invMass = 1.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_type = e_dynamicParticle;
|
||||
m_invMass = 1.0f / m_mass;
|
||||
}
|
||||
|
||||
m_radius = def.radius;
|
||||
m_friction = def.friction;
|
||||
m_userData = nullptr;
|
||||
@ -62,7 +75,48 @@ void b3Particle::Synchronize()
|
||||
b3AABB3 aabb;
|
||||
aabb.Set(m_position, m_radius);
|
||||
|
||||
m_cloth->m_particleTree.UpdateNode(m_treeId, aabb);
|
||||
b3Vec3 displacement = m_cloth->m_dt * m_velocity;
|
||||
|
||||
m_cloth->m_contactManager.m_broadPhase.MoveProxy(m_aabbProxy.broadPhaseId, aabb, displacement);
|
||||
}
|
||||
|
||||
void b3Particle::SynchronizeTriangles()
|
||||
{
|
||||
if (m_vertex == ~0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_cloth->m_mesh->triangleCount; ++i)
|
||||
{
|
||||
b3ClothMeshTriangle* triangle = m_cloth->m_mesh->triangles + i;
|
||||
|
||||
if (triangle->v1 == m_vertex || triangle->v2 == m_vertex || triangle->v3 == m_vertex)
|
||||
{
|
||||
m_cloth->SynchronizeTriangle(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void b3Particle::DestroyContacts()
|
||||
{
|
||||
// Destroy body contacts
|
||||
m_bodyContact.active = false;
|
||||
|
||||
// Destroy triangle contacts
|
||||
b3ParticleTriangleContact* c = m_cloth->m_contactManager.m_particleTriangleContactList.m_head;
|
||||
while (c)
|
||||
{
|
||||
if (c->m_p1 == this)
|
||||
{
|
||||
b3ParticleTriangleContact* quack = c;
|
||||
c = c->m_next;
|
||||
m_cloth->m_contactManager.Destroy(quack);
|
||||
continue;
|
||||
}
|
||||
|
||||
c = c->m_next;
|
||||
}
|
||||
}
|
||||
|
||||
void b3Particle::SetType(b3ParticleType type)
|
||||
@ -81,7 +135,11 @@ void b3Particle::SetType(b3ParticleType type)
|
||||
m_translation.SetZero();
|
||||
|
||||
Synchronize();
|
||||
SynchronizeTriangles();
|
||||
}
|
||||
|
||||
m_bodyContact.active = false;
|
||||
DestroyContacts();
|
||||
|
||||
// Move the proxy so new contacts can be created.
|
||||
m_cloth->m_contactManager.m_broadPhase.TouchProxy(m_aabbProxy.broadPhaseId);
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user