150 lines
6.4 KiB
C++
150 lines
6.4 KiB
C++
|
#ifndef B3_PGS_JACOBI_SOLVER
|
||
|
|
#define B3_PGS_JACOBI_SOLVER
|
||
|
|
|
||
|
|
|
||
|
|
struct b3Contact4;
|
||
|
|
struct b3ContactPoint;
|
||
|
|
|
||
|
|
|
||
|
|
class b3Dispatcher;
|
||
|
|
|
||
|
|
#include "b3TypedConstraint.h"
|
||
|
|
#include "b3ContactSolverInfo.h"
|
||
|
|
#include "b3SolverBody.h"
|
||
|
|
#include "b3SolverConstraint.h"
|
||
|
|
|
||
|
|
struct b3RigidBodyData;
|
||
|
|
struct b3InertiaData;
|
||
|
|
|
||
|
|
class b3PgsJacobiSolver
|
||
|
|
{
|
||
|
|
|
||
|
|
protected:
|
||
|
|
b3AlignedObjectArray<b3SolverBody> m_tmpSolverBodyPool;
|
||
|
|
b3ConstraintArray m_tmpSolverContactConstraintPool;
|
||
|
|
b3ConstraintArray m_tmpSolverNonContactConstraintPool;
|
||
|
|
b3ConstraintArray m_tmpSolverContactFrictionConstraintPool;
|
||
|
|
b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
|
||
|
|
|
||
|
|
b3AlignedObjectArray<int> m_orderTmpConstraintPool;
|
||
|
|
b3AlignedObjectArray<int> m_orderNonContactConstraintPool;
|
||
|
|
b3AlignedObjectArray<int> m_orderFrictionConstraintPool;
|
||
|
|
b3AlignedObjectArray<b3TypedConstraint::b3ConstraintInfo1> m_tmpConstraintSizesPool;
|
||
|
|
|
||
|
|
b3AlignedObjectArray<int> m_bodyCount;
|
||
|
|
b3AlignedObjectArray<int> m_bodyCountCheck;
|
||
|
|
|
||
|
|
b3AlignedObjectArray<b3Vector3> m_deltaLinearVelocities;
|
||
|
|
b3AlignedObjectArray<b3Vector3> m_deltaAngularVelocities;
|
||
|
|
|
||
|
|
bool m_usePgs;
|
||
|
|
void averageVelocities();
|
||
|
|
|
||
|
|
int m_maxOverrideNumSolverIterations;
|
||
|
|
|
||
|
|
int m_numSplitImpulseRecoveries;
|
||
|
|
|
||
|
|
b3Scalar getContactProcessingThreshold(b3Contact4* contact)
|
||
|
|
{
|
||
|
|
return 0.02f;
|
||
|
|
}
|
||
|
|
void setupFrictionConstraint( b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
|
||
|
|
b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
|
||
|
|
b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation,
|
||
|
|
b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
|
||
|
|
|
||
|
|
void setupRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
|
||
|
|
b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
|
||
|
|
b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation,
|
||
|
|
b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
|
||
|
|
|
||
|
|
b3SolverConstraint& addFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
|
||
|
|
b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0, b3Scalar cfmSlip=0.f);
|
||
|
|
|
||
|
|
|
||
|
|
void setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias,
|
||
|
|
b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp,
|
||
|
|
const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
|
||
|
|
b3Vector3& rel_pos1, b3Vector3& rel_pos2);
|
||
|
|
|
||
|
|
void setFrictionConstraintImpulse( b3RigidBodyData* bodies, b3InertiaData* inertias,b3SolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB,
|
||
|
|
b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal);
|
||
|
|
|
||
|
|
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
|
||
|
|
unsigned long m_btSeed2;
|
||
|
|
|
||
|
|
|
||
|
|
b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution);
|
||
|
|
|
||
|
|
void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal);
|
||
|
|
|
||
|
|
|
||
|
|
void resolveSplitPenetrationSIMD(
|
||
|
|
b3SolverBody& bodyA,b3SolverBody& bodyB,
|
||
|
|
const b3SolverConstraint& contactConstraint);
|
||
|
|
|
||
|
|
void resolveSplitPenetrationImpulseCacheFriendly(
|
||
|
|
b3SolverBody& bodyA,b3SolverBody& bodyB,
|
||
|
|
const b3SolverConstraint& contactConstraint);
|
||
|
|
|
||
|
|
//internal method
|
||
|
|
int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies,b3InertiaData* inertias);
|
||
|
|
void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject);
|
||
|
|
|
||
|
|
void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
|
||
|
|
|
||
|
|
void resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
|
||
|
|
|
||
|
|
void resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
|
||
|
|
|
||
|
|
void resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
|
||
|
|
|
||
|
|
protected:
|
||
|
|
|
||
|
|
virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
|
||
|
|
|
||
|
|
|
||
|
|
virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
|
||
|
|
virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
|
||
|
|
b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
|
||
|
|
|
||
|
|
|
||
|
|
virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal);
|
||
|
|
|
||
|
|
|
||
|
|
public:
|
||
|
|
|
||
|
|
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||
|
|
|
||
|
|
b3PgsJacobiSolver(bool usePgs);
|
||
|
|
virtual ~b3PgsJacobiSolver();
|
||
|
|
|
||
|
|
// void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts);
|
||
|
|
void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);
|
||
|
|
|
||
|
|
b3Scalar solveGroup(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
|
||
|
|
|
||
|
|
///clear internal cached data and reset random seed
|
||
|
|
virtual void reset();
|
||
|
|
|
||
|
|
unsigned long b3Rand2();
|
||
|
|
|
||
|
|
int b3RandInt2 (int n);
|
||
|
|
|
||
|
|
void setRandSeed(unsigned long seed)
|
||
|
|
{
|
||
|
|
m_btSeed2 = seed;
|
||
|
|
}
|
||
|
|
unsigned long getRandSeed() const
|
||
|
|
{
|
||
|
|
return m_btSeed2;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
#endif //B3_PGS_JACOBI_SOLVER
|
||
|
|
|