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

Merge remote-tracking branch 'remotes/bulletphysics/master'

Browse Source 
# Conflicts:
#	src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h
This commit is contained in:
nicolaichuk
2017-03-23 14:28:49 +03:00
parent 8e1b146568 59fcf7ca24
commit 5527e36139
155 changed files with 10273 additions and 5625 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
src/Bullet3Common/b3Quaternion.h
View File

@@ -126,15 +126,16 @@ public:
sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
}
/**@brief Set the quaternion using euler angles
/**@brief Set the quaternion using euler angles
* @param yaw Angle around Z
* @param pitch Angle around Y
* @param roll Angle around X */
void setEulerZYX(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
void setEulerZYX(const b3Scalar& yawZ, const b3Scalar& pitchY, const b3Scalar& rollX)
{
b3Scalar halfYaw = b3Scalar(yaw) * b3Scalar(0.5);
b3Scalar halfPitch = b3Scalar(pitch) * b3Scalar(0.5);
b3Scalar halfRoll = b3Scalar(roll) * b3Scalar(0.5);
b3Scalar halfYaw = b3Scalar(yawZ) * b3Scalar(0.5);
b3Scalar halfPitch = b3Scalar(pitchY) * b3Scalar(0.5);
b3Scalar halfRoll = b3Scalar(rollX) * b3Scalar(0.5);
b3Scalar cosYaw = b3Cos(halfYaw);
b3Scalar sinYaw = b3Sin(halfYaw);
b3Scalar cosPitch = b3Cos(halfPitch);
@@ -145,7 +146,30 @@ public:
cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
normalize();
}
/**@brief Get the euler angles from this quaternion
* @param yaw Angle around Z
* @param pitch Angle around Y
* @param roll Angle around X */
void getEulerZYX(b3Scalar& yawZ, b3Scalar& pitchY, b3Scalar& rollX) const
{
b3Scalar squ;
b3Scalar sqx;
b3Scalar sqy;
b3Scalar sqz;
b3Scalar sarg;
sqx = m_floats[0] * m_floats[0];
sqy = m_floats[1] * m_floats[1];
sqz = m_floats[2] * m_floats[2];
squ = m_floats[3] * m_floats[3];
rollX = b3Atan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz);
sarg = b3Scalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]);
pitchY = sarg <= b3Scalar(-1.0) ? b3Scalar(-0.5) * B3_PI: (sarg >= b3Scalar(1.0) ? b3Scalar(0.5) * B3_PI : b3Asin(sarg));
yawZ = b3Atan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz);
}
/**@brief Add two quaternions
* @param q The quaternion to add to this one */
B3_FORCE_INLINE b3Quaternion& operator+=(const b3Quaternion& q)

2
src/Bullet3Common/b3Scalar.h
View File

@@ -71,6 +71,7 @@ inline int b3GetVersion()
#else
#if (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (B3_USE_DOUBLE_PRECISION))
#if (defined (_M_IX86) || defined (_M_X64))
#define B3_USE_SSE
#ifdef B3_USE_SSE
//B3_USE_SSE_IN_API is disabled under Windows by default, because
@@ -82,6 +83,7 @@ inline int b3GetVersion()
//#define B3_USE_SSE_IN_API
#endif //B3_USE_SSE
#include <emmintrin.h>
#endif
#endif
#endif//_XBOX

11
src/BulletCollision/BroadphaseCollision/btDbvt.h
View File

@@ -942,7 +942,13 @@ inline void btDbvt::collideTV( const btDbvtNode* root,
ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol);
btAlignedObjectArray<const btDbvtNode*> stack;
stack.resize(0);
#ifndef BT_DISABLE_STACK_TEMP_MEMORY
char tempmemory[SIMPLE_STACKSIZE*sizeof(const btDbvtNode*)];
stack.initializeFromBuffer(tempmemory, 0, SIMPLE_STACKSIZE);
#else
stack.reserve(SIMPLE_STACKSIZE);
#endif //BT_DISABLE_STACK_TEMP_MEMORY
stack.push_back(root);
do {
const btDbvtNode* n=stack[stack.size()-1];
@@ -1078,7 +1084,12 @@ inline void btDbvt::rayTest( const btDbvtNode* root,
int depth=1;
int treshold=DOUBLE_STACKSIZE-2;
char tempmemory[DOUBLE_STACKSIZE * sizeof(const btDbvtNode*)];
#ifndef BT_DISABLE_STACK_TEMP_MEMORY
stack.initializeFromBuffer(tempmemory, DOUBLE_STACKSIZE, DOUBLE_STACKSIZE);
#else//BT_DISABLE_STACK_TEMP_MEMORY
stack.resize(DOUBLE_STACKSIZE);
#endif //BT_DISABLE_STACK_TEMP_MEMORY
stack[0]=root;
btVector3 bounds[2];
do {

1
src/BulletCollision/CollisionDispatch/btCollisionObject.h
View File

@@ -139,6 +139,7 @@ public:
CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing
CF_HAS_CONTACT_STIFFNESS_DAMPING = 128,
CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256,
CF_HAS_FRICTION_ANCHOR = 512,
};
enum CollisionObjectTypes

1
src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h
View File

@@ -124,7 +124,6 @@ public:
btCollisionShape* getCollisionShapeByIndex(int index);
int getNumRigidBodies() const;
btCollisionObject* getRigidBodyByIndex(int index) const;
//int getNumConstraints() const;
int getNumBvhs() const;
btOptimizedBvh* getBvhByIndex(int index) const;

8
src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
View File

@@ -146,7 +146,13 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
newPt.m_combinedContactStiffness1 = calculateCombinedContactStiffness(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING;
}
if ( (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_FRICTION_ANCHOR) ||
(m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_FRICTION_ANCHOR))
{
newPt.m_contactPointFlags |= BT_CONTACT_FLAG_FRICTION_ANCHOR;
}
btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2);

3
src/BulletCollision/CollisionShapes/btBox2dShape.h
View File

@@ -103,11 +103,12 @@ public:
btScalar minDimension = boxHalfExtents.getX();
if (minDimension>boxHalfExtents.getY())
minDimension = boxHalfExtents.getY();
setSafeMargin(minDimension);
m_shapeType = BOX_2D_SHAPE_PROXYTYPE;
btVector3 margin(getMargin(),getMargin(),getMargin());
m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin;
setSafeMargin(minDimension);
};
virtual void setMargin(btScalar collisionMargin)

4
src/BulletCollision/CollisionShapes/btBoxShape.cpp
View File

@@ -19,10 +19,10 @@ btBoxShape::btBoxShape( const btVector3& boxHalfExtents)
{
m_shapeType = BOX_SHAPE_PROXYTYPE;
setSafeMargin(boxHalfExtents);
btVector3 margin(getMargin(),getMargin(),getMargin());
m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin;
setSafeMargin(boxHalfExtents);
};

5
src/BulletCollision/CollisionShapes/btCylinderShape.cpp
View File

@@ -19,10 +19,11 @@ btCylinderShape::btCylinderShape (const btVector3& halfExtents)
:btConvexInternalShape(),
m_upAxis(1)
{
setSafeMargin(halfExtents);
btVector3 margin(getMargin(),getMargin(),getMargin());
m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin;
setSafeMargin(halfExtents);
m_shapeType = CYLINDER_SHAPE_PROXYTYPE;
}

1
src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
View File

@@ -41,6 +41,7 @@ enum btContactPointFlags
BT_CONTACT_FLAG_HAS_CONTACT_CFM=2,
BT_CONTACT_FLAG_HAS_CONTACT_ERP=4,
BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING = 8,
BT_CONTACT_FLAG_FRICTION_ANCHOR = 16,
};
/// ManifoldContactPoint collects and maintains persistent contactpoints.

1
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
View File

@@ -281,6 +281,7 @@ void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btT
removeContactPoint(i);
} else
{
//todo: friction anchor may require the contact to be around a bit longer
//contact also becomes invalid when relative movement orthogonal to normal exceeds margin
projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;

55
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
View File

@@ -185,39 +185,54 @@ public:
gContactEndedCallback(this);
}
}
void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex)
void replaceContactPoint(const btManifoldPoint& newPoint, int insertIndex)
{
btAssert(validContactDistance(newPoint));
#define MAINTAIN_PERSISTENCY 1
#ifdef MAINTAIN_PERSISTENCY
int lifeTime = m_pointCache[insertIndex].getLifeTime();
btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
// bool isLateralFrictionInitialized = m_pointCache[insertIndex].m_lateralFrictionInitialized;
btAssert(lifeTime>=0);
void* cache = m_pointCache[insertIndex].m_userPersistentData;
m_pointCache[insertIndex] = newPoint;
m_pointCache[insertIndex].m_userPersistentData = cache;
m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
int lifeTime = m_pointCache[insertIndex].getLifeTime();
btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
bool replacePoint = true;
///we keep existing contact points for friction anchors
///if the friction force is within the Coulomb friction cone
if (newPoint.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
{
// printf("appliedImpulse=%f\n", appliedImpulse);
// printf("appliedLateralImpulse1=%f\n", appliedLateralImpulse1);
// printf("appliedLateralImpulse2=%f\n", appliedLateralImpulse2);
// printf("mu = %f\n", m_pointCache[insertIndex].m_combinedFriction);
btScalar mu = m_pointCache[insertIndex].m_combinedFriction;
btScalar eps = 0; //we could allow to enlarge or shrink the tolerance to check against the friction cone a bit, say 1e-7
btScalar a = appliedLateralImpulse1 * appliedLateralImpulse1 + appliedLateralImpulse2 * appliedLateralImpulse2;
btScalar b = eps + mu * appliedImpulse;
b = b * b;
replacePoint = (a) > (b);
}
if (replacePoint)
{
btAssert(lifeTime >= 0);
void* cache = m_pointCache[insertIndex].m_userPersistentData;
m_pointCache[insertIndex] = newPoint;
m_pointCache[insertIndex].m_userPersistentData = cache;
m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
}
m_pointCache[insertIndex].m_lifeTime = lifeTime;
#else
clearUserCache(m_pointCache[insertIndex]);
m_pointCache[insertIndex] = newPoint;
#endif
}
bool validContactDistance(const btManifoldPoint& pt) const
{
return pt.m_distance1 <= getContactBreakingThreshold();

9
src/BulletDynamics/Character/btKinematicCharacterController.cpp
View File

@@ -137,8 +137,6 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho
m_ghostObject = ghostObject;
m_up.setValue(0.0f, 0.0f, 1.0f);
m_jumpAxis.setValue(0.0f, 0.0f, 1.0f);
setUp(up);
setStepHeight(stepHeight);
m_addedMargin = 0.02;
m_walkDirection.setValue(0.0,0.0,0.0);
m_AngVel.setValue(0.0, 0.0, 0.0);
@@ -156,13 +154,16 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho
m_wasOnGround = false;
m_wasJumping = false;
m_interpolateUp = true;
setMaxSlope(btRadians(45.0));
m_currentStepOffset = 0.0;
m_maxPenetrationDepth = 0.2;
full_drop = false;
bounce_fix = false;
m_linearDamping = btScalar(0.0);
m_angularDamping = btScalar(0.0);
setUp(up);
setStepHeight(stepHeight);
setMaxSlope(btRadians(45.0));
}
btKinematicCharacterController::~btKinematicCharacterController ()
@@ -657,7 +658,7 @@ void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity
if (c != 0)
{
//there is a component in walkdirection for vertical velocity
btVector3 upComponent = m_up * (sinf(SIMD_HALF_PI - acosf(c)) * m_walkDirection.length());
btVector3 upComponent = m_up * (btSin(SIMD_HALF_PI - btAcos(c)) * m_walkDirection.length());
m_walkDirection -= upComponent;
m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length();

4
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
View File

@@ -642,7 +642,7 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
btTransform trDeltaAB = trB * trPose * trA.inverse();
btQuaternion qDeltaAB = trDeltaAB.getRotation();
btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
float swingAxisLen2 = swingAxis.length2();
btScalar swingAxisLen2 = swingAxis.length2();
if(btFuzzyZero(swingAxisLen2))
{
return;
@@ -903,7 +903,7 @@ btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btSc
// a^2 b^2
// Do the math and it should be clear.
float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
btScalar swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
if (fabs(xEllipse) > SIMD_EPSILON)
{
btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);

13
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View File

@@ -43,10 +43,13 @@ struct btContactSolverInfoData
btScalar m_restitution;
int m_numIterations;
btScalar m_maxErrorReduction;
btScalar m_sor;
btScalar m_erp;//used as Baumgarte factor
btScalar m_erp2;//used in Split Impulse
btScalar m_globalCfm;//constraint force mixing
btScalar m_sor;//successive over-relaxation term
btScalar m_erp;//error reduction for non-contact constraints
btScalar m_erp2;//error reduction for contact constraints
btScalar m_globalCfm;//constraint force mixing for contacts and non-contacts
btScalar m_frictionERP;//error reduction for friction constraints
btScalar m_frictionCFM;//constraint force mixing for friction constraints
int m_splitImpulse;
btScalar m_splitImpulsePenetrationThreshold;
btScalar m_splitImpulseTurnErp;
@@ -79,6 +82,8 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_erp = btScalar(0.2);
m_erp2 = btScalar(0.2);
m_globalCfm = btScalar(0.);
m_frictionERP = btScalar(0.2);//positional friction 'anchors' are disabled by default
m_frictionCFM = btScalar(0.);
m_sor = btScalar(1.);
m_splitImpulse = true;
m_splitImpulsePenetrationThreshold = -.04f;

34
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -534,7 +534,7 @@ void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionOb
void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
{
@@ -612,7 +612,17 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
btScalar velocityError = desiredVelocity - rel_vel;
btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = velocityImpulse;
btScalar penetrationImpulse = btScalar(0);
if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
{
btScalar distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(normalAxis);
btScalar positionalError = -distance * infoGlobal.m_frictionERP/infoGlobal.m_timeStep;
penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
}
solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
@@ -621,12 +631,12 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
}
}
btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
{
btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
solverConstraint.m_frictionIndex = frictionIndex;
setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
return solverConstraint;
}
@@ -1168,6 +1178,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
///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_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
{
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
@@ -1177,7 +1188,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,infoGlobal);
if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
@@ -1185,7 +1196,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
cp.m_lateralFrictionDir2.normalize();//??
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
}
} else
@@ -1194,13 +1205,13 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
}
@@ -1212,10 +1223,10 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
} else
{
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_frictionCFM);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_frictionCFM);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
}
setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
@@ -1526,7 +1537,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
btScalar fsum = btFabs(sum);
btAssert(fsum > SIMD_EPSILON);
solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f;
btScalar sorRelaxation = 1.f;//todo: get from globalInfo?
solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?sorRelaxation/sum : 0.f;
}

3
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View File

@@ -62,6 +62,7 @@ protected:
void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
const btContactSolverInfo& infoGlobal,
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
void setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
@@ -69,7 +70,7 @@ protected:
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar torsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);

2
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -1238,7 +1238,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const
void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, btScalar result[6]) const
{
int num_links = getNumLinks();
///solve I * x = rhs, so the result = invI * rhs

2
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -616,7 +616,7 @@ private:
void operator=(const btMultiBody &); // not implemented
void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;
void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, btScalar result[6]) const;
void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
void updateLinksDofOffsets()

117
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -48,9 +48,11 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
}
//solve featherstone normal contact
for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++)
for (int j0=0;j0<m_multiBodyNormalContactConstraints.size();j0++)
{
btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
int index = j0;//iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0;
btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[index];
btScalar residual = 0.f;
if (iteration < infoGlobal.m_numIterations)
@@ -68,11 +70,13 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
//solve featherstone frictional contact
for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++)
for (int j1=0;j1<this->m_multiBodyFrictionContactConstraints.size();j1++)
{
if (iteration < infoGlobal.m_numIterations)
{
btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j];
int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
//adjust friction limits here
if (totalImpulse>btScalar(0))
@@ -240,32 +244,39 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
//cfm = 1 / ( dt * kp + kd )
//erp = dt * kp / ( dt * kp + kd )
btScalar cfm = infoGlobal.m_globalCfm;
btScalar erp = infoGlobal.m_erp2;
if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
{
if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
cfm = cp.m_contactCFM;
if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
erp = cp.m_contactERP;
} else
{
if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
{
btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
if (denom < SIMD_EPSILON)
{
denom = SIMD_EPSILON;
}
cfm = btScalar(1) / denom;
erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
}
}
cfm *= invTimeStep;
btScalar cfm;
btScalar erp;
if (isFriction)
{
cfm = infoGlobal.m_frictionCFM;
erp = infoGlobal.m_frictionERP;
} else
{
cfm = infoGlobal.m_globalCfm;
erp = infoGlobal.m_erp2;
if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
{
if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
cfm = cp.m_contactCFM;
if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
erp = cp.m_contactERP;
} else
{
if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
{
btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
if (denom < SIMD_EPSILON)
{
denom = SIMD_EPSILON;
}
cfm = btScalar(1) / denom;
erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
}
}
}
cfm *= invTimeStep;
if (multiBodyA)
{
@@ -425,8 +436,19 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
btScalar restitution = 0.f;
btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
btScalar distance = 0;
if (!isFriction)
{
distance = cp.getDistance()+infoGlobal.m_linearSlop;
} else
{
if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
{
distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal);
}
}
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
@@ -521,15 +543,20 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
btScalar positionalError = 0.f;
btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
if (penetration>0)
if (isFriction)
{
positionalError = 0;
velocityError -= penetration / infoGlobal.m_timeStep;
positionalError = -distance * erp/infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
if (distance>0)
{
positionalError = 0;
velocityError -= distance / infoGlobal.m_timeStep;
} else
{
positionalError = -distance * erp/infoGlobal.m_timeStep;
}
}
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
@@ -556,7 +583,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
}
else
{
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction;
@@ -1024,12 +1051,18 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
if (rollingFriction > 0)
if (rollingFriction > 0 )
{
addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal);
addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
if (cp.m_combinedSpinningFriction>0)
{
addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal);
}
if (cp.m_combinedRollingFriction>0)
{
addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
}
rollingFriction--;
}

12
src/BulletSoftBody/btDefaultSoftBodySolver.cpp
View File

@@ -100,9 +100,9 @@ void btDefaultSoftBodySolver::copySoftBodyToVertexBuffer( const btSoftBody *cons
for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex )
{
btVector3 position = clothVertices[vertexIndex].m_x;
*(vertexPointer + 0) = position.getX();
*(vertexPointer + 1) = position.getY();
*(vertexPointer + 2) = position.getZ();
*(vertexPointer + 0) = (float)position.getX();
*(vertexPointer + 1) = (float)position.getY();
*(vertexPointer + 2) = (float)position.getZ();
vertexPointer += vertexStride;
}
}
@@ -115,9 +115,9 @@ void btDefaultSoftBodySolver::copySoftBodyToVertexBuffer( const btSoftBody *cons
for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex )
{
btVector3 normal = clothVertices[vertexIndex].m_n;
*(normalPointer + 0) = normal.getX();
*(normalPointer + 1) = normal.getY();
*(normalPointer + 2) = normal.getZ();
*(normalPointer + 0) = (float)normal.getX();
*(normalPointer + 1) = (float)normal.getY();
*(normalPointer + 2) = (float)normal.getZ();
normalPointer += normalStride;
}
}

10
src/LinearMath/btQuickprof.cpp
View File

@@ -218,7 +218,7 @@ unsigned long long int btClock::getTimeNanoseconds()
QueryPerformanceCounter(&currentTime);
elapsedTime.QuadPart = currentTime.QuadPart -
m_data->mStartTime.QuadPart;
elapsedTime.QuadPart *= 1e9;
elapsedTime.QuadPart *= 1000000000;
elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart;
return (unsigned long long) elapsedTime.QuadPart;
@@ -287,7 +287,7 @@ static btClock gProfileClock;
inline void Profile_Get_Ticks(unsigned long int * ticks)
{
*ticks = gProfileClock.getTimeMicroseconds();
*ticks = (unsigned long int)gProfileClock.getTimeMicroseconds();
}
inline float Profile_Get_Tick_Rate(void)
@@ -687,7 +687,11 @@ unsigned int btQuickprofGetCurrentThreadIndex2()
{
const unsigned int kNullIndex = ~0U;
#ifdef _WIN32
__declspec( thread ) static unsigned int sThreadIndex = kNullIndex;
#if defined(__MINGW32__) || defined(__MINGW64__)
static __thread unsigned int sThreadIndex = kNullIndex;
#else
__declspec( thread ) static unsigned int sThreadIndex = kNullIndex;
#endif
#else
#ifdef __APPLE__
#if TARGET_OS_IPHONE

1003
src/LinearMath/btScalar.h
View File

File diff suppressed because it is too large Load Diff