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

use baumgarte for correcting positions

Browse Source
This commit is contained in:
Irlan 2018-07-25 15:19:27 -03:00
parent 65e5ff217e
commit e970e09541
6 changed files with 278 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/bounce/dynamics/body.h
View File

@ -293,7 +293,8 @@ private:
friend class b3MeshContact;
friend class b3ContactManager;
friend class b3ContactSolver;
friend class b3ClothSolver;
friend class b3Joint;
friend class b3JointManager;
friend class b3JointSolver;

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

@ -64,7 +64,7 @@ struct b3AccelerationConstraint
void Apply(const b3ClothSolverData* data);
};
struct b3ClothContactVelocityConstraint
struct b3ClothSolverContactVelocityConstraint
{
u32 indexA;
float32 invMassA;
@ -94,6 +94,28 @@ struct b3ClothContactVelocityConstraint
float32 motorImpulse;
};
struct b3ClothSolverContactPositionConstraint
{
u32 indexA;
float32 invMassA;
b3Mat33 invIA;
float32 radiusA;
b3Vec3 localCenterA;
b3Body* bodyB;
b3Vec3 localCenterB;
float32 invMassB;
b3Mat33 invIB;
float32 radiusB;
b3Vec3 rA;
b3Vec3 rB;
b3Vec3 localNormalA;
b3Vec3 localPointA;
b3Vec3 localPointB;
};
class b3ClothSolver
{
public:
@ -116,7 +138,7 @@ private:
void Solve(b3DenseVec3& x, u32& iterations, const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const;
//
void InitializeVelocityConstraints();
void InitializeConstraints();
//
void WarmStart();
@ -127,6 +149,9 @@ private:
//
void StoreImpulses();
//
bool SolvePositionConstraints();
b3StackAllocator* m_allocator;
u32 m_particleCapacity;
@ -136,16 +161,17 @@ private:
u32 m_forceCapacity;
u32 m_forceCount;
b3Force** m_forces;
u32 m_constraintCapacity;
u32 m_constraintCount;
b3AccelerationConstraint* m_constraints;
u32 m_contactCapacity;
u32 m_contactCount;
b3BodyContact** m_contacts;
u32 m_constraintCapacity;
u32 m_constraintCount;
b3AccelerationConstraint* m_constraints;
b3ClothContactVelocityConstraint* m_velocityConstraints;
b3ClothSolverContactVelocityConstraint* m_velocityConstraints;
b3ClothSolverContactPositionConstraint* m_positionConstraints;
b3ClothSolverData m_solverData;
};

14
include/bounce/dynamics/cloth/particle.h
View File

@ -82,8 +82,9 @@ public:
b3Shape* s2;
// Contact constraint
b3Vec3 p;
b3Vec3 n;
b3Vec3 localNormal1;
b3Vec3 localPoint1;
b3Vec3 localPoint2;
float32 normalImpulse;
// Friction constraint
@ -97,6 +98,15 @@ public:
bool active;
};
struct b3BodyContactWorldPoint
{
void Initialize(const b3BodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB);
b3Vec3 point;
b3Vec3 normal;
float32 separation;
};
// A cloth particle.
class b3Particle
{

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

@ -556,17 +556,20 @@ void b3Cloth::UpdateContacts()
}
b3Shape* shape = bestShape;
b3Body* body = shape->GetBody();
float32 s = bestSeparation;
b3Vec3 n = bestNormal;
b3Vec3 cp = p->m_position - s * n;
b3Vec3 cp2r = p->m_position - s * n;
b3Vec3 cp2 = cp2r - shape->m_radius * n;
// Store the contact manifold
// Here the normal points from shape 2 to the particle
// Store the local contact manifold
// Ensure the the normal points from the particle 1 to shape 2
c->active = true;
c->p1 = p;
c->s2 = shape;
c->p = cp;
c->n = n;
c->localNormal1 = -n;
c->localPoint1.SetZero();
c->localPoint2 = body->GetLocalPoint(cp2);
c->t1 = b3Perp(n);
c->t2 = b3Cross(c->t1, n);
c->normalImpulse = 0.0f;
@ -590,7 +593,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
b3ClothSolverDef solverDef;
solverDef.stack = &m_world->m_stackAllocator;
solverDef.particleCapacity = m_particleList.m_count;
solverDef.forceCapacity = m_forceList.m_count + (1 * m_particleList.m_count);
solverDef.forceCapacity = m_forceList.m_count;
solverDef.contactCapacity = m_particleList.m_count;
b3ClothSolver solver(solverDef);
@ -661,7 +664,19 @@ void b3Cloth::Draw() const
if (c->active)
{
b3Draw_draw->DrawSegment(c->p, c->p + c->n, b3Color_yellow);
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);
}
}

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

@ -48,22 +48,24 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
m_forceCount = 0;
m_forces = (b3Force**)m_allocator->Allocate(m_forceCapacity * sizeof(b3Force*));;
m_contactCapacity = def.contactCapacity;
m_contactCount = 0;
m_contacts = (b3BodyContact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3BodyContact*));
m_constraintCapacity = def.particleCapacity;
m_constraintCount = 0;
m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_constraintCapacity * sizeof(b3AccelerationConstraint));
m_velocityConstraints = (b3ClothContactVelocityConstraint*)m_allocator->Allocate(m_contactCount * sizeof(b3ClothContactVelocityConstraint));
m_contactCapacity = def.contactCapacity;
m_contactCount = 0;
m_contacts = (b3BodyContact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3BodyContact*));
m_velocityConstraints = (b3ClothSolverContactVelocityConstraint*)m_allocator->Allocate(m_contactCapacity * sizeof(b3ClothSolverContactVelocityConstraint));
m_positionConstraints = (b3ClothSolverContactPositionConstraint*)m_allocator->Allocate(m_contactCapacity * sizeof(b3ClothSolverContactPositionConstraint));
}
b3ClothSolver::~b3ClothSolver()
{
m_allocator->Free(m_positionConstraints);
m_allocator->Free(m_velocityConstraints);
m_allocator->Free(m_constraints);
m_allocator->Free(m_contacts);
m_allocator->Free(m_constraints);
m_allocator->Free(m_forces);
m_allocator->Free(m_particles);
}
@ -201,21 +203,8 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
sx0[i] = p->m_x;
}
// Apply internal translations
for (u32 i = 0; i < m_contactCount; ++i)
{
b3BodyContact* c = m_contacts[i];
b3Particle* p = c->p1;
if (p->m_type == e_dynamicParticle)
{
b3Vec3 dx = c->p - p->m_position;
sy[p->m_solverId] += dx;
}
}
// Integrate velocities
// Apply internal forces
ApplyForces();
@ -245,9 +234,16 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
b3_clothSolverIterations = iterations;
sv = sv + x;
sx = sx + sy;
// Initialize velocity constraints
InitializeVelocityConstraints();
// Store delta velocities to improve convergence
for (u32 i = 0; i < m_particleCount; ++i)
{
m_particles[i]->m_x = x[i];
}
// Initialize constraints
InitializeConstraints();
// Warm start velocity constraints
WarmStart();
@ -259,8 +255,24 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
SolveVelocityConstraints();
}
// Store impulses to improve convergence
StoreImpulses();
// Integrate positions
sx = sx + h * sv + sy;
sx = sx + h * sv;
#if 0
// Solve position constraints
const u32 kPositionIterations = 0;
for (u32 i = 0; i < kPositionIterations; ++i)
{
bool contactsSolved = SolvePositionConstraints();
if (contactsSolved)
{
break;
}
}
#endif
// Copy state buffers back to the particles
for (u32 i = 0; i < m_particleCount; ++i)
@ -269,9 +281,18 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
p->m_position = sx[i];
p->m_velocity = sv[i];
}
// Cache x to improve convergence
p->m_x = x[i];
// Synchronize bodies
for (u32 i = 0; i < m_contactCount; ++i)
{
b3Body* body = m_contacts[i]->s2->GetBody();
body->SynchronizeTransform();
body->m_worldInvI = b3RotateToFrame(body->m_invI, body->m_xf.rotation);
body->SynchronizeShapes();
}
}
@ -383,15 +404,18 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
iterations = iteration;
}
void b3ClothSolver::InitializeVelocityConstraints()
void b3ClothSolver::InitializeConstraints()
{
b3DenseVec3& x = *m_solverData.x;
b3DenseVec3& v = *m_solverData.v;
float32 inv_dt = m_solverData.invdt;
for (u32 i = 0; i < m_contactCount; ++i)
{
b3BodyContact* c = m_contacts[i];
b3ClothContactVelocityConstraint* vc = m_velocityConstraints + i;
b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i;
b3ClothSolverContactPositionConstraint* pc = m_positionConstraints + i;
vc->indexA = c->p1->m_solverId;
vc->bodyB = c->s2->GetBody();
@ -403,12 +427,32 @@ void b3ClothSolver::InitializeVelocityConstraints()
vc->invIB = vc->bodyB->GetWorldInverseInertia();
vc->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->GetInverseMass();
pc->invIA.SetZero();
pc->invIB = vc->bodyB->GetWorldInverseInertia();
pc->radiusA = c->p1->m_radius;
pc->radiusB = c->s2->m_radius;
pc->localCenterA.SetZero();
pc->localCenterB = pc->bodyB->m_sweep.localCenter;
pc->localNormalA = c->localNormal1;
pc->localPointA = c->localPoint1;
pc->localPointB = c->localPoint2;
}
for (u32 i = 0; i < m_contactCount; ++i)
{
b3BodyContact* c = m_contacts[i];
b3ClothContactVelocityConstraint* vc = m_velocityConstraints + i;
b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i;
b3ClothSolverContactPositionConstraint* pc = m_positionConstraints + i;
u32 indexA = vc->indexA;
b3Body* bodyB = vc->bodyB;
@ -420,35 +464,55 @@ void b3ClothSolver::InitializeVelocityConstraints()
b3Mat33 iB = vc->invIB;
b3Vec3 xA = x[indexA];
b3Vec3 xB = bodyB->GetPosition();
b3Vec3 xB = bodyB->m_sweep.worldCenter;
// Ensure the normal points from shape 1 to the shape 2
vc->normal = -c->n;
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 = c->p;
vc->point = wp.point;
b3Vec3 point = c->p;
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;
vc->motorImpulse = c->motorImpulse;
{
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;
float32 C = b3Min(0.0f, wp.separation + B3_LINEAR_SLOP);
vc->velocityBias = -inv_dt * B3_BAUMGARTE * C;
}
{
@ -486,8 +550,8 @@ void b3ClothSolver::WarmStart()
for (u32 i = 0; i < m_contactCount; ++i)
{
b3ClothContactVelocityConstraint* vc = m_velocityConstraints + i;
b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i;
u32 indexA = vc->indexA;
b3Body* bodyB = vc->bodyB;
@ -496,7 +560,7 @@ void b3ClothSolver::WarmStart()
b3Vec3 wA; wA.SetZero();
b3Vec3 wB = bodyB->GetAngularVelocity();
float32 mA = vc->invMassA;
float32 mB = vc->invMassB;
@ -534,7 +598,7 @@ void b3ClothSolver::SolveVelocityConstraints()
for (u32 i = 0; i < m_contactCount; ++i)
{
b3ClothContactVelocityConstraint* vc = m_velocityConstraints + i;
b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i;
u32 indexA = vc->indexA;
b3Body* bodyB = vc->bodyB;
@ -551,7 +615,6 @@ void b3ClothSolver::SolveVelocityConstraints()
b3Mat33 iA = vc->invIA;
b3Mat33 iB = vc->invIB;
// Ensure the normal points from shape 1 to the shape 2
b3Vec3 normal = vc->normal;
b3Vec3 point = vc->point;
@ -570,7 +633,7 @@ void b3ClothSolver::SolveVelocityConstraints()
b3Vec3 dv = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA);
float32 Cdot = b3Dot(normal, dv);
float32 impulse = -invK * Cdot;
float32 impulse = invK * (-Cdot + vc->velocityBias);
float32 oldImpulse = vc->normalImpulse;
vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f);
@ -644,10 +707,103 @@ void b3ClothSolver::StoreImpulses()
for (u32 i = 0; i < m_contactCount; ++i)
{
b3BodyContact* c = m_contacts[i];
b3ClothContactVelocityConstraint* vc = m_velocityConstraints + i;
b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i;
c->normalImpulse = vc->normalImpulse;
c->tangentImpulse = vc->tangentImpulse;
c->motorImpulse = vc->motorImpulse;
}
}
struct b3ClothSolverContactSolverPoint
{
void Initialize(const b3ClothSolverContactPositionConstraint* pc, const b3Transform& xfA, const b3Transform& xfB)
{
normal = b3Mul(xfA.rotation, pc->localNormalA);
b3Vec3 c1 = b3Mul(xfA, pc->localPointA);
b3Vec3 c2 = b3Mul(xfB, pc->localPointB);
point = c2;
separation = b3Dot(c2 - c1, normal) - pc->radiusA - pc->radiusB;
}
b3Vec3 normal;
b3Vec3 point;
float32 separation;
};
bool b3ClothSolver::SolvePositionConstraints()
{
b3DenseVec3& x = *m_solverData.v;
float32 minSeparation = 0.0f;
for (u32 i = 0; i < m_contactCount; ++i)
{
b3ClothSolverContactPositionConstraint* pc = m_positionConstraints + 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);
b3ClothSolverContactSolverPoint 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;
}

13
src/bounce/dynamics/cloth/particle.cpp
View File

@ -22,9 +22,18 @@
#include <bounce/dynamics/cloth/dense_vec3.h>
#include <bounce/dynamics/cloth/sparse_sym_mat33.h>
void b3FrictionForce::Apply(const b3ClothSolverData* data)
void b3BodyContactWorldPoint::Initialize(const b3BodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB)
{
// TODO
b3Vec3 nA = xfA.rotation * c->localNormal1;
b3Vec3 cA = xfA * c->localPoint1;
b3Vec3 cB = xfB * c->localPoint2;
b3Vec3 pA = cA + rA * nA;
b3Vec3 pB = cB - rB * nA;
point = 0.5f * (pA + pB);
normal = nA;
separation = b3Dot(cB - cA, nA) - rA - rB;
}
b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)