delegate cloth contacts, bugfix, optimization
This commit is contained in:
Irlan
parent
45f8ae2928
commit
22e2f58055
@ -294,6 +294,7 @@ private:
|
||||
friend class b3ContactManager;
|
||||
friend class b3ContactSolver;
|
||||
friend class b3ClothSolver;
|
||||
friend class b3ClothContactSolver;
|
||||
|
||||
friend class b3Joint;
|
||||
friend class b3JointManager;
|
||||
|
||||
@ -154,7 +154,10 @@ private:
|
||||
|
||||
// Update triangle contacts.
|
||||
void UpdateTriangleContacts();
|
||||
|
||||
|
||||
// Update contacts
|
||||
void UpdateContacts();
|
||||
|
||||
// Solve
|
||||
void Solve(float32 dt, const b3Vec3& gravity);
|
||||
|
||||
@ -170,6 +173,9 @@ private:
|
||||
// Pool of particles
|
||||
b3BlockPool m_particleBlocks;
|
||||
|
||||
// Pool of body contacts
|
||||
b3BlockPool m_bodyContactBlocks;
|
||||
|
||||
// Pool of particle contacts
|
||||
b3BlockPool m_particleContactBlocks;
|
||||
|
||||
@ -182,6 +188,9 @@ private:
|
||||
// List of forces
|
||||
b3List2<b3Force> m_forceList;
|
||||
|
||||
// List of particle contacts
|
||||
b3List2<b3BodyContact> m_bodyContactList;
|
||||
|
||||
// List of particle contacts
|
||||
b3List2<b3ParticleContact> m_particleContactList;
|
||||
|
||||
|
||||
230
include/bounce/dynamics/cloth/cloth_contact_solver.h
Normal file
230
include/bounce/dynamics/cloth/cloth_contact_solver.h
Normal file
@ -0,0 +1,230 @@
|
||||
/*
|
||||
* 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_CLOTH_CONTACT_SOLVER_H
|
||||
#define B3_CLOTH_CONTACT_SOLVER_H
|
||||
|
||||
#include <bounce/common/math/mat22.h>
|
||||
#include <bounce/common/math/mat33.h>
|
||||
|
||||
class b3StackAllocator;
|
||||
|
||||
class b3Particle;
|
||||
class b3Body;
|
||||
|
||||
struct b3DenseVec3;
|
||||
struct b3DiagMat33;
|
||||
struct b3SparseSymMat33;
|
||||
|
||||
class b3BodyContact;
|
||||
class b3ParticleContact;
|
||||
class b3TriangleContact;
|
||||
|
||||
struct b3ClothSolverBodyContactVelocityConstraint
|
||||
{
|
||||
u32 indexA;
|
||||
float32 invMassA;
|
||||
b3Mat33 invIA;
|
||||
|
||||
b3Body* bodyB;
|
||||
float32 invMassB;
|
||||
b3Mat33 invIB;
|
||||
|
||||
float32 friction;
|
||||
|
||||
b3Vec3 point;
|
||||
b3Vec3 rA;
|
||||
b3Vec3 rB;
|
||||
|
||||
b3Vec3 normal;
|
||||
float32 normalMass;
|
||||
float32 normalImpulse;
|
||||
float32 velocityBias;
|
||||
|
||||
b3Vec3 tangent1;
|
||||
b3Vec3 tangent2;
|
||||
b3Mat22 tangentMass;
|
||||
b3Vec2 tangentImpulse;
|
||||
};
|
||||
|
||||
struct b3ClothSolverBodyContactPositionConstraint
|
||||
{
|
||||
u32 indexA;
|
||||
float32 invMassA;
|
||||
b3Mat33 invIA;
|
||||
float32 radiusA;
|
||||
b3Vec3 localCenterA;
|
||||
|
||||
b3Body* bodyB;
|
||||
float32 invMassB;
|
||||
b3Mat33 invIB;
|
||||
float32 radiusB;
|
||||
b3Vec3 localCenterB;
|
||||
|
||||
b3Vec3 rA;
|
||||
b3Vec3 rB;
|
||||
|
||||
b3Vec3 localPointA;
|
||||
b3Vec3 localPointB;
|
||||
};
|
||||
|
||||
struct b3ClothSolverParticleContactVelocityConstraint
|
||||
{
|
||||
u32 indexA;
|
||||
float32 invMassA;
|
||||
|
||||
u32 indexB;
|
||||
float32 invMassB;
|
||||
|
||||
float32 friction;
|
||||
|
||||
b3Vec3 point;
|
||||
|
||||
b3Vec3 normal;
|
||||
float32 normalMass;
|
||||
float32 normalImpulse;
|
||||
float32 velocityBias;
|
||||
|
||||
b3Vec3 tangent1;
|
||||
b3Vec3 tangent2;
|
||||
b3Mat22 tangentMass;
|
||||
b3Vec2 tangentImpulse;
|
||||
};
|
||||
|
||||
struct b3ClothSolverParticleContactPositionConstraint
|
||||
{
|
||||
u32 indexA;
|
||||
float32 invMassA;
|
||||
float32 radiusA;
|
||||
|
||||
u32 indexB;
|
||||
float32 invMassB;
|
||||
float32 radiusB;
|
||||
};
|
||||
|
||||
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;
|
||||
|
||||
b3DenseVec3* positions;
|
||||
b3DenseVec3* velocities;
|
||||
|
||||
u32 bodyContactCount;
|
||||
b3BodyContact** bodyContacts;
|
||||
|
||||
u32 particleContactCount;
|
||||
b3ParticleContact** particleContacts;
|
||||
|
||||
u32 triangleContactCount;
|
||||
b3TriangleContact** triangleContacts;
|
||||
};
|
||||
|
||||
inline float32 b3MixFriction(float32 u1, float32 u2)
|
||||
{
|
||||
return b3Sqrt(u1 * u2);
|
||||
}
|
||||
|
||||
class b3ClothContactSolver
|
||||
{
|
||||
public:
|
||||
b3ClothContactSolver(const b3ClothContactSolverDef& def);
|
||||
~b3ClothContactSolver();
|
||||
|
||||
void InitializeBodyContactConstraints();
|
||||
|
||||
void InitializeParticleContactConstraints();
|
||||
|
||||
void InitializeTriangleContactConstraints();
|
||||
|
||||
void WarmStart();
|
||||
|
||||
void SolveBodyContactVelocityConstraints();
|
||||
|
||||
void SolveParticleContactVelocityConstraints();
|
||||
|
||||
void SolveTriangleContactVelocityConstraints();
|
||||
|
||||
void StoreImpulses();
|
||||
|
||||
bool SolveBodyContactPositionConstraints();
|
||||
|
||||
bool SolveParticleContactPositionConstraints();
|
||||
|
||||
bool SolveTriangleContactPositionConstraints();
|
||||
|
||||
protected:
|
||||
b3StackAllocator* m_allocator;
|
||||
|
||||
b3DenseVec3* m_positions;
|
||||
b3DenseVec3* m_velocities;
|
||||
|
||||
u32 m_bodyContactCount;
|
||||
b3BodyContact** m_bodyContacts;
|
||||
b3ClothSolverBodyContactVelocityConstraint* m_bodyVelocityConstraints;
|
||||
b3ClothSolverBodyContactPositionConstraint* m_bodyPositionConstraints;
|
||||
|
||||
u32 m_particleContactCount;
|
||||
b3ParticleContact** m_particleContacts;
|
||||
b3ClothSolverParticleContactVelocityConstraint* m_particleVelocityConstraints;
|
||||
b3ClothSolverParticleContactPositionConstraint* m_particlePositionConstraints;
|
||||
|
||||
u32 m_triangleContactCount;
|
||||
b3TriangleContact** m_triangleContacts;
|
||||
b3ClothSolverTriangleContactVelocityConstraint* m_triangleVelocityConstraints;
|
||||
b3ClothSolverTriangleContactPositionConstraint* m_trianglePositionConstraints;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -27,14 +27,15 @@ class b3StackAllocator;
|
||||
class b3Particle;
|
||||
class b3Body;
|
||||
class b3Force;
|
||||
class b3BodyContact;
|
||||
class b3ParticleContact;
|
||||
class b3TriangleContact;
|
||||
|
||||
struct b3DenseVec3;
|
||||
struct b3DiagMat33;
|
||||
struct b3SparseSymMat33;
|
||||
|
||||
class b3BodyContact;
|
||||
class b3ParticleContact;
|
||||
class b3TriangleContact;
|
||||
|
||||
struct b3ClothSolverDef
|
||||
{
|
||||
b3StackAllocator* stack;
|
||||
@ -68,126 +69,6 @@ struct b3AccelerationConstraint
|
||||
void Apply(const b3ClothSolverData* data);
|
||||
};
|
||||
|
||||
struct b3ClothSolverBodyContactVelocityConstraint
|
||||
{
|
||||
u32 indexA;
|
||||
float32 invMassA;
|
||||
b3Mat33 invIA;
|
||||
|
||||
b3Body* bodyB;
|
||||
float32 invMassB;
|
||||
b3Mat33 invIB;
|
||||
|
||||
float32 friction;
|
||||
|
||||
b3Vec3 point;
|
||||
b3Vec3 rA;
|
||||
b3Vec3 rB;
|
||||
|
||||
b3Vec3 normal;
|
||||
float32 normalMass;
|
||||
float32 normalImpulse;
|
||||
float32 velocityBias;
|
||||
|
||||
b3Vec3 tangent1;
|
||||
b3Vec3 tangent2;
|
||||
b3Mat22 tangentMass;
|
||||
b3Vec2 tangentImpulse;
|
||||
};
|
||||
|
||||
struct b3ClothSolverBodyContactPositionConstraint
|
||||
{
|
||||
u32 indexA;
|
||||
float32 invMassA;
|
||||
b3Mat33 invIA;
|
||||
float32 radiusA;
|
||||
b3Vec3 localCenterA;
|
||||
|
||||
b3Body* bodyB;
|
||||
float32 invMassB;
|
||||
b3Mat33 invIB;
|
||||
float32 radiusB;
|
||||
b3Vec3 localCenterB;
|
||||
|
||||
b3Vec3 rA;
|
||||
b3Vec3 rB;
|
||||
|
||||
b3Vec3 localPointA;
|
||||
b3Vec3 localPointB;
|
||||
};
|
||||
|
||||
struct b3ClothSolverParticleContactVelocityConstraint
|
||||
{
|
||||
u32 indexA;
|
||||
float32 invMassA;
|
||||
|
||||
u32 indexB;
|
||||
float32 invMassB;
|
||||
|
||||
float32 friction;
|
||||
|
||||
b3Vec3 point;
|
||||
|
||||
b3Vec3 normal;
|
||||
float32 normalMass;
|
||||
float32 normalImpulse;
|
||||
float32 velocityBias;
|
||||
|
||||
b3Vec3 tangent1;
|
||||
b3Vec3 tangent2;
|
||||
b3Mat22 tangentMass;
|
||||
b3Vec2 tangentImpulse;
|
||||
};
|
||||
|
||||
struct b3ClothSolverParticleContactPositionConstraint
|
||||
{
|
||||
u32 indexA;
|
||||
float32 invMassA;
|
||||
float32 radiusA;
|
||||
|
||||
u32 indexB;
|
||||
float32 invMassB;
|
||||
float32 radiusB;
|
||||
};
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
class b3ClothSolver
|
||||
{
|
||||
public:
|
||||
@ -211,39 +92,6 @@ private:
|
||||
// Solve Ax = b.
|
||||
void Solve(b3DenseVec3& x, u32& iterations, const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const;
|
||||
|
||||
//
|
||||
void InitializeBodyContactConstraints();
|
||||
|
||||
//
|
||||
void InitializeParticleContactConstraints();
|
||||
|
||||
//
|
||||
void InitializeTriangleContactConstraints();
|
||||
|
||||
//
|
||||
void WarmStart();
|
||||
|
||||
//
|
||||
void SolveBodyContactVelocityConstraints();
|
||||
|
||||
//
|
||||
void SolveParticleContactVelocityConstraints();
|
||||
|
||||
//
|
||||
void SolveTriangleContactVelocityConstraints();
|
||||
|
||||
//
|
||||
void StoreImpulses();
|
||||
|
||||
//
|
||||
bool SolveBodyContactPositionConstraints();
|
||||
|
||||
//
|
||||
bool SolveParticleContactPositionConstraints();
|
||||
|
||||
//
|
||||
bool SolveTriangleContactPositionConstraints();
|
||||
|
||||
b3StackAllocator* m_allocator;
|
||||
|
||||
u32 m_particleCapacity;
|
||||
@ -257,24 +105,18 @@ private:
|
||||
u32 m_constraintCapacity;
|
||||
u32 m_constraintCount;
|
||||
b3AccelerationConstraint* m_constraints;
|
||||
|
||||
|
||||
u32 m_bodyContactCapacity;
|
||||
u32 m_bodyContactCount;
|
||||
b3BodyContact** m_bodyContacts;
|
||||
b3ClothSolverBodyContactVelocityConstraint* m_bodyVelocityConstraints;
|
||||
b3ClothSolverBodyContactPositionConstraint* m_bodyPositionConstraints;
|
||||
|
||||
u32 m_particleContactCapacity;
|
||||
u32 m_particleContactCount;
|
||||
b3ParticleContact** m_particleContacts;
|
||||
b3ClothSolverParticleContactVelocityConstraint* m_particleVelocityConstraints;
|
||||
b3ClothSolverParticleContactPositionConstraint* m_particlePositionConstraints;
|
||||
|
||||
|
||||
u32 m_triangleContactCapacity;
|
||||
u32 m_triangleContactCount;
|
||||
b3TriangleContact** m_triangleContacts;
|
||||
b3ClothSolverTriangleContactVelocityConstraint* m_triangleVelocityConstraints;
|
||||
b3ClothSolverTriangleContactPositionConstraint* m_trianglePositionConstraints;
|
||||
|
||||
b3ClothSolverData m_solverData;
|
||||
};
|
||||
|
||||
@ -80,8 +80,8 @@ public:
|
||||
b3Vec3 t1, t2;
|
||||
b3Vec2 tangentImpulse;
|
||||
|
||||
//
|
||||
bool active;
|
||||
b3BodyContact* m_prev;
|
||||
b3BodyContact* m_next;
|
||||
};
|
||||
|
||||
struct b3BodyContactWorldPoint
|
||||
@ -198,6 +198,7 @@ private:
|
||||
friend class b3List2<b3Particle>;
|
||||
friend class b3Cloth;
|
||||
friend class b3ClothSolver;
|
||||
friend class b3ClothContactSolver;
|
||||
friend class b3Force;
|
||||
friend class b3SpringForce;
|
||||
friend class b3BendForce;
|
||||
@ -239,9 +240,6 @@ private:
|
||||
// Cloth mesh vertex index.
|
||||
u32 m_vertex;
|
||||
|
||||
// Contact
|
||||
b3BodyContact m_contact;
|
||||
|
||||
// Solver temp
|
||||
|
||||
// Identifier
|
||||
|
||||
@ -151,6 +151,7 @@ static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m)
|
||||
|
||||
b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) :
|
||||
m_particleBlocks(sizeof(b3Particle)),
|
||||
m_bodyContactBlocks(sizeof(b3BodyContact)),
|
||||
m_particleContactBlocks(sizeof(b3ParticleContact)),
|
||||
m_triangleContactBlocks(sizeof(b3TriangleContact))
|
||||
{
|
||||
@ -509,25 +510,30 @@ bool b3Cloth::RayCast(b3RayCastOutput* output, const b3RayCastInput* input, u32
|
||||
void b3Cloth::UpdateBodyContacts()
|
||||
{
|
||||
B3_PROFILE("Update Body Contacts");
|
||||
|
||||
// Clear buffer
|
||||
b3BodyContact* c = m_bodyContactList.m_head;
|
||||
while (c)
|
||||
{
|
||||
b3BodyContact* c0 = c;
|
||||
c = c->m_next;
|
||||
m_bodyContactList.Remove(c0);
|
||||
c0->~b3BodyContact();
|
||||
m_bodyContactBlocks.Free(c0);
|
||||
}
|
||||
|
||||
// Create contacts
|
||||
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
|
||||
{
|
||||
b3BodyContact* c = &p->m_contact;
|
||||
|
||||
b3BodyContact c0 = *c;
|
||||
|
||||
c->active = false;
|
||||
|
||||
b3Sphere s1;
|
||||
s1.vertex = p->m_position;
|
||||
s1.radius = p->m_radius;
|
||||
|
||||
// Find the deepest penetration
|
||||
b3Shape* bestShape = nullptr;
|
||||
float32 bestSeparation = 0.0f;
|
||||
b3Vec3 bestPoint(0.0f, 0.0f, 0.0f);
|
||||
b3Vec3 bestNormal(0.0f, 0.0f, 0.0f);
|
||||
b3Shape* bestShape = nullptr;
|
||||
|
||||
for (b3Body* body = m_world->GetBodyList().m_head; body; body = body->GetNext())
|
||||
{
|
||||
@ -545,10 +551,10 @@ void b3Cloth::UpdateBodyContacts()
|
||||
{
|
||||
if (output.separation < bestSeparation)
|
||||
{
|
||||
bestShape = shape;
|
||||
bestSeparation = output.separation;
|
||||
bestPoint = output.point;
|
||||
bestNormal = output.normal;
|
||||
bestShape = shape;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -562,25 +568,21 @@ void b3Cloth::UpdateBodyContacts()
|
||||
// Ensure the the normal points from the particle 1 to shape 2
|
||||
b3Shape* shape = bestShape;
|
||||
b3Body* body = shape->GetBody();
|
||||
float32 s = bestSeparation;
|
||||
b3Vec3 cb = bestPoint;
|
||||
b3Vec3 n = -bestNormal;
|
||||
float32 separation = bestSeparation;
|
||||
b3Vec3 point = bestPoint;
|
||||
b3Vec3 normal = -bestNormal;
|
||||
|
||||
c->active = true;
|
||||
b3BodyContact* c = (b3BodyContact*)m_bodyContactBlocks.Allocate();
|
||||
c->p1 = p;
|
||||
c->s2 = shape;
|
||||
c->localPoint1.SetZero();
|
||||
c->localPoint2 = body->GetLocalPoint(cb);
|
||||
c->t1 = b3Perp(n);
|
||||
c->t2 = b3Cross(c->t1, n);
|
||||
c->localPoint2 = body->GetLocalPoint(point);
|
||||
c->t1 = b3Perp(normal);
|
||||
c->t2 = b3Cross(c->t1, normal);
|
||||
c->normalImpulse = 0.0f;
|
||||
c->tangentImpulse.SetZero();
|
||||
|
||||
if (c0.active == true)
|
||||
{
|
||||
c->normalImpulse = c->normalImpulse;
|
||||
c->tangentImpulse = c->tangentImpulse;
|
||||
}
|
||||
m_bodyContactList.PushFront(c);
|
||||
}
|
||||
}
|
||||
|
||||
@ -888,7 +890,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
|
||||
solverDef.stack = &m_world->m_stackAllocator;
|
||||
solverDef.particleCapacity = m_particleList.m_count;
|
||||
solverDef.forceCapacity = m_forceList.m_count;
|
||||
solverDef.bodyContactCapacity = m_particleList.m_count;
|
||||
solverDef.bodyContactCapacity = m_bodyContactList.m_count;
|
||||
solverDef.particleContactCapacity = m_particleContactList.m_count;
|
||||
solverDef.triangleContactCapacity = m_triangleContactList.m_count;
|
||||
|
||||
@ -903,25 +905,20 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
|
||||
{
|
||||
solver.Add(f);
|
||||
}
|
||||
|
||||
for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
|
||||
|
||||
for (b3BodyContact* c = m_bodyContactList.m_head; c; c = c->m_next)
|
||||
{
|
||||
b3BodyContact* bc = &p->m_contact;
|
||||
|
||||
if (bc->active)
|
||||
{
|
||||
solver.Add(bc);
|
||||
}
|
||||
solver.Add(c);
|
||||
}
|
||||
|
||||
for (b3ParticleContact* pc = m_particleContactList.m_head; pc; pc = pc->m_next)
|
||||
|
||||
for (b3ParticleContact* c = m_particleContactList.m_head; c; c = c->m_next)
|
||||
{
|
||||
solver.Add(pc);
|
||||
solver.Add(c);
|
||||
}
|
||||
|
||||
for (b3TriangleContact* tc = m_triangleContactList.m_head; tc; tc = tc->m_next)
|
||||
|
||||
for (b3TriangleContact* c = m_triangleContactList.m_head; c; c = c->m_next)
|
||||
{
|
||||
solver.Add(tc);
|
||||
solver.Add(c);
|
||||
}
|
||||
|
||||
// Solve
|
||||
@ -935,10 +932,8 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
|
||||
}
|
||||
}
|
||||
|
||||
void b3Cloth::Step(float32 dt, const b3Vec3& gravity)
|
||||
void b3Cloth::UpdateContacts()
|
||||
{
|
||||
B3_PROFILE("Step");
|
||||
|
||||
// Update body contacts
|
||||
UpdateBodyContacts();
|
||||
|
||||
@ -947,7 +942,15 @@ void b3Cloth::Step(float32 dt, const b3Vec3& gravity)
|
||||
|
||||
// Update triangle contacts
|
||||
UpdateTriangleContacts();
|
||||
|
||||
}
|
||||
|
||||
void b3Cloth::Step(float32 dt, const b3Vec3& gravity)
|
||||
{
|
||||
B3_PROFILE("Step");
|
||||
|
||||
// Update contacts
|
||||
UpdateContacts();
|
||||
|
||||
// Solve constraints, integrate state, clear forces and translations.
|
||||
if (dt > 0.0f)
|
||||
{
|
||||
@ -973,25 +976,6 @@ void b3Cloth::Draw() const
|
||||
{
|
||||
b3Draw_draw->DrawPoint(p->m_position, 4.0f, b3Color_green);
|
||||
}
|
||||
|
||||
b3BodyContact* c = &p->m_contact;
|
||||
|
||||
if (c->active)
|
||||
{
|
||||
b3Particle* pA = c->p1;
|
||||
b3Body* bB = c->s2->GetBody();
|
||||
|
||||
b3Transform xfA;
|
||||
xfA.rotation.SetIdentity();
|
||||
xfA.position = pA->m_position;
|
||||
|
||||
b3Transform xfB = bB->GetTransform();
|
||||
|
||||
b3BodyContactWorldPoint cp;
|
||||
cp.Initialize(c, pA->m_radius, xfA, c->s2->m_radius, xfB);
|
||||
|
||||
b3Draw_draw->DrawSegment(cp.point, cp.point + cp.normal, b3Color_yellow);
|
||||
}
|
||||
}
|
||||
|
||||
for (b3Force* f = m_forceList.m_head; f; f = f->m_next)
|
||||
|
||||
982
src/bounce/dynamics/cloth/cloth_contact_solver.cpp
Normal file
982
src/bounce/dynamics/cloth/cloth_contact_solver.cpp
Normal file
@ -0,0 +1,982 @@
|
||||
/*
|
||||
* 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_contact_solver.h>
|
||||
#include <bounce/dynamics/cloth/particle.h>
|
||||
#include <bounce/dynamics/cloth/dense_vec3.h>
|
||||
#include <bounce/dynamics/shapes/shape.h>
|
||||
#include <bounce/dynamics/body.h>
|
||||
#include <bounce/common/memory/stack_allocator.h>
|
||||
|
||||
b3ClothContactSolver::b3ClothContactSolver(const b3ClothContactSolverDef& def)
|
||||
{
|
||||
m_allocator = def.allocator;
|
||||
|
||||
m_positions = def.positions;
|
||||
m_velocities = def.velocities;
|
||||
|
||||
m_bodyContactCount = def.bodyContactCount;
|
||||
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_particleContactCount = def.particleContactCount;
|
||||
m_particleContacts = def.particleContacts;
|
||||
m_particleVelocityConstraints = (b3ClothSolverParticleContactVelocityConstraint*)m_allocator->Allocate(m_particleContactCount * sizeof(b3ClothSolverParticleContactVelocityConstraint));
|
||||
m_particlePositionConstraints = (b3ClothSolverParticleContactPositionConstraint*)m_allocator->Allocate(m_particleContactCount * sizeof(b3ClothSolverParticleContactPositionConstraint));
|
||||
|
||||
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_particlePositionConstraints);
|
||||
m_allocator->Free(m_particleVelocityConstraints);
|
||||
|
||||
m_allocator->Free(m_bodyPositionConstraints);
|
||||
m_allocator->Free(m_bodyVelocityConstraints);
|
||||
}
|
||||
|
||||
void b3ClothContactSolver::InitializeBodyContactConstraints()
|
||||
{
|
||||
b3DenseVec3& x = *m_positions;
|
||||
b3DenseVec3& v = *m_velocities;
|
||||
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3BodyContact* c = m_bodyContacts[i];
|
||||
b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
|
||||
b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i;
|
||||
|
||||
vc->indexA = c->p1->m_solverId;
|
||||
vc->bodyB = c->s2->GetBody();
|
||||
|
||||
vc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass;
|
||||
vc->invMassB = vc->bodyB->GetInverseMass();
|
||||
|
||||
vc->invIA.SetZero();
|
||||
vc->invIB = vc->bodyB->GetWorldInverseInertia();
|
||||
|
||||
vc->friction = b3MixFriction(c->p1->m_friction, c->s2->GetFriction());
|
||||
|
||||
pc->indexA = c->p1->m_solverId;
|
||||
pc->bodyB = vc->bodyB;
|
||||
|
||||
pc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass;
|
||||
pc->invMassB = vc->bodyB->m_invMass;
|
||||
|
||||
pc->invIA.SetZero();
|
||||
pc->invIB = vc->bodyB->m_worldInvI;
|
||||
|
||||
pc->radiusA = c->p1->m_radius;
|
||||
pc->radiusB = c->s2->m_radius;
|
||||
|
||||
pc->localCenterA.SetZero();
|
||||
pc->localCenterB = pc->bodyB->m_sweep.localCenter;
|
||||
|
||||
pc->localPointA = c->localPoint1;
|
||||
pc->localPointB = c->localPoint2;
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3BodyContact* c = m_bodyContacts[i];
|
||||
b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
|
||||
b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i;
|
||||
|
||||
u32 indexA = vc->indexA;
|
||||
b3Body* bodyB = vc->bodyB;
|
||||
|
||||
float32 mA = vc->invMassA;
|
||||
float32 mB = vc->invMassB;
|
||||
|
||||
b3Mat33 iA = vc->invIA;
|
||||
b3Mat33 iB = vc->invIB;
|
||||
|
||||
b3Vec3 xA = x[indexA];
|
||||
b3Vec3 xB = bodyB->m_sweep.worldCenter;
|
||||
|
||||
b3Quat qA; qA.SetIdentity();
|
||||
b3Quat qB = bodyB->m_sweep.orientation;
|
||||
|
||||
b3Vec3 localCenterA = pc->localCenterA;
|
||||
b3Vec3 localCenterB = pc->localCenterB;
|
||||
|
||||
b3Transform xfA;
|
||||
xfA.rotation = b3QuatMat33(qA);
|
||||
xfA.position = xA - b3Mul(xfA.rotation, localCenterA);
|
||||
|
||||
b3Transform xfB;
|
||||
xfB.rotation = b3QuatMat33(qB);
|
||||
xfB.position = xB - b3Mul(xfB.rotation, localCenterB);
|
||||
|
||||
b3BodyContactWorldPoint wp;
|
||||
wp.Initialize(c, pc->radiusA, xfA, pc->radiusB, xfB);
|
||||
|
||||
vc->normal = wp.normal;
|
||||
vc->tangent1 = c->t1;
|
||||
vc->tangent2 = c->t2;
|
||||
vc->point = wp.point;
|
||||
|
||||
b3Vec3 point = vc->point;
|
||||
|
||||
b3Vec3 rA = point - xA;
|
||||
b3Vec3 rB = point - xB;
|
||||
|
||||
vc->rA = rA;
|
||||
vc->rB = rB;
|
||||
|
||||
vc->normalImpulse = c->normalImpulse;
|
||||
vc->tangentImpulse = c->tangentImpulse;
|
||||
|
||||
{
|
||||
b3Vec3 n = vc->normal;
|
||||
|
||||
b3Vec3 rnA = b3Cross(rA, n);
|
||||
b3Vec3 rnB = b3Cross(rB, n);
|
||||
|
||||
float32 K = mA + mB + b3Dot(iA * rnA, rnA) + b3Dot(iB * rnB, rnB);
|
||||
|
||||
vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f;
|
||||
vc->velocityBias = 0.0f;
|
||||
}
|
||||
|
||||
{
|
||||
b3Vec3 t1 = vc->tangent1;
|
||||
b3Vec3 t2 = vc->tangent2;
|
||||
|
||||
b3Vec3 rn1A = b3Cross(rA, t1);
|
||||
b3Vec3 rn1B = b3Cross(rB, t1);
|
||||
b3Vec3 rn2A = b3Cross(rA, t2);
|
||||
b3Vec3 rn2B = b3Cross(rB, t2);
|
||||
|
||||
// dot(t1, t2) = 0
|
||||
// J1_l1 * M1 * J2_l1 = J1_l2 * M2 * J2_l2 = 0
|
||||
float32 k11 = mA + mB + b3Dot(iA * rn1A, rn1A) + b3Dot(iB * rn1B, rn1B);
|
||||
float32 k12 = b3Dot(iA * rn1A, rn2A) + b3Dot(iB * rn1B, rn2B);
|
||||
float32 k22 = mA + mB + b3Dot(iA * rn2A, rn2A) + b3Dot(iB * rn2B, rn2B);
|
||||
|
||||
b3Mat22 K;
|
||||
K.x.Set(k11, k12);
|
||||
K.y.Set(k12, k22);
|
||||
|
||||
vc->tangentMass = b3Inverse(K);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothContactSolver::InitializeParticleContactConstraints()
|
||||
{
|
||||
b3DenseVec3& x = *m_positions;
|
||||
b3DenseVec3& v = *m_velocities;
|
||||
|
||||
for (u32 i = 0; i < m_particleContactCount; ++i)
|
||||
{
|
||||
b3ParticleContact* c = m_particleContacts[i];
|
||||
b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i;
|
||||
b3ClothSolverParticleContactPositionConstraint* pc = m_particlePositionConstraints + i;
|
||||
|
||||
vc->indexA = c->p1->m_solverId;
|
||||
vc->indexB = c->p2->m_solverId;
|
||||
|
||||
vc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass;
|
||||
vc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass;
|
||||
|
||||
vc->friction = b3MixFriction(c->p1->m_friction, c->p2->m_friction);
|
||||
|
||||
pc->indexA = c->p1->m_solverId;
|
||||
pc->indexB = c->p2->m_solverId;
|
||||
|
||||
pc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass;
|
||||
pc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass;
|
||||
|
||||
pc->radiusA = c->p1->m_radius;
|
||||
pc->radiusB = c->p2->m_radius;
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_particleContactCount; ++i)
|
||||
{
|
||||
b3ParticleContact* c = m_particleContacts[i];
|
||||
b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i;
|
||||
b3ClothSolverParticleContactPositionConstraint* pc = m_particlePositionConstraints + i;
|
||||
|
||||
u32 indexA = vc->indexA;
|
||||
u32 indexB = vc->indexB;
|
||||
|
||||
float32 mA = vc->invMassA;
|
||||
float32 mB = vc->invMassB;
|
||||
|
||||
b3Vec3 xA = x[indexA];
|
||||
b3Vec3 xB = x[indexB];
|
||||
|
||||
b3ParticleContactWorldPoint wp;
|
||||
wp.Initialize(c);
|
||||
|
||||
vc->normal = wp.normal;
|
||||
vc->tangent1 = c->t1;
|
||||
vc->tangent2 = c->t2;
|
||||
vc->point = wp.point;
|
||||
|
||||
b3Vec3 point = vc->point;
|
||||
|
||||
vc->normalImpulse = c->normalImpulse;
|
||||
vc->tangentImpulse = c->tangentImpulse;
|
||||
|
||||
{
|
||||
b3Vec3 n = vc->normal;
|
||||
|
||||
float32 K = mA + mB;
|
||||
|
||||
vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f;
|
||||
|
||||
vc->velocityBias = 0.0f;
|
||||
}
|
||||
|
||||
{
|
||||
b3Vec3 t1 = vc->tangent1;
|
||||
b3Vec3 t2 = vc->tangent2;
|
||||
|
||||
float32 k11 = mA + mB;
|
||||
float32 k12 = 0.0f;
|
||||
float32 k22 = mA + mB;
|
||||
|
||||
b3Mat22 K;
|
||||
K.x.Set(k11, k12);
|
||||
K.y.Set(k12, k22);
|
||||
|
||||
vc->tangentMass = b3Inverse(K);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothContactSolver::InitializeTriangleContactConstraints()
|
||||
{
|
||||
b3DenseVec3& x = *m_positions;
|
||||
|
||||
for (u32 i = 0; i < m_triangleContactCount; ++i)
|
||||
{
|
||||
b3TriangleContact* c = m_triangleContacts[i];
|
||||
b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i;
|
||||
b3ClothSolverTriangleContactPositionConstraint* pc = m_trianglePositionConstraints + i;
|
||||
|
||||
vc->indexA = c->p1->m_solverId;
|
||||
vc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass;
|
||||
|
||||
vc->indexB = c->p2->m_solverId;
|
||||
vc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass;
|
||||
|
||||
vc->indexC = c->p3->m_solverId;
|
||||
vc->invMassC = c->p3->m_type == e_staticParticle ? 0.0f : c->p3->m_invMass;
|
||||
|
||||
vc->indexD = c->p4->m_solverId;
|
||||
vc->invMassD = c->p4->m_type == e_staticParticle ? 0.0f : c->p4->m_invMass;
|
||||
|
||||
pc->indexA = c->p1->m_solverId;
|
||||
pc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass;
|
||||
pc->radiusA = c->p1->m_radius;
|
||||
|
||||
pc->indexB = c->p2->m_solverId;
|
||||
pc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass;
|
||||
|
||||
pc->indexC = c->p3->m_solverId;
|
||||
pc->invMassC = c->p3->m_type == e_staticParticle ? 0.0f : c->p3->m_invMass;
|
||||
|
||||
pc->indexD = c->p4->m_solverId;
|
||||
pc->invMassD = c->p4->m_type == e_staticParticle ? 0.0f : c->p4->m_invMass;
|
||||
|
||||
pc->triangleRadius = 0.0f;
|
||||
pc->front = c->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->JC = b3Cross(xD - xB, N_n);
|
||||
vc->JD = b3Cross(xC - xB, N_n);
|
||||
vc->JB = b3Cross(xC - xD, N_n) - n;
|
||||
|
||||
// Compute effective mass.
|
||||
float32 K = mA + mB + mC + mD;
|
||||
|
||||
vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f;
|
||||
vc->normalImpulse = c->normalImpulse;
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothContactSolver::WarmStart()
|
||||
{
|
||||
b3DenseVec3& v = *m_velocities;
|
||||
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
|
||||
|
||||
u32 indexA = vc->indexA;
|
||||
b3Body* bodyB = vc->bodyB;
|
||||
|
||||
b3Vec3 vA = v[indexA];
|
||||
b3Vec3 vB = bodyB->GetLinearVelocity();
|
||||
|
||||
b3Vec3 wA; wA.SetZero();
|
||||
b3Vec3 wB = bodyB->GetAngularVelocity();
|
||||
|
||||
float32 mA = vc->invMassA;
|
||||
float32 mB = vc->invMassB;
|
||||
|
||||
b3Mat33 iA = vc->invIA;
|
||||
b3Mat33 iB = vc->invIB;
|
||||
|
||||
b3Vec3 P = vc->normalImpulse * vc->normal;
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * b3Cross(vc->rA, P);
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * b3Cross(vc->rB, P);
|
||||
|
||||
b3Vec3 P1 = vc->tangentImpulse.x * vc->tangent1;
|
||||
b3Vec3 P2 = vc->tangentImpulse.y * vc->tangent2;
|
||||
|
||||
vA -= mA * (P1 + P2);
|
||||
wA -= iA * b3Cross(vc->rA, P1 + P2);
|
||||
|
||||
vB += mB * (P1 + P2);
|
||||
wB += iB * b3Cross(vc->rB, P1 + P2);
|
||||
|
||||
v[indexA] = vA;
|
||||
|
||||
bodyB->SetLinearVelocity(vB);
|
||||
bodyB->SetAngularVelocity(wB);
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_particleContactCount; ++i)
|
||||
{
|
||||
b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i;
|
||||
|
||||
u32 indexA = vc->indexA;
|
||||
u32 indexB = vc->indexB;
|
||||
|
||||
b3Vec3 vA = v[indexA];
|
||||
b3Vec3 vB = v[indexB];
|
||||
|
||||
float32 mA = vc->invMassA;
|
||||
float32 mB = vc->invMassB;
|
||||
|
||||
b3Vec3 P = vc->normalImpulse * vc->normal;
|
||||
|
||||
vA -= mA * P;
|
||||
vB += mB * P;
|
||||
|
||||
b3Vec3 P1 = vc->tangentImpulse.x * vc->tangent1;
|
||||
b3Vec3 P2 = vc->tangentImpulse.y * vc->tangent2;
|
||||
|
||||
vA -= mA * (P1 + P2);
|
||||
vB += mB * (P1 + P2);
|
||||
|
||||
v[indexA] = vA;
|
||||
v[indexB] = vB;
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
|
||||
|
||||
u32 indexA = vc->indexA;
|
||||
b3Body* bodyB = vc->bodyB;
|
||||
|
||||
b3Vec3 vA = v[indexA];
|
||||
b3Vec3 vB = bodyB->GetLinearVelocity();
|
||||
|
||||
b3Vec3 wA; wA.SetZero();
|
||||
b3Vec3 wB = bodyB->GetAngularVelocity();
|
||||
|
||||
float32 mA = vc->invMassA;
|
||||
float32 mB = vc->invMassB;
|
||||
|
||||
b3Mat33 iA = vc->invIA;
|
||||
b3Mat33 iB = vc->invIB;
|
||||
|
||||
b3Vec3 normal = vc->normal;
|
||||
b3Vec3 point = vc->point;
|
||||
|
||||
b3Vec3 rA = vc->rA;
|
||||
b3Vec3 rB = vc->rB;
|
||||
|
||||
// Solve normal constraint.
|
||||
{
|
||||
b3Vec3 dv = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA);
|
||||
float32 Cdot = b3Dot(normal, dv);
|
||||
|
||||
float32 impulse = vc->normalMass * (-Cdot + vc->velocityBias);
|
||||
|
||||
float32 oldImpulse = vc->normalImpulse;
|
||||
vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f);
|
||||
impulse = vc->normalImpulse - oldImpulse;
|
||||
|
||||
b3Vec3 P = impulse * normal;
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * b3Cross(rA, P);
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * b3Cross(rB, P);
|
||||
}
|
||||
|
||||
// Solve tangent constraints.
|
||||
{
|
||||
b3Vec3 dv = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA);
|
||||
|
||||
b3Vec2 Cdot;
|
||||
Cdot.x = b3Dot(dv, vc->tangent1);
|
||||
Cdot.y = b3Dot(dv, vc->tangent2);
|
||||
|
||||
b3Vec2 impulse = vc->tangentMass * -Cdot;
|
||||
b3Vec2 oldImpulse = vc->tangentImpulse;
|
||||
vc->tangentImpulse += impulse;
|
||||
|
||||
float32 maxImpulse = vc->friction * vc->normalImpulse;
|
||||
if (b3Dot(vc->tangentImpulse, vc->tangentImpulse) > maxImpulse * maxImpulse)
|
||||
{
|
||||
vc->tangentImpulse.Normalize();
|
||||
vc->tangentImpulse *= maxImpulse;
|
||||
}
|
||||
|
||||
impulse = vc->tangentImpulse - oldImpulse;
|
||||
|
||||
b3Vec3 P1 = impulse.x * vc->tangent1;
|
||||
b3Vec3 P2 = impulse.y * vc->tangent2;
|
||||
b3Vec3 P = P1 + P2;
|
||||
|
||||
vA -= mA * P;
|
||||
wA -= iA * b3Cross(rA, P);
|
||||
|
||||
vB += mB * P;
|
||||
wB += iB * b3Cross(rB, P);
|
||||
}
|
||||
|
||||
v[indexA] = vA;
|
||||
|
||||
bodyB->SetLinearVelocity(vB);
|
||||
bodyB->SetAngularVelocity(wB);
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothContactSolver::SolveParticleContactVelocityConstraints()
|
||||
{
|
||||
b3DenseVec3& v = *m_velocities;
|
||||
|
||||
for (u32 i = 0; i < m_particleContactCount; ++i)
|
||||
{
|
||||
b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i;
|
||||
|
||||
u32 indexA = vc->indexA;
|
||||
u32 indexB = vc->indexB;
|
||||
|
||||
b3Vec3 vA = v[indexA];
|
||||
b3Vec3 vB = v[indexB];
|
||||
|
||||
float32 mA = vc->invMassA;
|
||||
float32 mB = vc->invMassB;
|
||||
|
||||
b3Vec3 normal = vc->normal;
|
||||
b3Vec3 point = vc->point;
|
||||
|
||||
// Solve normal constraint.
|
||||
{
|
||||
b3Vec3 dv = vB - vA;
|
||||
float32 Cdot = b3Dot(normal, dv);
|
||||
|
||||
float32 impulse = vc->normalMass * (-Cdot + vc->velocityBias);
|
||||
|
||||
float32 oldImpulse = vc->normalImpulse;
|
||||
vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f);
|
||||
impulse = vc->normalImpulse - oldImpulse;
|
||||
|
||||
b3Vec3 P = impulse * normal;
|
||||
|
||||
vA -= mA * P;
|
||||
vB += mB * P;
|
||||
}
|
||||
|
||||
// Solve tangent constraints.
|
||||
{
|
||||
b3Vec3 dv = vB - vA;
|
||||
|
||||
b3Vec2 Cdot;
|
||||
Cdot.x = b3Dot(dv, vc->tangent1);
|
||||
Cdot.y = b3Dot(dv, vc->tangent2);
|
||||
|
||||
b3Vec2 impulse = vc->tangentMass * -Cdot;
|
||||
b3Vec2 oldImpulse = vc->tangentImpulse;
|
||||
vc->tangentImpulse += impulse;
|
||||
|
||||
float32 maxImpulse = vc->friction * vc->normalImpulse;
|
||||
if (b3Dot(vc->tangentImpulse, vc->tangentImpulse) > maxImpulse * maxImpulse)
|
||||
{
|
||||
vc->tangentImpulse.Normalize();
|
||||
vc->tangentImpulse *= maxImpulse;
|
||||
}
|
||||
|
||||
impulse = vc->tangentImpulse - oldImpulse;
|
||||
|
||||
b3Vec3 P1 = impulse.x * vc->tangent1;
|
||||
b3Vec3 P2 = impulse.y * vc->tangent2;
|
||||
b3Vec3 P = P1 + P2;
|
||||
|
||||
vA -= mA * P;
|
||||
vB += mB * P;
|
||||
}
|
||||
|
||||
v[indexA] = vA;
|
||||
v[indexB] = vB;
|
||||
}
|
||||
}
|
||||
|
||||
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];
|
||||
|
||||
b3Vec3 n = vc->JA;
|
||||
|
||||
// Allow some slop and prevent large corrections.
|
||||
float32 Cdot = b3Dot(n, vA - vB);
|
||||
|
||||
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)
|
||||
{
|
||||
b3BodyContact* c = m_bodyContacts[i];
|
||||
b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
|
||||
|
||||
c->normalImpulse = vc->normalImpulse;
|
||||
c->tangentImpulse = vc->tangentImpulse;
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_particleContactCount; ++i)
|
||||
{
|
||||
b3ParticleContact* c = m_particleContacts[i];
|
||||
b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i;
|
||||
|
||||
c->normalImpulse = vc->normalImpulse;
|
||||
c->tangentImpulse = vc->tangentImpulse;
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_triangleContactCount; ++i)
|
||||
{
|
||||
b3TriangleContact* c = m_triangleContacts[i];
|
||||
b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i;
|
||||
|
||||
c->normalImpulse = vc->normalImpulse;
|
||||
}
|
||||
}
|
||||
|
||||
struct b3ClothSolverBodyContactSolverPoint
|
||||
{
|
||||
void Initialize(const b3ClothSolverBodyContactPositionConstraint* pc, const b3Transform& xfA, const b3Transform& xfB)
|
||||
{
|
||||
b3Vec3 cA = b3Mul(xfA, pc->localPointA);
|
||||
b3Vec3 cB = b3Mul(xfB, pc->localPointB);
|
||||
|
||||
float32 rA = pc->radiusA;
|
||||
float32 rB = pc->radiusB;
|
||||
|
||||
b3Vec3 d = cB - cA;
|
||||
float32 distance = b3Length(d);
|
||||
|
||||
b3Vec3 nA(0.0f, 1.0f, 0.0f);
|
||||
if (distance > B3_EPSILON)
|
||||
{
|
||||
nA = d / distance;
|
||||
}
|
||||
|
||||
b3Vec3 pA = cA + rA * nA;
|
||||
b3Vec3 pB = cB - rB * nA;
|
||||
|
||||
point = 0.5f * (pA + pB);
|
||||
normal = nA;
|
||||
separation = distance - rA - rB;
|
||||
}
|
||||
|
||||
b3Vec3 normal;
|
||||
b3Vec3 point;
|
||||
float32 separation;
|
||||
};
|
||||
|
||||
bool b3ClothContactSolver::SolveBodyContactPositionConstraints()
|
||||
{
|
||||
b3DenseVec3& x = *m_positions;
|
||||
|
||||
float32 minSeparation = 0.0f;
|
||||
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i;
|
||||
|
||||
u32 indexA = pc->indexA;
|
||||
float32 mA = pc->invMassA;
|
||||
b3Mat33 iA = pc->invIA;
|
||||
b3Vec3 localCenterA = pc->localCenterA;
|
||||
|
||||
b3Body* bodyB = pc->bodyB;
|
||||
float32 mB = pc->invMassB;
|
||||
b3Mat33 iB = pc->invIB;
|
||||
b3Vec3 localCenterB = pc->localCenterB;
|
||||
|
||||
b3Vec3 cA = x[indexA];
|
||||
b3Quat qA; qA.SetIdentity();
|
||||
|
||||
b3Vec3 cB = bodyB->m_sweep.worldCenter;
|
||||
b3Quat qB = bodyB->m_sweep.orientation;
|
||||
|
||||
// Solve normal constraint
|
||||
b3Transform xfA;
|
||||
xfA.rotation = b3QuatMat33(qA);
|
||||
xfA.position = cA - b3Mul(xfA.rotation, localCenterA);
|
||||
|
||||
b3Transform xfB;
|
||||
xfB.rotation = b3QuatMat33(qB);
|
||||
xfB.position = cB - b3Mul(xfB.rotation, localCenterB);
|
||||
|
||||
b3ClothSolverBodyContactSolverPoint cpcp;
|
||||
cpcp.Initialize(pc, xfA, xfB);
|
||||
|
||||
b3Vec3 normal = cpcp.normal;
|
||||
b3Vec3 point = cpcp.point;
|
||||
float32 separation = cpcp.separation;
|
||||
|
||||
// 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);
|
||||
|
||||
// Compute effective mass.
|
||||
b3Vec3 rA = point - cA;
|
||||
b3Vec3 rB = point - cB;
|
||||
|
||||
b3Vec3 rnA = b3Cross(rA, normal);
|
||||
b3Vec3 rnB = b3Cross(rB, normal);
|
||||
float32 K = mA + mB + b3Dot(rnA, iA * rnA) + b3Dot(rnB, iB * rnB);
|
||||
|
||||
// Compute normal impulse.
|
||||
float32 impulse = K > 0.0f ? -C / K : 0.0f;
|
||||
b3Vec3 P = impulse * normal;
|
||||
|
||||
cA -= mA * P;
|
||||
qA -= b3Derivative(qA, iA * b3Cross(rA, P));
|
||||
qA.Normalize();
|
||||
|
||||
cB += mB * P;
|
||||
qB += b3Derivative(qB, iB * b3Cross(rB, P));
|
||||
qB.Normalize();
|
||||
|
||||
x[indexA] = cA;
|
||||
|
||||
bodyB->m_sweep.worldCenter = cB;
|
||||
bodyB->m_sweep.orientation = qB;
|
||||
}
|
||||
|
||||
return minSeparation >= -3.0f * B3_LINEAR_SLOP;
|
||||
}
|
||||
|
||||
struct b3ClothSolverParticleContactSolverPoint
|
||||
{
|
||||
void Initialize(const b3Vec3& cA, float32 rA, const b3Vec3& cB, float32 rB)
|
||||
{
|
||||
b3Vec3 d = cB - cA;
|
||||
float32 distance = b3Length(d);
|
||||
|
||||
b3Vec3 nA(0.0f, 1.0f, 0.0f);
|
||||
if (distance > B3_EPSILON)
|
||||
{
|
||||
nA = d / distance;
|
||||
}
|
||||
|
||||
b3Vec3 pA = cA + rA * nA;
|
||||
b3Vec3 pB = cB - rB * nA;
|
||||
|
||||
point = 0.5f * (pA + pB);
|
||||
normal = nA;
|
||||
separation = distance - rA - rB;
|
||||
}
|
||||
|
||||
b3Vec3 point;
|
||||
b3Vec3 normal;
|
||||
float32 separation;
|
||||
};
|
||||
|
||||
bool b3ClothContactSolver::SolveParticleContactPositionConstraints()
|
||||
{
|
||||
b3DenseVec3& x = *m_positions;
|
||||
|
||||
float32 minSeparation = 0.0f;
|
||||
|
||||
for (u32 i = 0; i < m_particleContactCount; ++i)
|
||||
{
|
||||
b3ClothSolverParticleContactPositionConstraint* pc = m_particlePositionConstraints + i;
|
||||
|
||||
u32 indexA = pc->indexA;
|
||||
float32 mA = pc->invMassA;
|
||||
float32 rA = pc->radiusA;
|
||||
|
||||
u32 indexB = pc->indexB;
|
||||
float32 mB = pc->invMassB;
|
||||
float32 rB = pc->radiusB;
|
||||
|
||||
b3Vec3 xA = x[indexA];
|
||||
b3Vec3 xB = x[indexB];
|
||||
|
||||
b3ClothSolverParticleContactSolverPoint cpcp;
|
||||
cpcp.Initialize(xA, rA, xB, rB);
|
||||
|
||||
b3Vec3 normal = cpcp.normal;
|
||||
b3Vec3 point = cpcp.point;
|
||||
float32 separation = cpcp.separation;
|
||||
|
||||
// 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);
|
||||
|
||||
// Compute effective mass.
|
||||
float32 K = mA + mB;
|
||||
|
||||
// Compute normal impulse.
|
||||
float32 impulse = K > 0.0f ? -C / K : 0.0f;
|
||||
b3Vec3 P = impulse * normal;
|
||||
|
||||
xA -= mA * P;
|
||||
xB += mB * P;
|
||||
|
||||
x[indexA] = xA;
|
||||
x[indexB] = xB;
|
||||
}
|
||||
|
||||
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 JC = b3Cross(xD - xB, N_n);
|
||||
b3Vec3 JD = b3Cross(xC - xB, N_n);
|
||||
b3Vec3 JB = b3Cross(xC - xD, N_n) - n;
|
||||
|
||||
// Compute effective mass.
|
||||
float32 K = mA + mB + mC + mD;
|
||||
|
||||
// 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;
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@ -85,8 +85,6 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)
|
||||
m_userData = nullptr;
|
||||
m_x.SetZero();
|
||||
m_vertex = ~0;
|
||||
|
||||
m_contact.active = false;
|
||||
}
|
||||
|
||||
b3Particle::~b3Particle()
|
||||
@ -108,7 +106,5 @@ void b3Particle::SetType(b3ParticleType type)
|
||||
{
|
||||
m_velocity.SetZero();
|
||||
m_translation.SetZero();
|
||||
|
||||
m_contact.active = false;
|
||||
}
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user