summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h')
-rw-r--r--thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h144
1 files changed, 64 insertions, 80 deletions
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h
index d2ca307fab..5b616541d9 100644
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h
@@ -1,11 +1,9 @@
#ifndef B3_PGS_JACOBI_SOLVER
#define B3_PGS_JACOBI_SOLVER
-
struct b3Contact4;
struct b3ContactPoint;
-
class b3Dispatcher;
#include "b3TypedConstraint.h"
@@ -18,132 +16,118 @@ 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<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();
+ 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_maxOverrideNumSolverIterations;
- int m_numSplitImpulseRecoveries;
+ int m_numSplitImpulseRecoveries;
- b3Scalar getContactProcessingThreshold(b3Contact4* contact)
+ 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 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 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,
+ 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);
+ 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;
+ unsigned long m_btSeed2;
-
b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution);
- void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal);
-
+ void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias, b3Contact4* manifold, const b3ContactSolverInfo& infoGlobal);
- void resolveSplitPenetrationSIMD(
- b3SolverBody& bodyA,b3SolverBody& bodyB,
- const b3SolverConstraint& contactConstraint);
+ void resolveSplitPenetrationSIMD(
+ b3SolverBody& bodyA, b3SolverBody& bodyB,
+ const b3SolverConstraint& contactConstraint);
- void resolveSplitPenetrationImpulseCacheFriendly(
- 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);
+ 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:
+ void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
- virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
+ void resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
+ void resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
- 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);
+ 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 solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,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);
+ // 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);
+ 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();
-
+ virtual void reset();
+
unsigned long b3Rand2();
- int b3RandInt2 (int n);
+ int b3RandInt2(int n);
- void setRandSeed(unsigned long seed)
+ void setRandSeed(unsigned long seed)
{
m_btSeed2 = seed;
}
- unsigned long getRandSeed() const
+ unsigned long getRandSeed() const
{
return m_btSeed2;
}
-
-
-
-
};
-#endif //B3_PGS_JACOBI_SOLVER
-
+#endif //B3_PGS_JACOBI_SOLVER
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-08 00:48:51 +0000