Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

bt -> b3 and BT -> B3 rename for content and filenames

Browse Source
This commit is contained in:
erwin coumans
2013-04-28 23:11:10 -07:00
parent 6bcb5b9d5f
commit 7366e262fd
178 changed files with 5218 additions and 5218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h
View File

@@ -13,12 +13,12 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_CONTACT_SOLVER_INFO
#define BT_CONTACT_SOLVER_INFO
#ifndef B3_CONTACT_SOLVER_INFO
#define B3_CONTACT_SOLVER_INFO
#include "Bullet3Common/b3Scalar.h"
enum btSolverMode
enum b3SolverMode
{
SOLVER_RANDMIZE_ORDER = 1,
SOLVER_FRICTION_SEPARATE = 2,
@@ -32,7 +32,7 @@ enum btSolverMode
SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024
};
struct btContactSolverInfoData
struct b3ContactSolverInfoData
{
@@ -62,7 +62,7 @@ struct btContactSolverInfoData
};
struct b3ContactSolverInfo : public btContactSolverInfoData
struct b3ContactSolverInfo : public b3ContactSolverInfoData
{
@@ -89,13 +89,13 @@ struct b3ContactSolverInfo : public btContactSolverInfoData
m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
m_restingContactRestitutionThreshold = 2;//unused as of 2.81
m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their BT_ENABLE_GYROPSCOPIC_FORCE flag set (using btRigidBody::setFlag)
m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their B3_ENABLE_GYROPSCOPIC_FORCE flag set (using b3RigidBody::setFlag)
m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
}
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btContactSolverInfoDoubleData
struct b3ContactSolverInfoDoubleData
{
double m_tau;
double m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
@@ -123,7 +123,7 @@ struct btContactSolverInfoDoubleData
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btContactSolverInfoFloatData
struct b3ContactSolverInfoFloatData
{
float m_tau;
float m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
@@ -156,4 +156,4 @@ struct btContactSolverInfoFloatData
#endif //BT_CONTACT_SOLVER_INFO
#endif //B3_CONTACT_SOLVER_INFO

164
src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp
View File

@@ -13,8 +13,8 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
//enable BT_SOLVER_DEBUG if you experience solver crashes
//#define BT_SOLVER_DEBUG
//enable B3_SOLVER_DEBUG if you experience solver crashes
//#define B3_SOLVER_DEBUG
//#define COMPUTE_IMPULSE_DENOM 1
//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
@@ -46,7 +46,7 @@ b3Transform getWorldTransform(b3RigidBodyCL* rb)
return newTrans;
}
const b3Matrix3x3& getInvInertiaTensorWorld(btInertiaCL* inertia)
const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaCL* inertia)
{
return inertia->m_invInertiaWorld;
}
@@ -70,7 +70,7 @@ b3Vector3 getVelocityInLocalPoint(b3RigidBodyCL* rb, const b3Vector3& rel_pos)
}
struct btContactPoint
struct b3ContactPoint
{
b3Vector3 m_positionWorldOnA;
b3Vector3 m_positionWorldOnB;
@@ -107,7 +107,7 @@ struct btContactPoint
}
};
void getContactPoint(b3Contact4* contact, int contactIndex, btContactPoint& pointOut)
void getContactPoint(b3Contact4* contact, int contactIndex, b3ContactPoint& pointOut)
{
pointOut.m_appliedImpulse = 0.f;
pointOut.m_appliedImpulseLateral1 = 0.f;
@@ -125,7 +125,7 @@ void getContactPoint(b3Contact4* contact, int contactIndex, btContactPoint& poin
normalOnB.normalize();
b3Vector3 l1,l2;
btPlaneSpace1(normalOnB,l1,l2);
b3PlaneSpace1(normalOnB,l1,l2);
pointOut.m_normalWorldOnB = normalOnB;
//printf("normalOnB = %f,%f,%f\n",normalOnB.getX(),normalOnB.getY(),normalOnB.getZ());
@@ -154,7 +154,7 @@ b3PgsJacobiSolver::~b3PgsJacobiSolver()
{
}
void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints)
void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyCL* bodies, b3InertiaCL* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints)
{
b3ContactSolverInfo infoGlobal;
infoGlobal.m_splitImpulse = false;
@@ -179,7 +179,7 @@ void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyCL* bodies, btIn
/// b3PgsJacobiSolver Sequentially applies impulses
b3Scalar b3PgsJacobiSolver::solveGroup(b3RigidBodyCL* bodies,
btInertiaCL* inertias,
b3InertiaCL* inertias,
int numBodies,
b3Contact4* manifoldPtr,
int numManifolds,
@@ -188,7 +188,7 @@ b3Scalar b3PgsJacobiSolver::solveGroup(b3RigidBodyCL* bodies,
const b3ContactSolverInfo& infoGlobal)
{
BT_PROFILE("solveGroup");
B3_PROFILE("solveGroup");
//you need to provide at least some bodies
solveGroupCacheFriendlySetup( bodies, inertias,numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal);
@@ -210,11 +210,11 @@ b3Scalar b3PgsJacobiSolver::solveGroup(b3RigidBodyCL* bodies,
#ifdef USE_SIMD
#include <emmintrin.h>
#define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
#define b3VecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
static inline __m128 b3SimdDot3( __m128 vec0, __m128 vec1 )
{
__m128 result = _mm_mul_ps( vec0, vec1);
return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) );
return _mm_add_ps( b3VecSplat( result, 0 ), _mm_add_ps( b3VecSplat( result, 1 ), b3VecSplat( result, 2 ) ) );
}
#endif//USE_SIMD
@@ -226,12 +226,12 @@ void b3PgsJacobiSolver::resolveSingleConstraintRowGenericSIMD(b3SolverBody& body
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
__m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),b3SimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
btSimdScalar resultLowerLess,resultUpperLess;
b3SimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
b3SimdScalar resultLowerLess,resultUpperLess;
resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
@@ -290,12 +290,12 @@ void b3PgsJacobiSolver::resolveSingleConstraintRowGenericSIMD(b3SolverBody& body
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
__m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),b3SimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
btSimdScalar resultLowerLess,resultUpperLess;
b3SimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
b3SimdScalar resultLowerLess,resultUpperLess;
resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
@@ -378,12 +378,12 @@ void b3PgsJacobiSolver::resolveSplitPenetrationImpulseCacheFriendly(
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128));
__m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),b3SimdDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
btSimdScalar resultLowerLess,resultUpperLess;
b3SimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
b3SimdScalar resultLowerLess,resultUpperLess;
resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
@@ -403,7 +403,7 @@ void b3PgsJacobiSolver::resolveSplitPenetrationImpulseCacheFriendly(
unsigned long b3PgsJacobiSolver::btRand2()
unsigned long b3PgsJacobiSolver::b3Rand2()
{
m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
return m_btSeed2;
@@ -412,11 +412,11 @@ unsigned long b3PgsJacobiSolver::btRand2()
//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
int b3PgsJacobiSolver::btRandInt2 (int n)
int b3PgsJacobiSolver::b3RandInt2 (int n)
{
// seems good; xor-fold and modulus
const unsigned long un = static_cast<unsigned long>(n);
unsigned long r = btRand2();
unsigned long r = b3Rand2();
// note: probably more aggressive than it needs to be -- might be
// able to get away without one or two of the innermost branches.
@@ -488,7 +488,7 @@ b3Scalar b3PgsJacobiSolver::restitutionCurve(b3Scalar rel_vel, b3Scalar restitut
void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyCL* bodies,btInertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
{
@@ -566,8 +566,8 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyCL* bodies,btInertiaC
// b3Scalar positionalError = 0.f;
btSimdScalar velocityError = desiredVelocity - rel_vel;
btSimdScalar velocityImpulse = velocityError * btSimdScalar(scaledDenom);//solverConstraint.m_jacDiagABInv);
b3SimdScalar velocityError = desiredVelocity - rel_vel;
b3SimdScalar velocityImpulse = velocityError * b3SimdScalar(scaledDenom);//solverConstraint.m_jacDiagABInv);
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = 0;
@@ -576,7 +576,7 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyCL* bodies,btInertiaC
}
}
b3SolverConstraint& b3PgsJacobiSolver::addFrictionConstraint(b3RigidBodyCL* bodies,btInertiaCL* inertias, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
b3SolverConstraint& b3PgsJacobiSolver::addFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
{
b3SolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
solverConstraint.m_frictionIndex = frictionIndex;
@@ -586,8 +586,8 @@ b3SolverConstraint& b3PgsJacobiSolver::addFrictionConstraint(b3RigidBodyCL* bodi
}
void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyCL* bodies,btInertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation,
b3Scalar desiredVelocity, b3Scalar cfmSlip)
@@ -645,8 +645,8 @@ void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyCL* bodies,btI
// b3Scalar positionalError = 0.f;
btSimdScalar velocityError = desiredVelocity - rel_vel;
btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
b3SimdScalar velocityError = desiredVelocity - rel_vel;
b3SimdScalar velocityImpulse = velocityError * b3SimdScalar(solverConstraint.m_jacDiagABInv);
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = 0;
@@ -662,7 +662,7 @@ void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyCL* bodies,btI
b3SolverConstraint& b3PgsJacobiSolver::addRollingFrictionConstraint(b3RigidBodyCL* bodies,btInertiaCL* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
b3SolverConstraint& b3PgsJacobiSolver::addRollingFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
{
b3SolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
solverConstraint.m_frictionIndex = frictionIndex;
@@ -672,9 +672,9 @@ b3SolverConstraint& b3PgsJacobiSolver::addRollingFrictionConstraint(b3RigidBodyC
}
int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,btInertiaCL* inertias)
int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,b3InertiaCL* inertias)
{
//btAssert(bodyIndex< m_tmpSolverBodyPool.size());
//b3Assert(bodyIndex< m_tmpSolverBodyPool.size());
b3RigidBodyCL& body = bodies[bodyIndex];
int curIndex = -1;
@@ -693,7 +693,7 @@ int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,
}
} else
{
btAssert(m_bodyCount[bodyIndex]>0);
b3Assert(m_bodyCount[bodyIndex]>0);
m_bodyCountCheck[bodyIndex]++;
curIndex = m_tmpSolverBodyPool.size();
b3SolverBody& solverBody = m_tmpSolverBodyPool.expand();
@@ -701,16 +701,16 @@ int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,
solverBody.m_originalBodyIndex = bodyIndex;
}
btAssert(curIndex>=0);
b3Assert(curIndex>=0);
return curIndex;
}
#include <stdio.h>
void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyCL* bodies, btInertiaCL* inertias,b3SolverConstraint& solverConstraint,
void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyCL* bodies, b3InertiaCL* inertias,b3SolverConstraint& solverConstraint,
int solverBodyIdA, int solverBodyIdB,
btContactPoint& cp, const b3ContactSolverInfo& infoGlobal,
b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal,
b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
b3Vector3& rel_pos1, b3Vector3& rel_pos2)
{
@@ -872,9 +872,9 @@ void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyCL* bodies, btInertiaC
void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyCL* bodies, btInertiaCL* inertias,b3SolverConstraint& solverConstraint,
void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyCL* bodies, b3InertiaCL* inertias,b3SolverConstraint& solverConstraint,
int solverBodyIdA, int solverBodyIdB,
btContactPoint& cp, const b3ContactSolverInfo& infoGlobal)
b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal)
{
b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
@@ -916,7 +916,7 @@ void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyCL* bodies, btI
void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, btInertiaCL* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal)
void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, b3InertiaCL* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal)
{
b3RigidBodyCL* colObj0=0,*colObj1=0;
@@ -924,8 +924,8 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, btInertiaCL* inert
int solverBodyIdA = getOrInitSolverBody(manifold->getBodyA(),bodies,inertias);
int solverBodyIdB = getOrInitSolverBody(manifold->getBodyB(),bodies,inertias);
// btRigidBody* bodyA = btRigidBody::upcast(colObj0);
// btRigidBody* bodyB = btRigidBody::upcast(colObj1);
// b3RigidBody* bodyA = b3RigidBody::upcast(colObj0);
// b3RigidBody* bodyB = b3RigidBody::upcast(colObj1);
b3SolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
b3SolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
@@ -941,7 +941,7 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, btInertiaCL* inert
for (int j=0;j<numContacts;j++)
{
btContactPoint cp;
b3ContactPoint cp;
getContactPoint(manifold,j,cp);
if (cp.getDistance() <= getContactProcessingThreshold(manifold))
@@ -954,8 +954,8 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, btInertiaCL* inert
int frictionIndex = m_tmpSolverContactConstraintPool.size();
b3SolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
// btRigidBody* rb0 = btRigidBody::upcast(colObj0);
// btRigidBody* rb1 = btRigidBody::upcast(colObj1);
// b3RigidBody* rb0 = b3RigidBody::upcast(colObj0);
// b3RigidBody* rb1 = b3RigidBody::upcast(colObj1);
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
@@ -989,7 +989,7 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, btInertiaCL* inert
{
addRollingFrictionConstraint(bodies,inertias,cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
b3Vector3 axis0,axis1;
btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
b3PlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
if (axis0.length()>0.001)
addRollingFrictionConstraint(bodies,inertias,axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
if (axis1.length()>0.001)
@@ -1019,7 +1019,7 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, btInertiaCL* inert
b3Scalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
{
cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
cp.m_lateralFrictionDir1 *= 1.f/b3Sqrt(lat_rel_vel);
if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
@@ -1032,7 +1032,7 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, btInertiaCL* inert
} else
{
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
b3PlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
@@ -1064,9 +1064,9 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, btInertiaCL* inert
}
}
b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, btInertiaCL* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal)
b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, b3InertiaCL* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal)
{
BT_PROFILE("solveGroupCacheFriendlySetup");
B3_PROFILE("solveGroupCacheFriendlySetup");
m_maxOverrideNumSolverIterations = 0;
@@ -1099,7 +1099,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
} else
{
//didn't implement joints with Jacobi version yet
btAssert(0);
b3Assert(0);
}
}
@@ -1143,7 +1143,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
}
}
//btRigidBody* rb0=0,*rb1=0;
//b3RigidBody* rb0=0,*rb1=0;
//if (1)
{
{
@@ -1155,8 +1155,8 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
//calculate the total number of contraint rows
for (i=0;i<numConstraints;i++)
{
b3TypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
btJointFeedback* fb = constraints[i]->getJointFeedback();
b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
b3JointFeedback* fb = constraints[i]->getJointFeedback();
if (fb)
{
fb->m_appliedForceBodyA.setZero();
@@ -1182,23 +1182,23 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
#ifndef DISABLE_JOINTS
///setup the btSolverConstraints
///setup the b3SolverConstraints
int currentRow = 0;
for (i=0;i<numConstraints;i++)
{
const b3TypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
const b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
if (info1.m_numConstraintRows)
{
btAssert(currentRow<totalNumRows);
b3Assert(currentRow<totalNumRows);
b3SolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
b3TypedConstraint* constraint = constraints[i];
b3RigidBodyCL& rbA = bodies[ constraint->getRigidBodyA()];
//btRigidBody& rbA = constraint->getRigidBodyA();
// btRigidBody& rbB = constraint->getRigidBodyB();
//b3RigidBody& rbA = constraint->getRigidBodyA();
// b3RigidBody& rbB = constraint->getRigidBodyB();
b3RigidBodyCL& rbB = bodies[ constraint->getRigidBodyB()];
int solverBodyIdA = getOrInitSolverBody(constraint->getRigidBodyA(),bodies,inertias);
@@ -1238,7 +1238,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
b3TypedConstraint::btConstraintInfo2 info2;
b3TypedConstraint::b3ConstraintInfo2 info2;
info2.fps = 1.f/infoGlobal.m_timeStep;
info2.erp = infoGlobal.m_erp;
info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
@@ -1247,7 +1247,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
info2.rowskip = sizeof(b3SolverConstraint)/sizeof(b3Scalar);//check this
///the size of b3SolverConstraint needs be a multiple of b3Scalar
btAssert(info2.rowskip*sizeof(b3Scalar)== sizeof(b3SolverConstraint));
b3Assert(info2.rowskip*sizeof(b3Scalar)== sizeof(b3SolverConstraint));
info2.m_constraintError = &currentConstraintRow->m_rhs;
currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
info2.m_damping = infoGlobal.m_damping;
@@ -1277,7 +1277,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
b3Matrix3x3& invInertiaWorldA= inertias[constraint->getRigidBodyA()].m_invInertiaWorld;
{
//btVector3 angularFactorA(1,1,1);
//b3Vector3 angularFactorA(1,1,1);
const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
solverConstraint.m_angularComponentA = invInertiaWorldA*ftorqueAxis1;//*angularFactorA;
}
@@ -1299,8 +1299,8 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
sum += iMJlB.dot(solverConstraint.m_contactNormal);
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
b3Scalar fsum = btFabs(sum);
btAssert(fsum > SIMD_EPSILON);
b3Scalar fsum = b3Fabs(sum);
b3Assert(fsum > SIMD_EPSILON);
solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?b3Scalar(1.)/sum : 0.f;
}
@@ -1392,7 +1392,7 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
for (int j=0; j<numNonContactPool; ++j) {
int tmp = m_orderNonContactConstraintPool[j];
int swapi = btRandInt2(j+1);
int swapi = b3RandInt2(j+1);
m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
m_orderNonContactConstraintPool[swapi] = tmp;
}
@@ -1402,14 +1402,14 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
{
for (int j=0; j<numConstraintPool; ++j) {
int tmp = m_orderTmpConstraintPool[j];
int swapi = btRandInt2(j+1);
int swapi = b3RandInt2(j+1);
m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
m_orderTmpConstraintPool[swapi] = tmp;
}
for (int j=0; j<numFrictionPool; ++j) {
int tmp = m_orderFrictionConstraintPool[j];
int swapi = btRandInt2(j+1);
int swapi = b3RandInt2(j+1);
m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
m_orderFrictionConstraintPool[swapi] = tmp;
}
@@ -1637,7 +1637,7 @@ void b3PgsJacobiSolver::solveGroupCacheFriendlySplitImpulseIterations(b3TypedCon
b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal)
{
BT_PROFILE("solveGroupCacheFriendlyIterations");
B3_PROFILE("solveGroupCacheFriendlyIterations");
{
///this is a special step to resolve penetrations (just for contacts)
@@ -1664,7 +1664,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyIterations(b3TypedConstraint*
void b3PgsJacobiSolver::averageVelocities()
{
BT_PROFILE("averaging");
B3_PROFILE("averaging");
//average the velocities
int numBodies = m_bodyCount.size();
@@ -1690,7 +1690,7 @@ void b3PgsJacobiSolver::averageVelocities()
if (!m_tmpSolverBodyPool[i].m_invMass.isZero())
{
btAssert(m_bodyCount[orgBodyIndex] == m_bodyCountCheck[orgBodyIndex]);
b3Assert(m_bodyCount[orgBodyIndex] == m_bodyCountCheck[orgBodyIndex]);
b3Scalar factor = 1.f/b3Scalar(m_bodyCount[orgBodyIndex]);
@@ -1701,7 +1701,7 @@ void b3PgsJacobiSolver::averageVelocities()
}
}
b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies,btInertiaCL* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal)
b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies,b3InertiaCL* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal)
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int i,j;
@@ -1711,8 +1711,8 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies,
for (j=0;j<numPoolConstraints;j++)
{
const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
btContactPoint* pt = (btContactPoint*) solveManifold.m_originalContactPoint;
btAssert(pt);
b3ContactPoint* pt = (b3ContactPoint*) solveManifold.m_originalContactPoint;
b3Assert(pt);
pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
// float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
// printf("pt->m_appliedImpulseLateral1 = %f\n", f);
@@ -1731,7 +1731,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies,
{
const b3SolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
b3TypedConstraint* constr = (b3TypedConstraint*)solverConstr.m_originalContactPoint;
btJointFeedback* fb = constr->getJointFeedback();
b3JointFeedback* fb = constr->getJointFeedback();
if (fb)
{
b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverConstr.m_solverBodyIdA];
@@ -1745,18 +1745,18 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies,
}
constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
if (b3Fabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
{
constr->setEnabled(false);
}
}
{
BT_PROFILE("write back velocities and transforms");
B3_PROFILE("write back velocities and transforms");
for ( i=0;i<m_tmpSolverBodyPool.size();i++)
{
int bodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex;
//btAssert(i==bodyIndex);
//b3Assert(i==bodyIndex);
b3RigidBodyCL* body = &bodies[bodyIndex];
if (body->getInvMass())

62
src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h
View File

@@ -1,12 +1,12 @@
#ifndef BT_PGS_JACOBI_SOLVER
#define BT_PGS_JACOBI_SOLVER
#ifndef B3_PGS_JACOBI_SOLVER
#define B3_PGS_JACOBI_SOLVER
struct b3Contact4;
struct btContactPoint;
struct b3ContactPoint;
class btDispatcher;
class b3Dispatcher;
#include "b3TypedConstraint.h"
#include "b3ContactSolverInfo.h"
@@ -14,22 +14,22 @@ class btDispatcher;
#include "b3SolverConstraint.h"
struct b3RigidBodyCL;
struct btInertiaCL;
struct b3InertiaCL;
class b3PgsJacobiSolver
{
protected:
b3AlignedObjectArray<b3SolverBody> m_tmpSolverBodyPool;
btConstraintArray m_tmpSolverContactConstraintPool;
btConstraintArray m_tmpSolverNonContactConstraintPool;
btConstraintArray m_tmpSolverContactFrictionConstraintPool;
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
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::btConstraintInfo1> m_tmpConstraintSizesPool;
b3AlignedObjectArray<b3TypedConstraint::b3ConstraintInfo1> m_tmpConstraintSizesPool;
b3AlignedObjectArray<int> m_bodyCount;
b3AlignedObjectArray<int> m_bodyCountCheck;
@@ -45,27 +45,27 @@ protected:
{
return 0.02f;
}
void setupFrictionConstraint( b3RigidBodyCL* bodies,btInertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
void setupFrictionConstraint( b3RigidBodyCL* bodies,b3InertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation,
b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
void setupRollingFrictionConstraint(b3RigidBodyCL* bodies,btInertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
void setupRollingFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation,
b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
b3SolverConstraint& addFrictionConstraint(b3RigidBodyCL* bodies,btInertiaCL* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyCL* bodies,btInertiaCL* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0, b3Scalar cfmSlip=0.f);
b3SolverConstraint& addFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0, b3Scalar cfmSlip=0.f);
void setupContactConstraint(b3RigidBodyCL* bodies, btInertiaCL* inertias,
b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btContactPoint& cp,
void setupContactConstraint(b3RigidBodyCL* bodies, b3InertiaCL* 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( b3RigidBodyCL* bodies, btInertiaCL* inertias,b3SolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB,
btContactPoint& cp, const b3ContactSolverInfo& infoGlobal);
void setFrictionConstraintImpulse( b3RigidBodyCL* bodies, b3InertiaCL* 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;
@@ -73,7 +73,7 @@ protected:
b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution);
void convertContact(b3RigidBodyCL* bodies, btInertiaCL* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal);
void convertContact(b3RigidBodyCL* bodies, b3InertiaCL* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal);
void resolveSplitPenetrationSIMD(
@@ -85,7 +85,7 @@ protected:
const b3SolverConstraint& contactConstraint);
//internal method
int getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,btInertiaCL* inertias);
int getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,b3InertiaCL* inertias);
void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyCL* collisionObject);
void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
@@ -98,7 +98,7 @@ protected:
protected:
virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, btInertiaCL* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, b3InertiaCL* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
@@ -106,27 +106,27 @@ protected:
b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies, btInertiaCL* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies, b3InertiaCL* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
B3_DECLARE_ALIGNED_ALLOCATOR();
b3PgsJacobiSolver();
virtual ~b3PgsJacobiSolver();
// void solveContacts(int numBodies, b3RigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, b3Contact4* contacts);
void solveContacts(int numBodies, b3RigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);
// void solveContacts(int numBodies, b3RigidBodyCL* bodies, b3InertiaCL* inertias, int numContacts, b3Contact4* contacts);
void solveContacts(int numBodies, b3RigidBodyCL* bodies, b3InertiaCL* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);
b3Scalar solveGroup(b3RigidBodyCL* bodies,btInertiaCL* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
b3Scalar solveGroup(b3RigidBodyCL* bodies,b3InertiaCL* 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 btRand2();
unsigned long b3Rand2();
int btRandInt2 (int n);
int b3RandInt2 (int n);
void setRandSeed(unsigned long seed)
{
@@ -142,5 +142,5 @@ public:
};
#endif //BT_PGS_JACOBI_SOLVER
#endif //B3_PGS_JACOBI_SOLVER

46
src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp
View File

@@ -51,12 +51,12 @@ void b3Point2PointConstraint::buildJacobian()
}
void b3Point2PointConstraint::getInfo1 (btConstraintInfo1* info)
void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info)
{
getInfo1NonVirtual(info);
}
void b3Point2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info)
{
if (m_useSolveConstraintObsolete)
{
@@ -72,7 +72,7 @@ void b3Point2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
void b3Point2PointConstraint::getInfo2 (btConstraintInfo2* info, const b3RigidBodyCL* bodies)
void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies)
{
b3Transform trA;
trA.setIdentity();
@@ -87,9 +87,9 @@ void b3Point2PointConstraint::getInfo2 (btConstraintInfo2* info, const b3RigidBo
getInfo2NonVirtual(info, trA,trB);
}
void b3Point2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
void b3Point2PointConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
{
btAssert(!m_useSolveConstraintObsolete);
b3Assert(!m_useSolveConstraintObsolete);
//retrieve matrices
@@ -129,7 +129,7 @@ void b3Point2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const
// set right hand side
b3Scalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp;
b3Scalar k = info->fps * currERP;
int j;
for (j=0; j<3; j++)
@@ -137,7 +137,7 @@ void b3Point2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const
info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
if(m_flags & BT_P2P_FLAGS_CFM)
if(m_flags & B3_P2P_FLAGS_CFM)
{
for (j=0; j<3; j++)
{
@@ -172,24 +172,24 @@ void b3Point2PointConstraint::setParam(int num, b3Scalar value, int axis)
{
if(axis != -1)
{
btAssertConstrParams(0);
b3AssertConstrParams(0);
}
else
{
switch(num)
{
case BT_CONSTRAINT_ERP :
case BT_CONSTRAINT_STOP_ERP :
case B3_CONSTRAINT_ERP :
case B3_CONSTRAINT_STOP_ERP :
m_erp = value;
m_flags |= BT_P2P_FLAGS_ERP;
m_flags |= B3_P2P_FLAGS_ERP;
break;
case BT_CONSTRAINT_CFM :
case BT_CONSTRAINT_STOP_CFM :
case B3_CONSTRAINT_CFM :
case B3_CONSTRAINT_STOP_CFM :
m_cfm = value;
m_flags |= BT_P2P_FLAGS_CFM;
m_flags |= B3_P2P_FLAGS_CFM;
break;
default:
btAssertConstrParams(0);
b3AssertConstrParams(0);
}
}
}
@@ -200,24 +200,24 @@ b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const
b3Scalar retVal(SIMD_INFINITY);
if(axis != -1)
{
btAssertConstrParams(0);
b3AssertConstrParams(0);
}
else
{
switch(num)
{
case BT_CONSTRAINT_ERP :
case BT_CONSTRAINT_STOP_ERP :
btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
case B3_CONSTRAINT_ERP :
case B3_CONSTRAINT_STOP_ERP :
b3AssertConstrParams(m_flags & B3_P2P_FLAGS_ERP);
retVal = m_erp;
break;
case BT_CONSTRAINT_CFM :
case BT_CONSTRAINT_STOP_CFM :
btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
case B3_CONSTRAINT_CFM :
case B3_CONSTRAINT_STOP_CFM :
b3AssertConstrParams(m_flags & B3_P2P_FLAGS_CFM);
retVal = m_cfm;
break;
default:
btAssertConstrParams(0);
b3AssertConstrParams(0);
}
}
return retVal;

68
src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h
View File

@@ -13,27 +13,27 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_POINT2POINTCONSTRAINT_H
#define BT_POINT2POINTCONSTRAINT_H
#ifndef B3_POINT2POINTCONSTRAINT_H
#define B3_POINT2POINTCONSTRAINT_H
#include "Bullet3Common/b3Vector3.h"
//#include "b3JacobianEntry.h"
#include "b3TypedConstraint.h"
class btRigidBody;
class b3RigidBody;
#ifdef BT_USE_DOUBLE_PRECISION
#define btPoint2PointConstraintData btPoint2PointConstraintDoubleData
#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData"
#ifdef B3_USE_DOUBLE_PRECISION
#define b3Point2PointConstraintData b3Point2PointConstraintDoubleData
#define b3Point2PointConstraintDataName "b3Point2PointConstraintDoubleData"
#else
#define btPoint2PointConstraintData btPoint2PointConstraintFloatData
#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION
#define b3Point2PointConstraintData b3Point2PointConstraintFloatData
#define b3Point2PointConstraintDataName "b3Point2PointConstraintFloatData"
#endif //B3_USE_DOUBLE_PRECISION
struct btConstraintSetting
struct b3ConstraintSetting
{
btConstraintSetting() :
b3ConstraintSetting() :
m_tau(b3Scalar(0.3)),
m_damping(b3Scalar(1.)),
m_impulseClamp(b3Scalar(0.))
@@ -44,10 +44,10 @@ struct btConstraintSetting
b3Scalar m_impulseClamp;
};
enum btPoint2PointFlags
enum b3Point2PointFlags
{
BT_P2P_FLAGS_ERP = 1,
BT_P2P_FLAGS_CFM = 2
B3_P2P_FLAGS_ERP = 1,
B3_P2P_FLAGS_CFM = 2
};
/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
@@ -66,12 +66,12 @@ public:
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
B3_DECLARE_ALIGNED_ALLOCATOR();
///for backwards compatibility during the transition to 'getInfo/getInfo2'
bool m_useSolveConstraintObsolete;
btConstraintSetting m_setting;
b3ConstraintSetting m_setting;
b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB);
@@ -80,13 +80,13 @@ public:
virtual void buildJacobian();
virtual void getInfo1 (btConstraintInfo1* info);
virtual void getInfo1 (b3ConstraintInfo1* info);
void getInfo1NonVirtual (btConstraintInfo1* info);
void getInfo1NonVirtual (b3ConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* info, const b3RigidBodyCL* bodies);
virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies);
void getInfo2NonVirtual (btConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans);
void getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans);
void updateRHS(b3Scalar timeStep);
@@ -119,45 +119,45 @@ public:
// virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
// virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
// virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btPoint2PointConstraintFloatData
struct b3Point2PointConstraintFloatData
{
btTypedConstraintData m_typeConstraintData;
btVector3FloatData m_pivotInA;
btVector3FloatData m_pivotInB;
b3TypedConstraintData m_typeConstraintData;
b3Vector3FloatData m_pivotInA;
b3Vector3FloatData m_pivotInB;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btPoint2PointConstraintDoubleData
struct b3Point2PointConstraintDoubleData
{
btTypedConstraintData m_typeConstraintData;
btVector3DoubleData m_pivotInA;
btVector3DoubleData m_pivotInB;
b3TypedConstraintData m_typeConstraintData;
b3Vector3DoubleData m_pivotInA;
b3Vector3DoubleData m_pivotInB;
};
/*
SIMD_FORCE_INLINE int b3Point2PointConstraint::calculateSerializeBufferSize() const
{
return sizeof(btPoint2PointConstraintData);
return sizeof(b3Point2PointConstraintData);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* b3Point2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
SIMD_FORCE_INLINE const char* b3Point2PointConstraint::serialize(void* dataBuffer, b3Serializer* serializer) const
{
btPoint2PointConstraintData* p2pData = (btPoint2PointConstraintData*)dataBuffer;
b3Point2PointConstraintData* p2pData = (b3Point2PointConstraintData*)dataBuffer;
b3TypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
m_pivotInA.serialize(p2pData->m_pivotInA);
m_pivotInB.serialize(p2pData->m_pivotInB);
return btPoint2PointConstraintDataName;
return b3Point2PointConstraintDataName;
}
*/
#endif //BT_POINT2POINTCONSTRAINT_H
#endif //B3_POINT2POINTCONSTRAINT_H

40
src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h
View File

@@ -13,10 +13,10 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_SOLVER_BODY_H
#define BT_SOLVER_BODY_H
#ifndef B3_SOLVER_BODY_H
#define B3_SOLVER_BODY_H
class btRigidBody;
class b3RigidBody;
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Matrix3x3.h"
@@ -24,26 +24,26 @@ class btRigidBody;
#include "Bullet3Common/b3TransformUtil.h"
///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
#ifdef BT_USE_SSE
#ifdef B3_USE_SSE
#define USE_SIMD 1
#endif //
#ifdef USE_SIMD
struct btSimdScalar
struct b3SimdScalar
{
SIMD_FORCE_INLINE btSimdScalar()
SIMD_FORCE_INLINE b3SimdScalar()
{
}
SIMD_FORCE_INLINE btSimdScalar(float fl)
SIMD_FORCE_INLINE b3SimdScalar(float fl)
:m_vec128 (_mm_set1_ps(fl))
{
}
SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
SIMD_FORCE_INLINE b3SimdScalar(__m128 v128)
:m_vec128(v128)
{
}
@@ -85,29 +85,29 @@ struct btSimdScalar
};
///@brief Return the elementwise product of two btSimdScalar
SIMD_FORCE_INLINE btSimdScalar
operator*(const btSimdScalar& v1, const btSimdScalar& v2)
///@brief Return the elementwise product of two b3SimdScalar
SIMD_FORCE_INLINE b3SimdScalar
operator*(const b3SimdScalar& v1, const b3SimdScalar& v2)
{
return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
return b3SimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
}
///@brief Return the elementwise product of two btSimdScalar
SIMD_FORCE_INLINE btSimdScalar
operator+(const btSimdScalar& v1, const btSimdScalar& v2)
///@brief Return the elementwise product of two b3SimdScalar
SIMD_FORCE_INLINE b3SimdScalar
operator+(const b3SimdScalar& v1, const b3SimdScalar& v2)
{
return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
return b3SimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
}
#else
#define btSimdScalar b3Scalar
#define b3SimdScalar b3Scalar
#endif
///The b3SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
ATTRIBUTE_ALIGNED64 (struct) b3SolverBody
{
BT_DECLARE_ALIGNED_ALLOCATOR();
B3_DECLARE_ALIGNED_ALLOCATOR();
b3Transform m_worldTransform;
b3Vector3 m_deltaLinearVelocity;
b3Vector3 m_deltaAngularVelocity;
@@ -281,7 +281,7 @@ ATTRIBUTE_ALIGNED64 (struct) b3SolverBody
b3Transform newTransform;
if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0)
{
// btQuaternion orn = m_worldTransform.getRotation();
// b3Quaternion orn = m_worldTransform.getRotation();
b3TransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
m_worldTransform = newTransform;
}
@@ -294,6 +294,6 @@ ATTRIBUTE_ALIGNED64 (struct) b3SolverBody
};
#endif //BT_SOLVER_BODY_H
#endif //B3_SOLVER_BODY_H

24
src/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h
View File

@@ -13,13 +13,13 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_SOLVER_CONSTRAINT_H
#define BT_SOLVER_CONSTRAINT_H
#ifndef B3_SOLVER_CONSTRAINT_H
#define B3_SOLVER_CONSTRAINT_H
class btRigidBody;
class b3RigidBody;
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Matrix3x3.h"
//#include "btJacobianEntry.h"
//#include "b3JacobianEntry.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
//#define NO_FRICTION_TANGENTIALS 1
@@ -29,7 +29,7 @@ class btRigidBody;
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
ATTRIBUTE_ALIGNED16 (struct) b3SolverConstraint
{
BT_DECLARE_ALIGNED_ALLOCATOR();
B3_DECLARE_ALIGNED_ALLOCATOR();
b3Vector3 m_relpos1CrossNormal;
b3Vector3 m_contactNormal;
@@ -40,8 +40,8 @@ ATTRIBUTE_ALIGNED16 (struct) b3SolverConstraint
b3Vector3 m_angularComponentA;
b3Vector3 m_angularComponentB;
mutable btSimdScalar m_appliedPushImpulse;
mutable btSimdScalar m_appliedImpulse;
mutable b3SimdScalar m_appliedPushImpulse;
mutable b3SimdScalar m_appliedImpulse;
b3Scalar m_friction;
b3Scalar m_jacDiagABInv;
@@ -63,17 +63,17 @@ ATTRIBUTE_ALIGNED16 (struct) b3SolverConstraint
int m_solverBodyIdB;
enum btSolverConstraintType
enum b3SolverConstraintType
{
BT_SOLVER_CONTACT_1D = 0,
BT_SOLVER_FRICTION_1D
B3_SOLVER_CONTACT_1D = 0,
B3_SOLVER_FRICTION_1D
};
};
typedef b3AlignedObjectArray<b3SolverConstraint> btConstraintArray;
typedef b3AlignedObjectArray<b3SolverConstraint> b3ConstraintArray;
#endif //BT_SOLVER_CONSTRAINT_H
#endif //B3_SOLVER_CONSTRAINT_H

30
src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp
View File

@@ -15,15 +15,15 @@ subject to the following restrictions:
#include "b3TypedConstraint.h"
//#include "Bullet3Common/btSerializer.h"
//#include "Bullet3Common/b3Serializer.h"
#define DEFAULT_DEBUGDRAW_SIZE b3Scalar(0.3f)
b3TypedConstraint::b3TypedConstraint(btTypedConstraintType type, int rbA,int rbB)
:btTypedObject(type),
b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA,int rbB)
:b3TypedObject(type),
m_userConstraintType(-1),
m_userConstraintId(-1),
m_breakingImpulseThreshold(SIMD_INFINITY),
@@ -92,16 +92,16 @@ b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scal
void btAngularLimit::set(b3Scalar low, b3Scalar high, b3Scalar _softness, b3Scalar _biasFactor, b3Scalar _relaxationFactor)
void b3AngularLimit::set(b3Scalar low, b3Scalar high, b3Scalar _softness, b3Scalar _biasFactor, b3Scalar _relaxationFactor)
{
m_halfRange = (high - low) / 2.0f;
m_center = btNormalizeAngle(low + m_halfRange);
m_center = b3NormalizeAngle(low + m_halfRange);
m_softness = _softness;
m_biasFactor = _biasFactor;
m_relaxationFactor = _relaxationFactor;
}
void btAngularLimit::test(const b3Scalar angle)
void b3AngularLimit::test(const b3Scalar angle)
{
m_correction = 0.0f;
m_sign = 0.0f;
@@ -109,7 +109,7 @@ void btAngularLimit::test(const b3Scalar angle)
if (m_halfRange >= 0.0f)
{
b3Scalar deviation = btNormalizeAngle(angle - m_center);
b3Scalar deviation = b3NormalizeAngle(angle - m_center);
if (deviation < -m_halfRange)
{
m_solveLimit = true;
@@ -126,17 +126,17 @@ void btAngularLimit::test(const b3Scalar angle)
}
b3Scalar btAngularLimit::getError() const
b3Scalar b3AngularLimit::getError() const
{
return m_correction * m_sign;
}
void btAngularLimit::fit(b3Scalar& angle) const
void b3AngularLimit::fit(b3Scalar& angle) const
{
if (m_halfRange > 0.0f)
{
b3Scalar relativeAngle = btNormalizeAngle(angle - m_center);
if (!btEqual(relativeAngle, m_halfRange))
b3Scalar relativeAngle = b3NormalizeAngle(angle - m_center);
if (!b3Equal(relativeAngle, m_halfRange))
{
if (relativeAngle > 0.0f)
{
@@ -150,12 +150,12 @@ void btAngularLimit::fit(b3Scalar& angle) const
}
}
b3Scalar btAngularLimit::getLow() const
b3Scalar b3AngularLimit::getLow() const
{
return btNormalizeAngle(m_center - m_halfRange);
return b3NormalizeAngle(m_center - m_halfRange);
}
b3Scalar btAngularLimit::getHigh() const
b3Scalar b3AngularLimit::getHigh() const
{
return btNormalizeAngle(m_center + m_halfRange);
return b3NormalizeAngle(m_center + m_halfRange);
}

80
src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h
View File

@@ -13,17 +13,17 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_TYPED_CONSTRAINT_H
#define BT_TYPED_CONSTRAINT_H
#ifndef B3_TYPED_CONSTRAINT_H
#define B3_TYPED_CONSTRAINT_H
#include "Bullet3Common/b3Scalar.h"
#include "b3SolverConstraint.h"
class btSerializer;
class b3Serializer;
//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
enum btTypedConstraintType
enum b3TypedConstraintType
{
POINT2POINT_CONSTRAINT_TYPE=3,
HINGE_CONSTRAINT_TYPE,
@@ -37,22 +37,22 @@ enum btTypedConstraintType
};
enum btConstraintParams
enum b3ConstraintParams
{
BT_CONSTRAINT_ERP=1,
BT_CONSTRAINT_STOP_ERP,
BT_CONSTRAINT_CFM,
BT_CONSTRAINT_STOP_CFM
B3_CONSTRAINT_ERP=1,
B3_CONSTRAINT_STOP_ERP,
B3_CONSTRAINT_CFM,
B3_CONSTRAINT_STOP_CFM
};
#if 1
#define btAssertConstrParams(_par) btAssert(_par)
#define b3AssertConstrParams(_par) b3Assert(_par)
#else
#define btAssertConstrParams(_par)
#define b3AssertConstrParams(_par)
#endif
ATTRIBUTE_ALIGNED16(struct) btJointFeedback
ATTRIBUTE_ALIGNED16(struct) b3JointFeedback
{
b3Vector3 m_appliedForceBodyA;
b3Vector3 m_appliedTorqueBodyA;
@@ -64,7 +64,7 @@ struct b3RigidBodyCL;
///TypedConstraint is the baseclass for Bullet constraints and vehicles
ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public btTypedObject
ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public b3TypedObject
{
int m_userConstraintType;
@@ -82,7 +82,7 @@ ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public btTypedObject
b3TypedConstraint& operator=(b3TypedConstraint& other)
{
btAssert(0);
b3Assert(0);
(void) other;
return *this;
}
@@ -92,7 +92,7 @@ protected:
int m_rbB;
b3Scalar m_appliedImpulse;
b3Scalar m_dbgDrawSize;
btJointFeedback* m_jointFeedback;
b3JointFeedback* m_jointFeedback;
///internal method used by the constraint solver, don't use them directly
b3Scalar getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact);
@@ -100,18 +100,18 @@ protected:
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
B3_DECLARE_ALIGNED_ALLOCATOR();
virtual ~b3TypedConstraint() {};
b3TypedConstraint(btTypedConstraintType type, int bodyA,int bodyB);
b3TypedConstraint(b3TypedConstraintType type, int bodyA,int bodyB);
struct btConstraintInfo1 {
struct b3ConstraintInfo1 {
int m_numConstraintRows,nub;
};
static btRigidBody& getFixedBody();
static b3RigidBody& getFixedBody();
struct btConstraintInfo2 {
struct b3ConstraintInfo2 {
// integrator parameters: frames per second (1/stepsize), default error
// reduction parameter (0..1).
b3Scalar fps,erp;
@@ -161,7 +161,7 @@ public:
virtual void buildJacobian() {};
///internal method used by the constraint solver, don't use them directly
virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, b3Scalar timeStep)
virtual void setupSolverConstraint(b3ConstraintArray& ca, int solverBodyA,int solverBodyB, b3Scalar timeStep)
{
(void)ca;
(void)solverBodyA;
@@ -170,10 +170,10 @@ public:
}
///internal method used by the constraint solver, don't use them directly
virtual void getInfo1 (btConstraintInfo1* info)=0;
virtual void getInfo1 (b3ConstraintInfo1* info)=0;
///internal method used by the constraint solver, don't use them directly
virtual void getInfo2 (btConstraintInfo2* info, const b3RigidBodyCL* bodies)=0;
virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies)=0;
///internal method used by the constraint solver, don't use them directly
void internalSetAppliedImpulse(b3Scalar appliedImpulse)
@@ -261,17 +261,17 @@ public:
return m_userConstraintPtr;
}
void setJointFeedback(btJointFeedback* jointFeedback)
void setJointFeedback(b3JointFeedback* jointFeedback)
{
m_jointFeedback = jointFeedback;
}
const btJointFeedback* getJointFeedback() const
const b3JointFeedback* getJointFeedback() const
{
return m_jointFeedback;
}
btJointFeedback* getJointFeedback()
b3JointFeedback* getJointFeedback()
{
return m_jointFeedback;
}
@@ -298,13 +298,13 @@ public:
///This feedback could be used to determine breaking constraints or playing sounds.
b3Scalar getAppliedImpulse() const
{
btAssert(m_needsFeedback);
b3Assert(m_needsFeedback);
return m_appliedImpulse;
}
btTypedConstraintType getConstraintType () const
b3TypedConstraintType getConstraintType () const
{
return btTypedConstraintType(m_objectType);
return b3TypedConstraintType(m_objectType);
}
void setDbgDrawSize(b3Scalar dbgDrawSize)
@@ -326,13 +326,13 @@ public:
// virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
//virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
//virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const;
};
// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
SIMD_FORCE_INLINE b3Scalar btAdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar angleLowerLimitInRadians, b3Scalar angleUpperLimitInRadians)
SIMD_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar angleLowerLimitInRadians, b3Scalar angleUpperLimitInRadians)
{
if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
{
@@ -340,14 +340,14 @@ SIMD_FORCE_INLINE b3Scalar btAdjustAngleToLimits(b3Scalar angleInRadians, b3Scal
}
else if(angleInRadians < angleLowerLimitInRadians)
{
b3Scalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
b3Scalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleLowerLimitInRadians - angleInRadians));
b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleUpperLimitInRadians - angleInRadians));
return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
}
else if(angleInRadians > angleUpperLimitInRadians)
{
b3Scalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
b3Scalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleInRadians - angleUpperLimitInRadians));
b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleInRadians - angleLowerLimitInRadians));
return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
}
else
@@ -357,7 +357,7 @@ SIMD_FORCE_INLINE b3Scalar btAdjustAngleToLimits(b3Scalar angleInRadians, b3Scal
}
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btTypedConstraintData
struct b3TypedConstraintData
{
int m_bodyA;
int m_bodyB;
@@ -381,12 +381,12 @@ struct btTypedConstraintData
/*SIMD_FORCE_INLINE int b3TypedConstraint::calculateSerializeBufferSize() const
{
return sizeof(btTypedConstraintData);
return sizeof(b3TypedConstraintData);
}
*/
class btAngularLimit
class b3AngularLimit
{
private:
b3Scalar
@@ -403,7 +403,7 @@ private:
public:
/// Default constructor initializes limit as inactive, allowing free constraint movement
btAngularLimit()
b3AngularLimit()
:m_center(0.0f),
m_halfRange(-1.0f),
m_softness(0.9f),
@@ -480,4 +480,4 @@ public:
#endif //BT_TYPED_CONSTRAINT_H
#endif //B3_TYPED_CONSTRAINT_H