Split the solvers into force solver and contact solver
This commit is contained in:
Irlan
parent
9765e72ab9
commit
625310be71
@ -30,8 +30,6 @@ class b3Body;
|
||||
struct b3ParticleBodyContact;
|
||||
class b3ParticleTriangleContact;
|
||||
|
||||
struct b3DenseVec3;
|
||||
|
||||
struct b3ClothSolverBodyContactVelocityConstraint
|
||||
{
|
||||
u32 indexA;
|
||||
@ -130,8 +128,8 @@ struct b3ClothContactSolverDef
|
||||
{
|
||||
b3StackAllocator* allocator;
|
||||
|
||||
b3DenseVec3* positions;
|
||||
b3DenseVec3* velocities;
|
||||
b3Vec3* positions;
|
||||
b3Vec3* velocities;
|
||||
|
||||
u32 bodyContactCount;
|
||||
b3ParticleBodyContact** bodyContacts;
|
||||
@ -167,8 +165,8 @@ public:
|
||||
protected:
|
||||
b3StackAllocator* m_allocator;
|
||||
|
||||
b3DenseVec3* m_positions;
|
||||
b3DenseVec3* m_velocities;
|
||||
b3Vec3* m_positions;
|
||||
b3Vec3* m_velocities;
|
||||
|
||||
u32 m_bodyContactCount;
|
||||
b3ParticleBodyContact** m_bodyContacts;
|
||||
|
||||
89
include/bounce/cloth/cloth_force_solver.h
Normal file
89
include/bounce/cloth/cloth_force_solver.h
Normal file
@ -0,0 +1,89 @@
|
||||
/*
|
||||
* 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_FORCE_SOLVER_H
|
||||
#define B3_CLOTH_FORCE_SOLVER_H
|
||||
|
||||
#include <bounce/common/math/mat22.h>
|
||||
#include <bounce/common/math/mat33.h>
|
||||
|
||||
class b3StackAllocator;
|
||||
|
||||
class b3Particle;
|
||||
class b3Force;
|
||||
|
||||
struct b3DenseVec3;
|
||||
struct b3DiagMat33;
|
||||
struct b3SparseSymMat33;
|
||||
struct b3SparseSymMat33View;
|
||||
|
||||
struct b3ClothForceSolverDef
|
||||
{
|
||||
b3StackAllocator* stack;
|
||||
u32 particleCount;
|
||||
b3Particle** particles;
|
||||
u32 forceCount;
|
||||
b3Force** forces;
|
||||
};
|
||||
|
||||
struct b3ClothForceSolverData
|
||||
{
|
||||
b3DenseVec3* x;
|
||||
b3DenseVec3* v;
|
||||
b3DenseVec3* f;
|
||||
b3DenseVec3* y;
|
||||
b3SparseSymMat33* dfdx;
|
||||
b3SparseSymMat33* dfdv;
|
||||
b3DiagMat33* S;
|
||||
b3DenseVec3* z;
|
||||
};
|
||||
|
||||
struct b3AccelerationConstraint
|
||||
{
|
||||
u32 i1;
|
||||
u32 ndof;
|
||||
b3Vec3 p, q, z;
|
||||
|
||||
void Apply(const b3ClothForceSolverData* data);
|
||||
};
|
||||
|
||||
class b3ClothForceSolver
|
||||
{
|
||||
public:
|
||||
b3ClothForceSolver(const b3ClothForceSolverDef& def);
|
||||
~b3ClothForceSolver();
|
||||
|
||||
void Solve(float32 dt, const b3Vec3& gravity);
|
||||
private:
|
||||
void ApplyForces();
|
||||
void ApplyConstraints();
|
||||
|
||||
b3StackAllocator* m_allocator;
|
||||
|
||||
u32 m_particleCount;
|
||||
b3Particle** m_particles;
|
||||
|
||||
u32 m_forceCount;
|
||||
b3Force** m_forces;
|
||||
|
||||
b3AccelerationConstraint* m_constraints;
|
||||
|
||||
b3ClothForceSolverData m_solverData;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -25,14 +25,7 @@
|
||||
class b3StackAllocator;
|
||||
|
||||
class b3Particle;
|
||||
class b3Body;
|
||||
class b3Force;
|
||||
|
||||
struct b3DenseVec3;
|
||||
struct b3DiagMat33;
|
||||
struct b3SparseSymMat33;
|
||||
struct b3SparseSymMat33View;
|
||||
|
||||
struct b3ParticleBodyContact;
|
||||
class b3ParticleTriangleContact;
|
||||
|
||||
@ -45,29 +38,6 @@ struct b3ClothSolverDef
|
||||
u32 triangleContactCapacity;
|
||||
};
|
||||
|
||||
struct b3ClothSolverData
|
||||
{
|
||||
b3DenseVec3* x;
|
||||
b3DenseVec3* v;
|
||||
b3DenseVec3* f;
|
||||
b3DenseVec3* y;
|
||||
b3SparseSymMat33* dfdx;
|
||||
b3SparseSymMat33* dfdv;
|
||||
b3DiagMat33* S;
|
||||
b3DenseVec3* z;
|
||||
float32 dt;
|
||||
float32 invdt;
|
||||
};
|
||||
|
||||
struct b3AccelerationConstraint
|
||||
{
|
||||
u32 i1;
|
||||
u32 ndof;
|
||||
b3Vec3 p, q, z;
|
||||
|
||||
void Apply(const b3ClothSolverData* data);
|
||||
};
|
||||
|
||||
class b3ClothSolver
|
||||
{
|
||||
public:
|
||||
@ -81,9 +51,6 @@ public:
|
||||
|
||||
void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations);
|
||||
private:
|
||||
void ApplyForces();
|
||||
void ApplyConstraints();
|
||||
|
||||
b3StackAllocator* m_allocator;
|
||||
|
||||
u32 m_particleCapacity;
|
||||
@ -93,11 +60,7 @@ private:
|
||||
u32 m_forceCapacity;
|
||||
u32 m_forceCount;
|
||||
b3Force** m_forces;
|
||||
|
||||
u32 m_constraintCapacity;
|
||||
u32 m_constraintCount;
|
||||
b3AccelerationConstraint* m_constraints;
|
||||
|
||||
|
||||
u32 m_bodyContactCapacity;
|
||||
u32 m_bodyContactCount;
|
||||
b3ParticleBodyContact** m_bodyContacts;
|
||||
@ -105,8 +68,6 @@ private:
|
||||
u32 m_triangleContactCapacity;
|
||||
u32 m_triangleContactCount;
|
||||
b3ParticleTriangleContact** m_triangleContacts;
|
||||
|
||||
b3ClothSolverData m_solverData;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -22,7 +22,7 @@
|
||||
#include <bounce/common/math/transform.h>
|
||||
#include <bounce/common/template/list.h>
|
||||
|
||||
struct b3ClothSolverData;
|
||||
struct b3ClothForceSolverData;
|
||||
|
||||
class b3Particle;
|
||||
|
||||
@ -52,7 +52,7 @@ public:
|
||||
protected:
|
||||
friend class b3List2<b3Force>;
|
||||
friend class b3Cloth;
|
||||
friend class b3ClothSolver;
|
||||
friend class b3ClothForceSolver;
|
||||
friend class b3Particle;
|
||||
|
||||
static b3Force* Create(const b3ForceDef* def);
|
||||
@ -61,7 +61,7 @@ protected:
|
||||
b3Force() { }
|
||||
virtual ~b3Force() { }
|
||||
|
||||
virtual void Apply(const b3ClothSolverData* data) = 0;
|
||||
virtual void Apply(const b3ClothForceSolverData* data) = 0;
|
||||
|
||||
// Force type
|
||||
b3ForceType m_type;
|
||||
|
||||
@ -160,6 +160,7 @@ private:
|
||||
friend class b3Cloth;
|
||||
friend class b3ClothTriangle;
|
||||
friend class b3ClothSolver;
|
||||
friend class b3ClothForceSolver;
|
||||
friend class b3ClothContactManager;
|
||||
friend class b3ParticleTriangleContact;
|
||||
friend class b3ClothContactSolver;
|
||||
|
||||
@ -76,7 +76,7 @@ private:
|
||||
b3SpringForce(const b3SpringForceDef* def);
|
||||
~b3SpringForce();
|
||||
|
||||
void Apply(const b3ClothSolverData* data);
|
||||
void Apply(const b3ClothForceSolverData* data);
|
||||
|
||||
// Solver shared
|
||||
|
||||
|
||||
@ -20,7 +20,6 @@
|
||||
#include <bounce/cloth/cloth_contact_manager.h>
|
||||
#include <bounce/cloth/particle.h>
|
||||
#include <bounce/cloth/cloth_triangle.h>
|
||||
#include <bounce/cloth/dense_vec3.h>
|
||||
#include <bounce/common/memory/stack_allocator.h>
|
||||
#include <bounce/dynamics/shapes/shape.h>
|
||||
#include <bounce/dynamics/body.h>
|
||||
@ -54,9 +53,6 @@ b3ClothContactSolver::~b3ClothContactSolver()
|
||||
|
||||
void b3ClothContactSolver::InitializeBodyContactConstraints()
|
||||
{
|
||||
b3DenseVec3& x = *m_positions;
|
||||
b3DenseVec3& v = *m_velocities;
|
||||
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3ParticleBodyContact* c = m_bodyContacts[i];
|
||||
@ -109,7 +105,7 @@ void b3ClothContactSolver::InitializeBodyContactConstraints()
|
||||
b3Mat33 iA = vc->invIA;
|
||||
b3Mat33 iB = vc->invIB;
|
||||
|
||||
b3Vec3 xA = x[indexA];
|
||||
b3Vec3 xA = m_positions[indexA];
|
||||
b3Vec3 xB = bodyB->m_sweep.worldCenter;
|
||||
|
||||
b3Quat qA; qA.SetIdentity();
|
||||
@ -182,8 +178,6 @@ void b3ClothContactSolver::InitializeBodyContactConstraints()
|
||||
}
|
||||
void b3ClothContactSolver::InitializeTriangleContactConstraints()
|
||||
{
|
||||
b3DenseVec3& x = *m_positions;
|
||||
|
||||
for (u32 i = 0; i < m_triangleContactCount; ++i)
|
||||
{
|
||||
b3ParticleTriangleContact* c = m_triangleContacts[i];
|
||||
@ -233,10 +227,10 @@ void b3ClothContactSolver::InitializeTriangleContactConstraints()
|
||||
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 xA = m_positions[indexA];
|
||||
b3Vec3 xB = m_positions[indexB];
|
||||
b3Vec3 xC = m_positions[indexC];
|
||||
b3Vec3 xD = m_positions[indexD];
|
||||
|
||||
float32 wB = pc->wB;
|
||||
float32 wC = pc->wC;
|
||||
@ -269,8 +263,6 @@ void b3ClothContactSolver::InitializeTriangleContactConstraints()
|
||||
|
||||
void b3ClothContactSolver::WarmStartBodyContactConstraints()
|
||||
{
|
||||
b3DenseVec3& v = *m_velocities;
|
||||
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
|
||||
@ -278,7 +270,7 @@ void b3ClothContactSolver::WarmStartBodyContactConstraints()
|
||||
u32 indexA = vc->indexA;
|
||||
b3Body* bodyB = vc->bodyB;
|
||||
|
||||
b3Vec3 vA = v[indexA];
|
||||
b3Vec3 vA = m_velocities[indexA];
|
||||
b3Vec3 vB = bodyB->GetLinearVelocity();
|
||||
|
||||
b3Vec3 wA; wA.SetZero();
|
||||
@ -307,7 +299,7 @@ void b3ClothContactSolver::WarmStartBodyContactConstraints()
|
||||
vB += mB * (P1 + P2);
|
||||
wB += iB * b3Cross(vc->rB, P1 + P2);
|
||||
|
||||
v[indexA] = vA;
|
||||
m_velocities[indexA] = vA;
|
||||
|
||||
bodyB->SetLinearVelocity(vB);
|
||||
bodyB->SetAngularVelocity(wB);
|
||||
@ -316,8 +308,6 @@ void b3ClothContactSolver::WarmStartBodyContactConstraints()
|
||||
|
||||
void b3ClothContactSolver::WarmStartTriangleContactConstraints()
|
||||
{
|
||||
b3DenseVec3& v = *m_velocities;
|
||||
|
||||
for (u32 i = 0; i < m_triangleContactCount; ++i)
|
||||
{
|
||||
b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i;
|
||||
@ -327,10 +317,10 @@ void b3ClothContactSolver::WarmStartTriangleContactConstraints()
|
||||
u32 indexC = vc->indexC;
|
||||
u32 indexD = vc->indexD;
|
||||
|
||||
b3Vec3 vA = v[indexA];
|
||||
b3Vec3 vB = v[indexB];
|
||||
b3Vec3 vC = v[indexC];
|
||||
b3Vec3 vD = v[indexD];
|
||||
b3Vec3 vA = m_velocities[indexA];
|
||||
b3Vec3 vB = m_velocities[indexB];
|
||||
b3Vec3 vC = m_velocities[indexC];
|
||||
b3Vec3 vD = m_velocities[indexD];
|
||||
|
||||
float32 mA = vc->invMassA;
|
||||
float32 mB = vc->invMassB;
|
||||
@ -388,17 +378,15 @@ void b3ClothContactSolver::WarmStartTriangleContactConstraints()
|
||||
vD += mD * PD;
|
||||
}
|
||||
|
||||
v[indexA] = vA;
|
||||
v[indexB] = vB;
|
||||
v[indexC] = vC;
|
||||
v[indexD] = vD;
|
||||
m_velocities[indexA] = vA;
|
||||
m_velocities[indexB] = vB;
|
||||
m_velocities[indexC] = vC;
|
||||
m_velocities[indexD] = vD;
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothContactSolver::SolveBodyContactVelocityConstraints()
|
||||
{
|
||||
b3DenseVec3& v = *m_velocities;
|
||||
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
|
||||
@ -406,7 +394,7 @@ void b3ClothContactSolver::SolveBodyContactVelocityConstraints()
|
||||
u32 indexA = vc->indexA;
|
||||
b3Body* bodyB = vc->bodyB;
|
||||
|
||||
b3Vec3 vA = v[indexA];
|
||||
b3Vec3 vA = m_velocities[indexA];
|
||||
b3Vec3 vB = bodyB->GetLinearVelocity();
|
||||
|
||||
b3Vec3 wA; wA.SetZero();
|
||||
@ -476,7 +464,7 @@ void b3ClothContactSolver::SolveBodyContactVelocityConstraints()
|
||||
wB += iB * b3Cross(rB, P);
|
||||
}
|
||||
|
||||
v[indexA] = vA;
|
||||
m_velocities[indexA] = vA;
|
||||
|
||||
bodyB->SetLinearVelocity(vB);
|
||||
bodyB->SetAngularVelocity(wB);
|
||||
@ -485,8 +473,6 @@ void b3ClothContactSolver::SolveBodyContactVelocityConstraints()
|
||||
|
||||
void b3ClothContactSolver::SolveTriangleContactVelocityConstraints()
|
||||
{
|
||||
b3DenseVec3& v = *m_velocities;
|
||||
|
||||
for (u32 i = 0; i < m_triangleContactCount; ++i)
|
||||
{
|
||||
b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i;
|
||||
@ -507,10 +493,10 @@ void b3ClothContactSolver::SolveTriangleContactVelocityConstraints()
|
||||
float32 wC = vc->wC;
|
||||
float32 wD = vc->wD;
|
||||
|
||||
b3Vec3 vA = v[indexA];
|
||||
b3Vec3 vB = v[indexB];
|
||||
b3Vec3 vC = v[indexC];
|
||||
b3Vec3 vD = v[indexD];
|
||||
b3Vec3 vA = m_velocities[indexA];
|
||||
b3Vec3 vB = m_velocities[indexB];
|
||||
b3Vec3 vC = m_velocities[indexC];
|
||||
b3Vec3 vD = m_velocities[indexD];
|
||||
|
||||
// Solve normal constraint.
|
||||
{
|
||||
@ -602,10 +588,10 @@ void b3ClothContactSolver::SolveTriangleContactVelocityConstraints()
|
||||
vD += mD * PD;
|
||||
}
|
||||
|
||||
v[indexA] = vA;
|
||||
v[indexB] = vB;
|
||||
v[indexC] = vC;
|
||||
v[indexD] = vD;
|
||||
m_velocities[indexA] = vA;
|
||||
m_velocities[indexB] = vB;
|
||||
m_velocities[indexC] = vC;
|
||||
m_velocities[indexD] = vD;
|
||||
}
|
||||
}
|
||||
|
||||
@ -658,8 +644,6 @@ struct b3ClothSolverBodyContactSolverPoint
|
||||
|
||||
bool b3ClothContactSolver::SolveBodyContactPositionConstraints()
|
||||
{
|
||||
b3DenseVec3& x = *m_positions;
|
||||
|
||||
float32 minSeparation = 0.0f;
|
||||
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
@ -676,7 +660,7 @@ bool b3ClothContactSolver::SolveBodyContactPositionConstraints()
|
||||
b3Mat33 iB = pc->invIB;
|
||||
b3Vec3 localCenterB = pc->localCenterB;
|
||||
|
||||
b3Vec3 cA = x[indexA];
|
||||
b3Vec3 cA = m_positions[indexA];
|
||||
b3Quat qA; qA.SetIdentity();
|
||||
|
||||
b3Vec3 cB = bodyB->m_sweep.worldCenter;
|
||||
@ -724,7 +708,7 @@ bool b3ClothContactSolver::SolveBodyContactPositionConstraints()
|
||||
qB += b3Derivative(qB, iB * b3Cross(rB, P));
|
||||
qB.Normalize();
|
||||
|
||||
x[indexA] = cA;
|
||||
m_positions[indexA] = cA;
|
||||
|
||||
bodyB->m_sweep.worldCenter = cB;
|
||||
bodyB->m_sweep.orientation = qB;
|
||||
@ -735,8 +719,6 @@ bool b3ClothContactSolver::SolveBodyContactPositionConstraints()
|
||||
|
||||
bool b3ClothContactSolver::SolveTriangleContactPositionConstraints()
|
||||
{
|
||||
b3DenseVec3& x = *m_positions;
|
||||
|
||||
float32 minSeparation = 0.0f;
|
||||
|
||||
for (u32 i = 0; i < m_triangleContactCount; ++i)
|
||||
@ -762,10 +744,10 @@ bool b3ClothContactSolver::SolveTriangleContactPositionConstraints()
|
||||
float32 wC = pc->wC;
|
||||
float32 wD = pc->wD;
|
||||
|
||||
b3Vec3 xA = x[indexA];
|
||||
b3Vec3 xB = x[indexB];
|
||||
b3Vec3 xC = x[indexC];
|
||||
b3Vec3 xD = x[indexD];
|
||||
b3Vec3 xA = m_positions[indexA];
|
||||
b3Vec3 xB = m_positions[indexB];
|
||||
b3Vec3 xC = m_positions[indexC];
|
||||
b3Vec3 xD = m_positions[indexD];
|
||||
|
||||
b3Vec3 cB = wB * xB + wC * xC + wD * xD;
|
||||
|
||||
@ -802,10 +784,10 @@ bool b3ClothContactSolver::SolveTriangleContactPositionConstraints()
|
||||
xC += mC * PC;
|
||||
xD += mD * PD;
|
||||
|
||||
x[indexA] = xA;
|
||||
x[indexB] = xB;
|
||||
x[indexC] = xC;
|
||||
x[indexD] = xD;
|
||||
m_positions[indexA] = xA;
|
||||
m_positions[indexB] = xB;
|
||||
m_positions[indexC] = xC;
|
||||
m_positions[indexD] = xD;
|
||||
}
|
||||
|
||||
return minSeparation >= -3.0f * B3_LINEAR_SLOP;
|
||||
|
||||
293
src/bounce/cloth/cloth_force_solver.cpp
Normal file
293
src/bounce/cloth/cloth_force_solver.cpp
Normal file
@ -0,0 +1,293 @@
|
||||
/*
|
||||
* 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_force_solver.h>
|
||||
#include <bounce/cloth/particle.h>
|
||||
#include <bounce/cloth/force.h>
|
||||
#include <bounce/cloth/dense_vec3.h>
|
||||
#include <bounce/cloth/diag_mat33.h>
|
||||
#include <bounce/cloth/sparse_sym_mat33.h>
|
||||
#include <bounce/cloth/sparse_sym_mat33_view.h>
|
||||
#include <bounce/common/memory/stack_allocator.h>
|
||||
|
||||
// Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm.
|
||||
// described in the paper:
|
||||
// "Large Steps in Cloth Simulation - David Baraff, Andrew Witkin".
|
||||
|
||||
// Some improvements for the original MPCG algorithm are described in the paper:
|
||||
// "On the modified conjugate gradient method in cloth simulation - Uri M. Ascher, Eddy Boxerman".
|
||||
|
||||
u32 b3_clothSolverIterations = 0;
|
||||
|
||||
b3ClothForceSolver::b3ClothForceSolver(const b3ClothForceSolverDef& def)
|
||||
{
|
||||
m_allocator = def.stack;
|
||||
|
||||
m_particleCount = def.particleCount;
|
||||
m_particles = def.particles;
|
||||
|
||||
m_forceCount = def.forceCount;
|
||||
m_forces = def.forces;
|
||||
|
||||
m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_particleCount * sizeof(b3AccelerationConstraint));
|
||||
}
|
||||
|
||||
b3ClothForceSolver::~b3ClothForceSolver()
|
||||
{
|
||||
m_allocator->Free(m_constraints);
|
||||
}
|
||||
|
||||
void b3ClothForceSolver::ApplyForces()
|
||||
{
|
||||
for (u32 i = 0; i < m_forceCount; ++i)
|
||||
{
|
||||
m_forces[i]->Apply(&m_solverData);
|
||||
}
|
||||
}
|
||||
|
||||
void b3AccelerationConstraint::Apply(const b3ClothForceSolverData* data)
|
||||
{
|
||||
b3DiagMat33& sS = *data->S;
|
||||
b3DenseVec3& sz = *data->z;
|
||||
|
||||
sz[i1] = z;
|
||||
|
||||
b3Mat33 I; I.SetIdentity();
|
||||
|
||||
switch (ndof)
|
||||
{
|
||||
case 3:
|
||||
{
|
||||
sS[i1] = I;
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
{
|
||||
sS[i1] = I - b3Outer(p, p);
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
sS[i1] = I - b3Outer(p, p) - b3Outer(q, q);
|
||||
break;
|
||||
}
|
||||
case 0:
|
||||
{
|
||||
sS[i1].SetZero();
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
B3_ASSERT(false);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothForceSolver::ApplyConstraints()
|
||||
{
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_particles[i];
|
||||
b3AccelerationConstraint* c = m_constraints + i;
|
||||
|
||||
c->i1 = i;
|
||||
|
||||
if (p->m_type != e_dynamicParticle)
|
||||
{
|
||||
c->ndof = 0;
|
||||
c->z.SetZero();
|
||||
}
|
||||
else
|
||||
{
|
||||
c->ndof = 3;
|
||||
c->z.SetZero();
|
||||
}
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
m_constraints[i].Apply(&m_solverData);
|
||||
}
|
||||
}
|
||||
|
||||
// Solve Ax = b
|
||||
static void b3SolveMPCG(b3DenseVec3& x,
|
||||
const b3SparseSymMat33View& A, const b3DenseVec3& b,
|
||||
const b3DiagMat33& S, const b3DenseVec3& z,
|
||||
const b3DenseVec3& y, const b3DiagMat33& I, u32 maxIterations = 20)
|
||||
{
|
||||
B3_PROFILE("Cloth Solve MPCG");
|
||||
|
||||
// Jacobi preconditioner
|
||||
// P = diag(A)
|
||||
b3DiagMat33 inv_P(A.rowCount);
|
||||
for (u32 i = 0; i < A.rowCount; ++i)
|
||||
{
|
||||
b3Mat33 a = A(i, i);
|
||||
|
||||
// Sylvester Criterion to ensure PD-ness
|
||||
B3_ASSERT(b3Det(a.x, a.y, a.z) > 0.0f);
|
||||
|
||||
B3_ASSERT(a.x.x > 0.0f);
|
||||
float32 xx = 1.0f / a.x.x;
|
||||
|
||||
B3_ASSERT(a.y.y > 0.0f);
|
||||
float32 yy = 1.0f / a.y.y;
|
||||
|
||||
B3_ASSERT(a.z.z > 0.0f);
|
||||
float32 zz = 1.0f / a.z.z;
|
||||
|
||||
inv_P[i] = b3Diagonal(xx, yy, zz);
|
||||
}
|
||||
|
||||
x = (S * y) + (I - S) * z;
|
||||
|
||||
b3DenseVec3 b_hat = S * (b - A * ((I - S) * z));
|
||||
|
||||
float32 b_delta = b3Dot(b_hat, inv_P * b_hat);
|
||||
|
||||
b3DenseVec3 r = S * (b - A * x);
|
||||
|
||||
b3DenseVec3 p = S * (inv_P * r);
|
||||
|
||||
float32 delta_new = b3Dot(r, p);
|
||||
|
||||
u32 iteration = 0;
|
||||
for (;;)
|
||||
{
|
||||
if (iteration == maxIterations)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
if (delta_new <= B3_EPSILON * B3_EPSILON * b_delta)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
b3DenseVec3 s = S * (A * p);
|
||||
|
||||
float32 alpha = delta_new / b3Dot(p, s);
|
||||
|
||||
x = x + alpha * p;
|
||||
r = r - alpha * s;
|
||||
|
||||
b3DenseVec3 h = inv_P * r;
|
||||
|
||||
float32 delta_old = delta_new;
|
||||
|
||||
delta_new = b3Dot(r, h);
|
||||
|
||||
float32 beta = delta_new / delta_old;
|
||||
|
||||
p = S * (h + beta * p);
|
||||
|
||||
++iteration;
|
||||
}
|
||||
|
||||
b3_clothSolverIterations = iteration;
|
||||
}
|
||||
|
||||
void b3ClothForceSolver::Solve(float32 dt, const b3Vec3& gravity)
|
||||
{
|
||||
float32 h = dt;
|
||||
|
||||
b3DenseVec3 sx(m_particleCount);
|
||||
b3DenseVec3 sv(m_particleCount);
|
||||
b3DenseVec3 sf(m_particleCount);
|
||||
b3DenseVec3 sy(m_particleCount);
|
||||
b3DenseVec3 sz(m_particleCount);
|
||||
b3DenseVec3 sx0(m_particleCount);
|
||||
b3SparseSymMat33 dfdx(m_particleCount);
|
||||
b3SparseSymMat33 dfdv(m_particleCount);
|
||||
b3DiagMat33 S(m_particleCount);
|
||||
|
||||
m_solverData.x = &sx;
|
||||
m_solverData.v = &sv;
|
||||
m_solverData.f = &sf;
|
||||
m_solverData.y = &sy;
|
||||
m_solverData.z = &sz;
|
||||
m_solverData.dfdx = &dfdx;
|
||||
m_solverData.dfdv = &dfdv;
|
||||
m_solverData.S = &S;
|
||||
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_particles[i];
|
||||
|
||||
sx[i] = p->m_position;
|
||||
sv[i] = p->m_velocity;
|
||||
sf[i] = p->m_force;
|
||||
|
||||
if (p->m_type == e_dynamicParticle)
|
||||
{
|
||||
// Apply weight
|
||||
sf[i] += p->m_mass * gravity;
|
||||
}
|
||||
|
||||
sy[i] = p->m_translation;
|
||||
sx0[i] = p->m_x;
|
||||
}
|
||||
|
||||
// Apply internal forces
|
||||
ApplyForces();
|
||||
|
||||
// Apply constraints
|
||||
ApplyConstraints();
|
||||
|
||||
// Solve Ax = b, where
|
||||
// A = M - h * dfdv - h * h * dfdx
|
||||
// b = h * (f0 + h * dfdx * v0 + dfdx * y)
|
||||
b3SparseSymMat33 M(m_particleCount);
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
M(i, i) = b3Diagonal(m_particles[i]->m_mass);
|
||||
}
|
||||
|
||||
// A
|
||||
b3SparseSymMat33 A = M - h * dfdv - h * h * dfdx;
|
||||
|
||||
// View for A
|
||||
b3SparseSymMat33View viewA(A);
|
||||
|
||||
// b
|
||||
b3DenseVec3 b = h * (sf + h * (dfdx * sv) + dfdx * sy);
|
||||
|
||||
// I
|
||||
b3DiagMat33 I(m_particleCount);
|
||||
I.SetIdentity();
|
||||
|
||||
// x
|
||||
b3DenseVec3 x(m_particleCount);
|
||||
b3SolveMPCG(x, viewA, b, S, sz, sx0, I);
|
||||
|
||||
// Velocity update
|
||||
sv = sv + x;
|
||||
sx = sx + sy;
|
||||
|
||||
// Copy state buffers back to the particle
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_particles[i];
|
||||
|
||||
p->m_x = x[i];
|
||||
p->m_position = sx[i];
|
||||
p->m_velocity = sv[i];
|
||||
}
|
||||
}
|
||||
@ -17,27 +17,13 @@
|
||||
*/
|
||||
|
||||
#include <bounce/cloth/cloth_solver.h>
|
||||
#include <bounce/cloth/cloth_force_solver.h>
|
||||
#include <bounce/cloth/cloth_contact_solver.h>
|
||||
#include <bounce/cloth/cloth.h>
|
||||
#include <bounce/cloth/particle.h>
|
||||
#include <bounce/cloth/force.h>
|
||||
#include <bounce/cloth/dense_vec3.h>
|
||||
#include <bounce/cloth/diag_mat33.h>
|
||||
#include <bounce/cloth/sparse_sym_mat33.h>
|
||||
#include <bounce/cloth/sparse_sym_mat33_view.h>
|
||||
#include <bounce/common/memory/stack_allocator.h>
|
||||
#include <bounce/common/math/mat.h>
|
||||
#include <bounce/dynamics/shapes/shape.h>
|
||||
#include <bounce/dynamics/body.h>
|
||||
|
||||
// Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm.
|
||||
// described in the paper:
|
||||
// "Large Steps in Cloth Simulation - David Baraff, Andrew Witkin".
|
||||
|
||||
// Some improvements for the original MPCG algorithm are described in the paper:
|
||||
// "On the modified conjugate gradient method in cloth simulation - Uri M. Ascher, Eddy Boxerman".
|
||||
|
||||
u32 b3_clothSolverIterations = 0;
|
||||
#include <bounce/common/memory/stack_allocator.h>
|
||||
|
||||
b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
|
||||
{
|
||||
@ -51,10 +37,6 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
|
||||
m_forceCount = 0;
|
||||
m_forces = (b3Force**)m_allocator->Allocate(m_forceCapacity * sizeof(b3Force*));;
|
||||
|
||||
m_constraintCapacity = def.particleCapacity;
|
||||
m_constraintCount = 0;
|
||||
m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_constraintCapacity * sizeof(b3AccelerationConstraint));
|
||||
|
||||
m_bodyContactCapacity = def.bodyContactCapacity;
|
||||
m_bodyContactCount = 0;
|
||||
m_bodyContacts = (b3ParticleBodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3ParticleBodyContact*));;
|
||||
@ -68,8 +50,6 @@ b3ClothSolver::~b3ClothSolver()
|
||||
{
|
||||
m_allocator->Free(m_triangleContacts);
|
||||
m_allocator->Free(m_bodyContacts);
|
||||
|
||||
m_allocator->Free(m_constraints);
|
||||
m_allocator->Free(m_forces);
|
||||
m_allocator->Free(m_particles);
|
||||
}
|
||||
@ -95,326 +75,106 @@ void b3ClothSolver::Add(b3ParticleTriangleContact* c)
|
||||
m_triangleContacts[m_triangleContactCount++] = c;
|
||||
}
|
||||
|
||||
void b3ClothSolver::ApplyForces()
|
||||
{
|
||||
for (u32 i = 0; i < m_forceCount; ++i)
|
||||
{
|
||||
m_forces[i]->Apply(&m_solverData);
|
||||
}
|
||||
}
|
||||
|
||||
void b3AccelerationConstraint::Apply(const b3ClothSolverData* data)
|
||||
{
|
||||
b3DiagMat33& sS = *data->S;
|
||||
b3DenseVec3& sz = *data->z;
|
||||
|
||||
sz[i1] = z;
|
||||
|
||||
b3Mat33 I; I.SetIdentity();
|
||||
|
||||
switch (ndof)
|
||||
{
|
||||
case 3:
|
||||
{
|
||||
sS[i1] = I;
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
{
|
||||
sS[i1] = I - b3Outer(p, p);
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
sS[i1] = I - b3Outer(p, p) - b3Outer(q, q);
|
||||
break;
|
||||
}
|
||||
case 0:
|
||||
{
|
||||
sS[i1].SetZero();
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
B3_ASSERT(false);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothSolver::ApplyConstraints()
|
||||
{
|
||||
b3DiagMat33& S = *m_solverData.S;
|
||||
b3DenseVec3& z = *m_solverData.z;
|
||||
b3DenseVec3& x = *m_solverData.x;
|
||||
|
||||
S.SetIdentity();
|
||||
z.SetZero();
|
||||
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_particles[i];
|
||||
|
||||
if (p->m_type != e_dynamicParticle)
|
||||
{
|
||||
b3AccelerationConstraint* ac = m_constraints + m_constraintCount;
|
||||
++m_constraintCount;
|
||||
ac->i1 = i;
|
||||
ac->ndof = 0;
|
||||
ac->z.SetZero();
|
||||
}
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_constraintCount; ++i)
|
||||
{
|
||||
m_constraints[i].Apply(&m_solverData);
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
static void b3SolveMPCG(b3DenseVec3& x,
|
||||
const b3SparseSymMat33View& A, const b3DenseVec3& b,
|
||||
const b3DiagMat33& S, const b3DenseVec3& z,
|
||||
const b3DenseVec3& y, const b3DiagMat33& I, u32 maxIterations = 20)
|
||||
{
|
||||
B3_PROFILE("Cloth Solve MPCG");
|
||||
|
||||
// Jacobi preconditioner
|
||||
// P = diag(A)
|
||||
b3DiagMat33 inv_P(A.rowCount);
|
||||
for (u32 i = 0; i < A.rowCount; ++i)
|
||||
{
|
||||
b3Mat33 a = A(i, i);
|
||||
|
||||
// Sylvester Criterion to ensure PD-ness
|
||||
B3_ASSERT(b3Det(a.x, a.y, a.z) > 0.0f);
|
||||
|
||||
B3_ASSERT(a.x.x > 0.0f);
|
||||
float32 xx = 1.0f / a.x.x;
|
||||
|
||||
B3_ASSERT(a.y.y > 0.0f);
|
||||
float32 yy = 1.0f / a.y.y;
|
||||
|
||||
B3_ASSERT(a.z.z > 0.0f);
|
||||
float32 zz = 1.0f / a.z.z;
|
||||
|
||||
inv_P[i] = b3Diagonal(xx, yy, zz);
|
||||
}
|
||||
|
||||
x = (S * y) + (I - S) * z;
|
||||
|
||||
b3DenseVec3 b_hat = S * (b - A * ((I - S) * z));
|
||||
|
||||
float32 b_delta = b3Dot(b_hat, inv_P * b_hat);
|
||||
|
||||
b3DenseVec3 r = S * (b - A * x);
|
||||
|
||||
b3DenseVec3 p = S * (inv_P * r);
|
||||
|
||||
float32 delta_new = b3Dot(r, p);
|
||||
|
||||
u32 iteration = 0;
|
||||
for (;;)
|
||||
{
|
||||
if (iteration == maxIterations)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
if (delta_new <= B3_EPSILON * B3_EPSILON * b_delta)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
b3DenseVec3 s = S * (A * p);
|
||||
|
||||
float32 alpha = delta_new / b3Dot(p, s);
|
||||
|
||||
x = x + alpha * p;
|
||||
r = r - alpha * s;
|
||||
|
||||
b3DenseVec3 h = inv_P * r;
|
||||
|
||||
float32 delta_old = delta_new;
|
||||
|
||||
delta_new = b3Dot(r, h);
|
||||
|
||||
float32 beta = delta_new / delta_old;
|
||||
|
||||
p = S * (h + beta * p);
|
||||
|
||||
++iteration;
|
||||
}
|
||||
|
||||
b3_clothSolverIterations = iteration;
|
||||
}
|
||||
|
||||
void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations)
|
||||
{
|
||||
float32 h = dt;
|
||||
float32 inv_h = 1.0f / h;
|
||||
{
|
||||
// Solve internal dynamics
|
||||
b3ClothForceSolverDef forceSolverDef;
|
||||
forceSolverDef.stack = m_allocator;
|
||||
forceSolverDef.particleCount = m_particleCount;
|
||||
forceSolverDef.particles = m_particles;
|
||||
forceSolverDef.forceCount = m_forceCount;
|
||||
forceSolverDef.forces = m_forces;
|
||||
|
||||
b3DenseVec3 sx(m_particleCount);
|
||||
b3DenseVec3 sv(m_particleCount);
|
||||
b3DenseVec3 sf(m_particleCount);
|
||||
b3ClothForceSolver forceSolver(forceSolverDef);
|
||||
|
||||
m_solverData.dt = h;
|
||||
m_solverData.invdt = inv_h;
|
||||
m_solverData.x = &sx;
|
||||
m_solverData.v = &sv;
|
||||
m_solverData.f = &sf;
|
||||
forceSolver.Solve(dt, gravity);
|
||||
}
|
||||
|
||||
// Copy particle state to state buffer
|
||||
b3Vec3* positions = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3));
|
||||
b3Vec3* velocities = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3));
|
||||
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_particles[i];
|
||||
|
||||
sx[i] = p->m_position;
|
||||
sv[i] = p->m_velocity;
|
||||
sf[i] = p->m_force;
|
||||
|
||||
// Apply weight
|
||||
if (p->m_type == e_dynamicParticle)
|
||||
{
|
||||
sf[i] += p->m_mass * gravity;
|
||||
}
|
||||
positions[i] = m_particles[i]->m_position;
|
||||
velocities[i] = m_particles[i]->m_velocity;
|
||||
}
|
||||
|
||||
// Integrate velocities
|
||||
{
|
||||
b3SparseSymMat33 dfdx(m_particleCount);
|
||||
b3SparseSymMat33 dfdv(m_particleCount);
|
||||
b3DiagMat33 S(m_particleCount);
|
||||
b3DenseVec3 z(m_particleCount);
|
||||
b3DenseVec3 sy(m_particleCount);
|
||||
b3DenseVec3 sx0(m_particleCount);
|
||||
// Solve constraints
|
||||
b3ClothContactSolverDef contactSolverDef;
|
||||
contactSolverDef.allocator = m_allocator;
|
||||
contactSolverDef.positions = positions;
|
||||
contactSolverDef.velocities = velocities;
|
||||
contactSolverDef.bodyContactCount = m_bodyContactCount;
|
||||
contactSolverDef.bodyContacts = m_bodyContacts;
|
||||
contactSolverDef.triangleContactCount = m_triangleContactCount;
|
||||
contactSolverDef.triangleContacts = m_triangleContacts;
|
||||
|
||||
m_solverData.y = &sy;
|
||||
m_solverData.dfdx = &dfdx;
|
||||
m_solverData.dfdv = &dfdv;
|
||||
m_solverData.S = &S;
|
||||
m_solverData.z = &z;
|
||||
b3ClothContactSolver contactSolver(contactSolverDef);
|
||||
|
||||
// Apply position correction
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_particles[i];
|
||||
|
||||
sy[i] = p->m_translation;
|
||||
sx0[i] = p->m_x;
|
||||
// Initialize constraints
|
||||
contactSolver.InitializeBodyContactConstraints();
|
||||
contactSolver.InitializeTriangleContactConstraints();
|
||||
}
|
||||
|
||||
// Apply internal forces
|
||||
ApplyForces();
|
||||
|
||||
// Apply constraints
|
||||
ApplyConstraints();
|
||||
|
||||
// Solve Ax = b, where
|
||||
// A = M - h * dfdv - h * h * dfdx
|
||||
// b = h * (f0 + h * dfdx * v0 + dfdx * y)
|
||||
|
||||
b3SparseSymMat33 M(m_particleCount);
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
M(i, i) = b3Diagonal(m_particles[i]->m_mass);
|
||||
// Warm start velocity constraints
|
||||
contactSolver.WarmStartBodyContactConstraints();
|
||||
contactSolver.WarmStartTriangleContactConstraints();
|
||||
}
|
||||
|
||||
// A
|
||||
b3SparseSymMat33 A = M - h * dfdv - h * h * dfdx;
|
||||
|
||||
// View for A
|
||||
b3SparseSymMat33View viewA(A);
|
||||
|
||||
// b
|
||||
b3DenseVec3 b = h * (sf + h * (dfdx * sv) + dfdx * sy);
|
||||
|
||||
// I
|
||||
b3DiagMat33 I(m_particleCount);
|
||||
I.SetIdentity();
|
||||
|
||||
// x
|
||||
b3DenseVec3 x(m_particleCount);
|
||||
b3SolveMPCG(x, viewA, b, S, z, sx0, I);
|
||||
|
||||
// Velocity update
|
||||
sv = sv + x;
|
||||
sx = sx + sy;
|
||||
|
||||
// Copy state buffers back to the particle
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_particles[i];
|
||||
|
||||
p->m_x = x[i];
|
||||
p->m_position = sx[i];
|
||||
p->m_velocity = sv[i];
|
||||
}
|
||||
}
|
||||
|
||||
// Solve constraints
|
||||
b3ClothContactSolverDef contactSolverDef;
|
||||
contactSolverDef.allocator = m_allocator;
|
||||
contactSolverDef.positions = m_solverData.x;
|
||||
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.WarmStartBodyContactConstraints();
|
||||
contactSolver.WarmStartTriangleContactConstraints();
|
||||
}
|
||||
|
||||
{
|
||||
for (u32 i = 0; i < velocityIterations; ++i)
|
||||
{
|
||||
contactSolver.SolveBodyContactVelocityConstraints();
|
||||
contactSolver.SolveTriangleContactVelocityConstraints();
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
contactSolver.StoreImpulses();
|
||||
}
|
||||
|
||||
// Integrate positions
|
||||
sx = sx + h * sv;
|
||||
|
||||
// Solve position constraints
|
||||
{
|
||||
bool positionSolved = false;
|
||||
for (u32 i = 0; i < positionIterations; ++i)
|
||||
{
|
||||
bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints();
|
||||
bool triangleContactsSolved = contactSolver.SolveTriangleContactPositionConstraints();
|
||||
|
||||
if (bodyContactsSolved && triangleContactsSolved)
|
||||
// Solve velocity constraints
|
||||
for (u32 i = 0; i < velocityIterations; ++i)
|
||||
{
|
||||
// Early out if the position errors are small.
|
||||
positionSolved = true;
|
||||
break;
|
||||
contactSolver.SolveBodyContactVelocityConstraints();
|
||||
contactSolver.SolveTriangleContactVelocityConstraints();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Synchronize bodies
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3Body* body = m_bodyContacts[i]->s2->GetBody();
|
||||
{
|
||||
// Cache impulses for warm-starting
|
||||
contactSolver.StoreImpulses();
|
||||
}
|
||||
|
||||
body->SynchronizeTransform();
|
||||
// Integrate positions
|
||||
float32 h = dt;
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
positions[i] += h * velocities[i];
|
||||
}
|
||||
|
||||
body->m_worldInvI = b3RotateToFrame(body->m_invI, body->m_xf.rotation);
|
||||
// Solve position constraints
|
||||
{
|
||||
bool positionSolved = false;
|
||||
for (u32 i = 0; i < positionIterations; ++i)
|
||||
{
|
||||
bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints();
|
||||
bool triangleContactsSolved = contactSolver.SolveTriangleContactPositionConstraints();
|
||||
|
||||
body->SynchronizeShapes();
|
||||
if (bodyContactsSolved && triangleContactsSolved)
|
||||
{
|
||||
// Early out if the position errors are small.
|
||||
positionSolved = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Synchronize bodies
|
||||
for (u32 i = 0; i < m_bodyContactCount; ++i)
|
||||
{
|
||||
b3Body* body = m_bodyContacts[i]->s2->GetBody();
|
||||
|
||||
body->SynchronizeTransform();
|
||||
|
||||
body->m_worldInvI = b3RotateToFrame(body->m_invI, body->m_xf.rotation);
|
||||
|
||||
body->SynchronizeShapes();
|
||||
}
|
||||
}
|
||||
|
||||
// Copy state buffers back to the particles
|
||||
@ -422,7 +182,10 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterati
|
||||
{
|
||||
b3Particle* p = m_particles[i];
|
||||
|
||||
p->m_position = sx[i];
|
||||
p->m_velocity = sv[i];
|
||||
p->m_position = positions[i];
|
||||
p->m_velocity = velocities[i];
|
||||
}
|
||||
}
|
||||
|
||||
m_allocator->Free(velocities);
|
||||
m_allocator->Free(positions);
|
||||
}
|
||||
@ -18,7 +18,7 @@
|
||||
|
||||
#include <bounce/cloth/spring_force.h>
|
||||
#include <bounce/cloth/particle.h>
|
||||
#include <bounce/cloth/cloth_solver.h>
|
||||
#include <bounce/cloth/cloth_force_solver.h>
|
||||
#include <bounce/cloth/dense_vec3.h>
|
||||
#include <bounce/cloth/sparse_sym_mat33.h>
|
||||
|
||||
@ -50,7 +50,7 @@ b3SpringForce::~b3SpringForce()
|
||||
|
||||
}
|
||||
|
||||
void b3SpringForce::Apply(const b3ClothSolverData* data)
|
||||
void b3SpringForce::Apply(const b3ClothForceSolverData* data)
|
||||
{
|
||||
b3DenseVec3& x = *data->x;
|
||||
b3DenseVec3& v = *data->v;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user