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

rename to b3 convention, to avoid naming conflicts when using in combination with Bullet 2.x

Browse Source
This commit is contained in:
erwincoumans
2013-04-29 15:19:36 -07:00
parent 7366e262fd
commit 55b69201a9
88 changed files with 1682 additions and 1584 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -20,16 +20,16 @@ subject to the following restrictions:
enum b3SolverMode
{
SOLVER_RANDMIZE_ORDER = 1,
SOLVER_FRICTION_SEPARATE = 2,
SOLVER_USE_WARMSTARTING = 4,
SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
SOLVER_CACHE_FRIENDLY = 128,
SOLVER_SIMD = 256,
SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024
B3_SOLVER_RANDMIZE_ORDER = 1,
B3_SOLVER_FRICTION_SEPARATE = 2,
B3_SOLVER_USE_WARMSTARTING = 4,
B3_SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
B3_SOLVER_CACHE_FRIENDLY = 128,
B3_SOLVER_SIMD = 256,
B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
B3_SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024
};
struct b3ContactSolverInfoData
@@ -85,8 +85,8 @@ struct b3ContactSolverInfo : public b3ContactSolverInfoData
m_splitImpulseTurnErp = 0.1f;
m_linearSlop = b3Scalar(0.0);
m_warmstartingFactor=b3Scalar(0.85);
//m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER;
m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
//m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD | B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | B3_SOLVER_RANDMIZE_ORDER;
m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD;// | B3_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 B3_ENABLE_GYROPSCOPIC_FORCE flag set (using b3RigidBody::setFlag)

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

@@ -33,8 +33,6 @@ subject to the following restrictions:
//#include "../../dynamics/basic_demo/Stubs/AdlContact4.h"
#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h"
bool usePgs = true;
int gNumSplitImpulseRecoveries2 = 0;
#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
@@ -144,8 +142,9 @@ int getNumContacts(b3Contact4* contact)
return contact->getNPoints();
}
b3PgsJacobiSolver::b3PgsJacobiSolver()
:m_btSeed2(0),m_usePgs(usePgs)
b3PgsJacobiSolver::b3PgsJacobiSolver(bool usePgs)
:m_btSeed2(0),m_usePgs(usePgs),
m_numSplitImpulseRecoveries(0)
{
}
@@ -160,12 +159,12 @@ void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyCL* bodies, b3In
infoGlobal.m_splitImpulse = false;
infoGlobal.m_timeStep = 1.f/60.f;
infoGlobal.m_numIterations = 4;//4;
// infoGlobal.m_solverMode|=SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS|SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION;
//infoGlobal.m_solverMode|=SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS;
infoGlobal.m_solverMode|=SOLVER_USE_2_FRICTION_DIRECTIONS;
// infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS|B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION;
//infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS;
infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS;
//if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
//if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
//if (infoGlobal.m_solverMode & B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
//if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
solveGroup(bodies,inertias,numBodies,contacts,numContacts,constraints,numConstraints,infoGlobal);
@@ -344,7 +343,7 @@ void b3PgsJacobiSolver::resolveSplitPenetrationImpulseCacheFriendly(
{
if (c.m_rhsPenetration)
{
gNumSplitImpulseRecoveries2++;
m_numSplitImpulseRecoveries++;
b3Scalar deltaImpulse = c.m_rhsPenetration-b3Scalar(c.m_appliedPushImpulse)*c.m_cfm;
const b3Scalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
const b3Scalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
@@ -372,7 +371,7 @@ void b3PgsJacobiSolver::resolveSplitPenetrationImpulseCacheFriendly(
if (!c.m_rhsPenetration)
return;
gNumSplitImpulseRecoveries2++;
m_numSplitImpulseRecoveries++;
__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
@@ -804,7 +803,7 @@ void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyCL* bodies, b3InertiaC
///warm starting (or zero if disabled)
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING)
{
solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (rb0)
@@ -883,7 +882,7 @@ void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyCL* bodies, b3I
{
b3SolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING)
{
frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
if (bodies[bodyA->m_originalBodyIndex].m_invMass)
@@ -896,10 +895,10 @@ void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyCL* bodies, b3I
}
}
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
{
b3SolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING)
{
frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
if (bodies[bodyA->m_originalBodyIndex].m_invMass)
@@ -1003,24 +1002,24 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, b3InertiaCL* inert
///based on the relative linear velocity.
///If the relative velocity it zero, it will automatically compute a friction direction.
///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
///You can also enable two friction directions, using the B3_SOLVER_USE_2_FRICTION_DIRECTIONS.
///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
///
///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
///If you choose B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
///
///The user can manually override the friction directions for certain contacts using a contact callback,
///and set the cp.m_lateralFrictionInitialized to true
///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
///this will give a conveyor belt effect
///
if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
if (!(infoGlobal.m_solverMode & B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
{
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
b3Scalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
if (!(infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > B3_EPSILON)
{
cp.m_lateralFrictionDir1 *= 1.f/b3Sqrt(lat_rel_vel);
if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
if((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
{
cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
cp.m_lateralFrictionDir2.normalize();//??
@@ -1034,14 +1033,14 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, b3InertiaCL* inert
{
b3PlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
{
addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
{
cp.m_lateralFrictionInitialized = true;
}
@@ -1051,7 +1050,7 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, b3InertiaCL* inert
{
addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
setFrictionConstraintImpulse( bodies,inertias,solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
@@ -1219,8 +1218,8 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
for ( j=0;j<info1.m_numConstraintRows;j++)
{
memset(&currentConstraintRow[j],0,sizeof(b3SolverConstraint));
currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
currentConstraintRow[j].m_lowerLimit = -B3_INFINITY;
currentConstraintRow[j].m_upperLimit = B3_INFINITY;
currentConstraintRow[j].m_appliedImpulse = 0.f;
currentConstraintRow[j].m_appliedPushImpulse = 0.f;
currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
@@ -1300,8 +1299,8 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
sum += iMJlB.dot(solverConstraint.m_contactNormal);
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
b3Scalar fsum = b3Fabs(sum);
b3Assert(fsum > SIMD_EPSILON);
solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?b3Scalar(1.)/sum : 0.f;
b3Assert(fsum > B3_EPSILON);
solverConstraint.m_jacDiagABInv = fsum>B3_EPSILON?b3Scalar(1.)/sum : 0.f;
}
@@ -1351,7 +1350,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2);
else
m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
@@ -1385,7 +1384,7 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
int numConstraintPool = m_tmpSolverContactConstraintPool.size();
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
if (infoGlobal.m_solverMode & B3_SOLVER_RANDMIZE_ORDER)
{
if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
{
@@ -1417,7 +1416,7 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
}
}
if (infoGlobal.m_solverMode & SOLVER_SIMD)
if (infoGlobal.m_solverMode & B3_SOLVER_SIMD)
{
///solve all joint constraints, using SIMD, if available
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
@@ -1431,10 +1430,10 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
{
///solve all contact constraints using SIMD, if available
if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
if (infoGlobal.m_solverMode & B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
int multiplier = (infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
for (int c=0;c<numPoolConstraints;c++)
{
@@ -1461,7 +1460,7 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
}
}
if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
if (infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)
{
b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
@@ -1478,7 +1477,7 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
}
}
else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
else//B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
{
//solve the friction constraints after all contact constraints, don't interleave them
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
@@ -1600,7 +1599,7 @@ void b3PgsJacobiSolver::solveGroupCacheFriendlySplitImpulseIterations(b3TypedCon
int iteration;
if (infoGlobal.m_splitImpulse)
{
if (infoGlobal.m_solverMode & SOLVER_SIMD)
if (infoGlobal.m_solverMode & B3_SOLVER_SIMD)
{
for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
{
@@ -1706,7 +1705,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies,
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int i,j;
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING)
{
for (j=0;j<numPoolConstraints;j++)
{
@@ -1718,7 +1717,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies,
// printf("pt->m_appliedImpulseLateral1 = %f\n", f);
pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
{
pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
}

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

@@ -41,6 +41,9 @@ protected:
void averageVelocities();
int m_maxOverrideNumSolverIterations;
int m_numSplitImpulseRecoveries;
b3Scalar getContactProcessingThreshold(b3Contact4* contact)
{
return 0.02f;
@@ -113,7 +116,7 @@ public:
B3_DECLARE_ALIGNED_ALLOCATOR();
b3PgsJacobiSolver();
b3PgsJacobiSolver(bool usePgs);
virtual ~b3PgsJacobiSolver();
// void solveContacts(int numBodies, b3RigidBodyCL* bodies, b3InertiaCL* inertias, int numContacts, b3Contact4* contacts);

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

@@ -24,7 +24,7 @@ subject to the following restrictions:
b3Point2PointConstraint::b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB)
:b3TypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
:b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
m_flags(0),
m_useSolveConstraintObsolete(false)
{
@@ -33,7 +33,7 @@ m_useSolveConstraintObsolete(false)
/*
b3Point2PointConstraint::b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA)
:b3TypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
:b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
m_flags(0),
m_useSolveConstraintObsolete(false)
{
@@ -197,7 +197,7 @@ void b3Point2PointConstraint::setParam(int num, b3Scalar value, int axis)
///return the local value of parameter
b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const
{
b3Scalar retVal(SIMD_INFINITY);
b3Scalar retVal(B3_INFINITY);
if(axis != -1)
{
b3AssertConstrParams(0);

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

@@ -51,7 +51,7 @@ enum b3Point2PointFlags
};
/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
ATTRIBUTE_ALIGNED16(class) b3Point2PointConstraint : public b3TypedConstraint
B3_ATTRIBUTE_ALIGNED16(class) b3Point2PointConstraint : public b3TypedConstraint
{
#ifdef IN_PARALLELL_SOLVER
public:
@@ -141,14 +141,14 @@ struct b3Point2PointConstraintDoubleData
};
/*
SIMD_FORCE_INLINE int b3Point2PointConstraint::calculateSerializeBufferSize() const
B3_FORCE_INLINE int b3Point2PointConstraint::calculateSerializeBufferSize() const
{
return sizeof(b3Point2PointConstraintData);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* b3Point2PointConstraint::serialize(void* dataBuffer, b3Serializer* serializer) const
B3_FORCE_INLINE const char* b3Point2PointConstraint::serialize(void* dataBuffer, b3Serializer* serializer) const
{
b3Point2PointConstraintData* p2pData = (b3Point2PointConstraintData*)dataBuffer;

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

@@ -33,17 +33,17 @@ class b3RigidBody;
struct b3SimdScalar
{
SIMD_FORCE_INLINE b3SimdScalar()
B3_FORCE_INLINE b3SimdScalar()
{
}
SIMD_FORCE_INLINE b3SimdScalar(float fl)
B3_FORCE_INLINE b3SimdScalar(float fl)
:m_vec128 (_mm_set1_ps(fl))
{
}
SIMD_FORCE_INLINE b3SimdScalar(__m128 v128)
B3_FORCE_INLINE b3SimdScalar(__m128 v128)
:m_vec128(v128)
{
}
@@ -54,31 +54,31 @@ struct b3SimdScalar
int m_ints[4];
b3Scalar m_unusedPadding;
};
SIMD_FORCE_INLINE __m128 get128()
B3_FORCE_INLINE __m128 get128()
{
return m_vec128;
}
SIMD_FORCE_INLINE const __m128 get128() const
B3_FORCE_INLINE const __m128 get128() const
{
return m_vec128;
}
SIMD_FORCE_INLINE void set128(__m128 v128)
B3_FORCE_INLINE void set128(__m128 v128)
{
m_vec128 = v128;
}
SIMD_FORCE_INLINE operator __m128()
B3_FORCE_INLINE operator __m128()
{
return m_vec128;
}
SIMD_FORCE_INLINE operator const __m128() const
B3_FORCE_INLINE operator const __m128() const
{
return m_vec128;
}
SIMD_FORCE_INLINE operator float() const
B3_FORCE_INLINE operator float() const
{
return m_floats[0];
}
@@ -86,14 +86,14 @@ struct b3SimdScalar
};
///@brief Return the elementwise product of two b3SimdScalar
SIMD_FORCE_INLINE b3SimdScalar
B3_FORCE_INLINE b3SimdScalar
operator*(const b3SimdScalar& v1, const b3SimdScalar& v2)
{
return b3SimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
}
///@brief Return the elementwise product of two b3SimdScalar
SIMD_FORCE_INLINE b3SimdScalar
B3_FORCE_INLINE b3SimdScalar
operator+(const b3SimdScalar& v1, const b3SimdScalar& v2)
{
return b3SimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
@@ -105,7 +105,7 @@ operator+(const b3SimdScalar& v1, const b3SimdScalar& v2)
#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
B3_ATTRIBUTE_ALIGNED64 (struct) b3SolverBody
{
B3_DECLARE_ALIGNED_ALLOCATOR();
b3Transform m_worldTransform;
@@ -136,7 +136,7 @@ ATTRIBUTE_ALIGNED64 (struct) b3SolverBody
return m_worldTransform;
}
SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const
B3_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const
{
if (m_originalBody)
velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
@@ -144,7 +144,7 @@ ATTRIBUTE_ALIGNED64 (struct) b3SolverBody
velocity.setValue(0,0,0);
}
SIMD_FORCE_INLINE void getAngularVelocity(b3Vector3& angVel) const
B3_FORCE_INLINE void getAngularVelocity(b3Vector3& angVel) const
{
if (m_originalBody)
angVel =m_angularVelocity+m_deltaAngularVelocity;
@@ -154,7 +154,7 @@ ATTRIBUTE_ALIGNED64 (struct) b3SolverBody
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
SIMD_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude)
B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude)
{
if (m_originalBody)
{
@@ -163,7 +163,7 @@ ATTRIBUTE_ALIGNED64 (struct) b3SolverBody
}
}
SIMD_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,b3Scalar impulseMagnitude)
B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,b3Scalar impulseMagnitude)
{
if (m_originalBody)
{
@@ -233,19 +233,19 @@ ATTRIBUTE_ALIGNED64 (struct) b3SolverBody
return m_turnVelocity;
}
SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const
B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const
{
velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
}
SIMD_FORCE_INLINE void internalGetAngularVelocity(b3Vector3& angVel) const
B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3& angVel) const
{
angVel = m_angularVelocity+m_deltaAngularVelocity;
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
SIMD_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude)
B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude)
{
if (m_originalBody)
{

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

@@ -27,7 +27,7 @@ class b3RigidBody;
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
ATTRIBUTE_ALIGNED16 (struct) b3SolverConstraint
B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverConstraint
{
B3_DECLARE_ALIGNED_ALLOCATOR();

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

@@ -18,7 +18,7 @@ subject to the following restrictions:
//#include "Bullet3Common/b3Serializer.h"
#define DEFAULT_DEBUGDRAW_SIZE b3Scalar(0.3f)
#define B3_DEFAULT_DEBUGDRAW_SIZE b3Scalar(0.3f)
@@ -26,14 +26,14 @@ b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA,int rbB
:b3TypedObject(type),
m_userConstraintType(-1),
m_userConstraintId(-1),
m_breakingImpulseThreshold(SIMD_INFINITY),
m_breakingImpulseThreshold(B3_INFINITY),
m_isEnabled(true),
m_needsFeedback(false),
m_overrideNumSolverIterations(-1),
m_rbA(rbA),
m_rbB(rbB),
m_appliedImpulse(b3Scalar(0.)),
m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
m_dbgDrawSize(B3_DEFAULT_DEBUGDRAW_SIZE),
m_jointFeedback(0)
{
}

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

@@ -25,15 +25,15 @@ class b3Serializer;
//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
enum b3TypedConstraintType
{
POINT2POINT_CONSTRAINT_TYPE=3,
HINGE_CONSTRAINT_TYPE,
CONETWIST_CONSTRAINT_TYPE,
D6_CONSTRAINT_TYPE,
SLIDER_CONSTRAINT_TYPE,
CONTACT_CONSTRAINT_TYPE,
D6_SPRING_CONSTRAINT_TYPE,
GEAR_CONSTRAINT_TYPE,
MAX_CONSTRAINT_TYPE
B3_POINT2POINT_CONSTRAINT_TYPE=3,
B3_HINGE_CONSTRAINT_TYPE,
B3_CONETWIST_CONSTRAINT_TYPE,
B3_D6_CONSTRAINT_TYPE,
B3_SLIDER_CONSTRAINT_TYPE,
B3_CONTACT_CONSTRAINT_TYPE,
B3_D6_SPRING_CONSTRAINT_TYPE,
B3_GEAR_CONSTRAINT_TYPE,
B3_MAX_CONSTRAINT_TYPE
};
@@ -52,7 +52,7 @@ enum b3ConstraintParams
#endif
ATTRIBUTE_ALIGNED16(struct) b3JointFeedback
B3_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 b3TypedObject
B3_ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public b3TypedObject
{
int m_userConstraintType;
@@ -330,9 +330,9 @@ public:
};
// 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 b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar angleLowerLimitInRadians, b3Scalar angleUpperLimitInRadians)
// returns angle in range [-B3_2_PI, B3_2_PI], closest to one of the limits
// all arguments should be normalized angles (i.e. in range [-B3_PI, B3_PI])
B3_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar angleLowerLimitInRadians, b3Scalar angleUpperLimitInRadians)
{
if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
{
@@ -342,13 +342,13 @@ SIMD_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scal
{
b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleLowerLimitInRadians - angleInRadians));
b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleUpperLimitInRadians - angleInRadians));
return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
return (diffLo < diffHi) ? angleInRadians : (angleInRadians + B3_2_PI);
}
else if(angleInRadians > angleUpperLimitInRadians)
{
b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleInRadians - angleUpperLimitInRadians));
b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleInRadians - angleLowerLimitInRadians));
return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
return (diffLo < diffHi) ? (angleInRadians - B3_2_PI) : angleInRadians;
}
else
{
@@ -379,7 +379,7 @@ struct b3TypedConstraintData
};
/*SIMD_FORCE_INLINE int b3TypedConstraint::calculateSerializeBufferSize() const
/*B3_FORCE_INLINE int b3TypedConstraint::calculateSerializeBufferSize() const
{
return sizeof(b3TypedConstraintData);
}