blender
/
blender
133
398
Fork 579
Code Issues 6.3k Pull Requests 747 Projects 19 Wiki Activity

Bullet: Update to svn r2719 

Fixes part of T37905, fixed constraint didn't work correctly.
This commit is contained in:
Sergej Reich 2013-12-26 12:41:52 +01:00
parent 709041ed0b
commit 05eebf49d3
18 changed files with 450 additions and 295 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp vendored
View File

@ -770,7 +770,7 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,
hitPointLocal,
hitFraction);
bool normalInWorldSpace = false;
bool normalInWorldSpace = true;
return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
}

1
extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp vendored
View File

@ -229,6 +229,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
removeChildAlgorithms();
preallocateChildAlgorithms(body0Wrap,body1Wrap);
m_compoundShapeRevision = compoundShape->getUpdateRevision();
}

1
extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h vendored
View File

@ -36,6 +36,7 @@ extern btShapePairCallback gCompoundChildShapePairCallback;
/// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes
class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
protected:
btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;
bool m_isSwapped;

16
extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp vendored
View File

@ -27,10 +27,8 @@ subject to the following restrictions:
btShapePairCallback gCompoundCompoundChildShapePairCallback = 0;
btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
:btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
m_sharedManifold(ci.m_manifold)
:btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped)
{
m_ownsManifold = false;
void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16);
m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache();
@ -292,12 +290,21 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
if (!tree0 || !tree1)
{
return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut);
}
///btCompoundShape might have changed:
////make sure the internal child collision algorithm caches are still valid
if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1))
{
///clear all
removeChildAlgorithms();
m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
}
@ -329,8 +336,7 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
}
const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold);

7
extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h vendored
View File

@ -17,6 +17,8 @@ subject to the following restrictions:
#ifndef BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
#define BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
#include "btCompoundCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
@ -35,15 +37,12 @@ typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCol
extern btShapePairCallback gCompoundCompoundChildShapePairCallback;
/// btCompoundCompoundCollisionAlgorithm supports collision between two btCompoundCollisionShape shapes
class btCompoundCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm
class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm
{
class btHashedSimplePairCache* m_childCollisionAlgorithmCache;
btSimplePairArray m_removePairs;
class btPersistentManifold* m_sharedManifold;
bool m_ownsManifold;
int m_compoundShapeRevision0;//to keep track of changes, so that childAlgorithm array can be updated
int m_compoundShapeRevision1;

3
extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h vendored
View File

@ -33,7 +33,8 @@ class btDispatcher;
enum btConstraintSolverType
{
BT_SEQUENTIAL_IMPULSE_SOLVER=1,
BT_MLCP_SOLVER=2
BT_MLCP_SOLVER=2,
BT_NNCG_SOLVER=4
};
class btConstraintSolver

139
extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp vendored
View File

@ -23,9 +23,8 @@ subject to the following restrictions:
btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB)
{
m_pivotInA = frameInA.getOrigin();
m_pivotInB = frameInB.getOrigin();
m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
m_frameInA = frameInA;
m_frameInB = frameInB;
}
@ -37,14 +36,16 @@ btFixedConstraint::~btFixedConstraint ()
void btFixedConstraint::getInfo1 (btConstraintInfo1* info)
{
info->m_numConstraintRows = 6;
info->nub = 6;
info->nub = 0;
}
void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
{
//fix the 3 linear degrees of freedom
const btTransform& transA = m_rbA.getCenterOfMassTransform();
const btTransform& transB = m_rbB.getCenterOfMassTransform();
const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
@ -55,15 +56,15 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
info->m_J1linearAxis[info->rowskip+1] = 1;
info->m_J1linearAxis[2*info->rowskip+2] = 1;
btVector3 a1 = worldOrnA*m_pivotInA;
{
btVector3 a1 = worldOrnA * m_frameInA.getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
btVector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
if (info->m_J2linearAxis)
{
info->m_J2linearAxis[0] = -1;
@ -71,10 +72,8 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
info->m_J2linearAxis[2*info->rowskip+2] = -1;
}
btVector3 a2 = worldOrnB*m_pivotInB;
{
// btVector3 a2n = -a2;
btVector3 a2 = worldOrnB*m_frameInB.getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
@ -88,42 +87,100 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*info->rowskip] = linearError[j];
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
//fix the 3 angular degrees of freedom
btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0);
btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1);
btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2);
btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0);
btVector3 target;
btScalar x = ivB.dot(ivA);
btScalar y = ivB.dot(jvA);
btScalar z = ivB.dot(kvA);
btVector3 swingAxis(0,0,0);
{
if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
{
swingAxis = -ivB.cross(ivA);
}
}
btVector3 vTwist(1,0,0);
int start_row = 3;
int s = info->rowskip;
int start_index = start_row * s;
// compute rotation of A wrt B (in constraint space)
btQuaternion qA = transA.getRotation() * m_frameInA.getRotation();
btQuaternion qB = transB.getRotation() * m_frameInB.getRotation();
btQuaternion qAB = qB.inverse() * qA;
// split rotation into cone and twist
// (all this is done from B's perspective. Maybe I should be averaging axes...)
btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
// 3 rows to make body rotations equal
info->m_J1angularAxis[start_index] = 1;
info->m_J1angularAxis[start_index + s + 1] = 1;
info->m_J1angularAxis[start_index + s*2+2] = 1;
if ( info->m_J2angularAxis)
{
info->m_J2angularAxis[start_index] = -1;
info->m_J2angularAxis[start_index + s+1] = -1;
info->m_J2angularAxis[start_index + s*2+2] = -1;
}
int row = 3;
int srow = row * info->rowskip;
btVector3 ax1;
// angular limits
{
btScalar *J1 = info->m_J1angularAxis;
btScalar *J2 = info->m_J2angularAxis;
btTransform trA = transA*m_frameInA;
btVector3 twistAxis = trA.getBasis().getColumn(0);
// set right hand side for the angular dofs
btVector3 p = trA.getBasis().getColumn(1);
btVector3 q = trA.getBasis().getColumn(2);
int srow1 = srow + info->rowskip;
J1[srow+0] = p[0];
J1[srow+1] = p[1];
J1[srow+2] = p[2];
J1[srow1+0] = q[0];
J1[srow1+1] = q[1];
J1[srow1+2] = q[2];
J2[srow+0] = -p[0];
J2[srow+1] = -p[1];
J2[srow+2] = -p[2];
J2[srow1+0] = -q[0];
J2[srow1+1] = -q[1];
J2[srow1+2] = -q[2];
btScalar fact = info->fps;
info->m_constraintError[srow] = fact * swingAxis.dot(p);
info->m_constraintError[srow1] = fact * swingAxis.dot(q);
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
info->m_lowerLimit[srow1] = -SIMD_INFINITY;
info->m_upperLimit[srow1] = SIMD_INFINITY;
srow = srow1 + info->rowskip;
btVector3 diff;
btScalar angle;
btMatrix3x3 mrelCur = worldOrnA *worldOrnB.inverse();
btQuaternion qrelCur;
mrelCur.getRotation(qrelCur);
btTransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle);
diff*=-angle;
for (j=0; j<3; j++)
{
info->m_constraintError[(3+j)*info->rowskip] = k * diff[j];
}
{
btQuaternion qMinTwist = qABTwist;
btScalar twistAngle = qABTwist.getAngle();
if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
{
qMinTwist = -(qABTwist);
twistAngle = qMinTwist.getAngle();
}
if (twistAngle > SIMD_EPSILON)
{
twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
twistAxis.normalize();
twistAxis = quatRotate(qB, -twistAxis);
}
ax1 = twistAxis;
btScalar *J1 = info->m_J1angularAxis;
btScalar *J2 = info->m_J2angularAxis;
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
btScalar k = info->fps;
info->m_constraintError[srow] = k * twistAngle;
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
}
}

5
extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h vendored
View File

@ -20,10 +20,9 @@ subject to the following restrictions:
ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint
{
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btQuaternion m_relTargetAB;
btTransform m_frameInA;
btTransform m_frameInB;
public:
btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);

34
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp vendored
View File

@ -58,13 +58,13 @@ static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
#endif//USE_SIMD
// Project Gauss Seidel or the equivalent Sequential Impulse
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
#ifdef USE_SIMD
__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
__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)));
btSimdScalar 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_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
@ -86,13 +86,15 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
return deltaImpulse;
#else
resolveSingleConstraintRowGeneric(body1,body2,c);
return resolveSingleConstraintRowGeneric(body1,body2,c);
#endif
}
// Project Gauss Seidel or the equivalent Sequential Impulse
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
@ -120,15 +122,17 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
return deltaImpulse;
}
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
#ifdef USE_SIMD
__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
__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)));
btSimdScalar 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_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
@ -147,13 +151,14 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
return deltaImpulse;
#else
resolveSingleConstraintRowLowerLimit(body1,body2,c);
return resolveSingleConstraintRowLowerLimit(body1,body2,c);
#endif
}
// Projected Gauss Seidel or the equivalent Sequential Impulse
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
@ -173,6 +178,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
}
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
return deltaImpulse;
}
@ -430,6 +437,7 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
btSimdScalar velocityError = desiredVelocity - rel_vel;
btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction;
@ -812,7 +820,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
///avoid collision response between two static objects
if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
return;
int rollingFriction=1;
@ -1452,8 +1460,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
for (j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
}
@ -1472,8 +1479,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
}
}

11
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h vendored
View File

@ -88,13 +88,10 @@ protected:
int getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);
void resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btSimdScalar resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btSimdScalar resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btSimdScalar resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btSimdScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
protected:

28
extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp vendored
View File

@ -19,9 +19,11 @@ subject to the following restrictions:
#include "LinearMath/btQuickprof.h"
#include "btSolveProjectedGaussSeidel.h"
btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver)
:m_solver(solver),
m_fallback(0)
m_fallback(0),
m_cfm(0.000001)//0.0000001
{
}
@ -160,13 +162,17 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
BT_PROFILE("init b (rhs)");
m_b.resize(numConstraintRows);
m_bSplit.resize(numConstraintRows);
//m_b.setZero();
m_b.setZero();
m_bSplit.setZero();
for (int i=0;i<numConstraintRows ;i++)
{
if (m_allConstraintArray[i].m_jacDiagABInv)
btScalar jacDiag = m_allConstraintArray[i].m_jacDiagABInv;
if (!btFuzzyZero(jacDiag))
{
m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
btScalar rhs = m_allConstraintArray[i].m_rhs;
btScalar rhsPenetration = m_allConstraintArray[i].m_rhsPenetration;
m_b[i]=rhs/jacDiag;
m_bSplit[i] = rhsPenetration/jacDiag;
}
}
@ -425,14 +431,12 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
}
}
///todo: use proper cfm values from the constraints (getInfo2)
if (1)
{
// add cfm to the diagonal of m_A
for ( int i=0; i<m_A.rows(); ++i)
{
float cfm = 0.00001f;
m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep);
m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
}
}
@ -473,6 +477,9 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
if (infoGlobal.m_splitImpulse)
m_bSplit.resize(numConstraintRows);
m_bSplit.setZero();
m_b.setZero();
for (int i=0;i<numConstraintRows ;i++)
{
if (m_allConstraintArray[i].m_jacDiagABInv)
@ -554,12 +561,10 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
if (1)
{
///todo: use proper cfm values from the constraints (getInfo2)
// add cfm to the diagonal of m_A
for ( int i=0; i<m_A.rows(); ++i)
{
float cfm = 0.0001f;
m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep);
m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
}
}
@ -618,6 +623,7 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
}
else
{
// printf("m_fallback = %d\n",m_fallback);
m_fallback++;
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
}

10
extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h vendored
View File

@ -42,6 +42,7 @@ protected:
btConstraintArray m_allConstraintArray;
btMLCPSolverInterface* m_solver;
int m_fallback;
btScalar m_cfm;
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
@ -70,6 +71,15 @@ public:
m_fallback = num;
}
btScalar getCfm() const
{
return m_cfm;
}
void setCfm(btScalar cfm)
{
m_cfm = cfm;
}
virtual btConstraintSolverType getSolverType() const
{
return BT_MLCP_SOLVER;

6
extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h vendored
View File

@ -20,11 +20,17 @@ subject to the following restrictions:
#include "btMLCPSolverInterface.h"
///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix)
class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
{
public:
virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
{
if (!A.rows())
return true;
//the A matrix is sparse, so compute the non-zero elements
A.rowComputeNonZeroElements();
//A is a m-n matrix, m rows, n columns
btAssert(A.rows() == b.rows());

3
extern/bullet2/src/BulletSoftBody/btSoftBody.cpp vendored
View File

@ -3027,7 +3027,8 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti)
{
const RContact& c = psb->m_rcontacts[i];
const sCti& cti = c.m_cti;
if (cti.m_colObj->hasContactResponse()) {
if (cti.m_colObj->hasContactResponse())
{
btRigidBody* tmpRigid = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
const btVector3 va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0);
const btVector3 vb = c.m_node->m_x-c.m_node->m_q;

443
extern/bullet2/src/LinearMath/btMatrixX.h vendored
View File

@ -20,6 +20,12 @@ subject to the following restrictions:
#include "LinearMath/btQuickprof.h"
#include "LinearMath/btAlignedObjectArray.h"
//#define BT_DEBUG_OSTREAM
#ifdef BT_DEBUG_OSTREAM
#include <iostream>
#include <iomanip> // std::setw
#endif //BT_DEBUG_OSTREAM
class btIntSortPredicate
{
public:
@ -30,6 +36,121 @@ class btIntSortPredicate
};
template <typename T>
struct btVectorX
{
btAlignedObjectArray<T> m_storage;
btVectorX()
{
}
btVectorX(int numRows)
{
m_storage.resize(numRows);
}
void resize(int rows)
{
m_storage.resize(rows);
}
int cols() const
{
return 1;
}
int rows() const
{
return m_storage.size();
}
int size() const
{
return rows();
}
T nrm2() const
{
T norm = T(0);
int nn = rows();
{
if (nn == 1)
{
norm = btFabs((*this)[0]);
}
else
{
T scale = 0.0;
T ssq = 1.0;
/* The following loop is equivalent to this call to the LAPACK
auxiliary routine: CALL SLASSQ( N, X, INCX, SCALE, SSQ ) */
for (int ix=0;ix<nn;ix++)
{
if ((*this)[ix] != 0.0)
{
T absxi = btFabs((*this)[ix]);
if (scale < absxi)
{
T temp;
temp = scale / absxi;
ssq = ssq * (temp * temp) + 1.0;
scale = absxi;
}
else
{
T temp;
temp = absxi / scale;
ssq += temp * temp;
}
}
}
norm = scale * sqrt(ssq);
}
}
return norm;
}
void setZero()
{
if (m_storage.size())
{
// for (int i=0;i<m_storage.size();i++)
// m_storage[i]=0;
//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
btSetZero(&m_storage[0],m_storage.size());
}
}
const T& operator[] (int index) const
{
return m_storage[index];
}
T& operator[] (int index)
{
return m_storage[index];
}
T* getBufferPointerWritable()
{
return m_storage.size() ? &m_storage[0] : 0;
}
const T* getBufferPointer() const
{
return m_storage.size() ? &m_storage[0] : 0;
}
};
/*
template <typename T>
void setElem(btMatrixX<T>& mat, int row, int col, T val)
{
mat.setElem(row,col,val);
}
*/
template <typename T>
struct btMatrixX
{
@ -40,8 +161,7 @@ struct btMatrixX
int m_setElemOperations;
btAlignedObjectArray<T> m_storage;
btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
btAlignedObjectArray< btAlignedObjectArray<int> > m_colNonZeroElements;
mutable btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
T* getBufferPointerWritable()
{
@ -78,7 +198,6 @@ struct btMatrixX
BT_PROFILE("m_storage.resize");
m_storage.resize(rows*cols);
}
clearSparseInfo();
}
int cols() const
{
@ -109,49 +228,44 @@ struct btMatrixX
}
}
void setElem(int row,int col, T val)
{
m_setElemOperations++;
m_storage[row*m_cols+col] = val;
}
void mulElem(int row,int col, T val)
{
m_setElemOperations++;
//mul doesn't change sparsity info
m_storage[row*m_cols+col] *= val;
}
void copyLowerToUpperTriangle()
{
int count=0;
for (int row=0;row<m_rowNonZeroElements1.size();row++)
for (int row=0;row<rows();row++)
{
for (int j=0;j<m_rowNonZeroElements1[row].size();j++)
for (int col=0;col<row;col++)
{
int col = m_rowNonZeroElements1[row][j];
setElem(col,row, (*this)(row,col));
count++;
}
}
//printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows());
}
void setElem(int row,int col, T val)
{
m_setElemOperations++;
if (val)
{
if (m_storage[col+row*m_cols]==0.f)
{
m_rowNonZeroElements1[row].push_back(col);
m_colNonZeroElements[col].push_back(row);
}
m_storage[row*m_cols+col] = val;
}
}
const T& operator() (int row,int col) const
{
return m_storage[col+row*m_cols];
}
void clearSparseInfo()
{
BT_PROFILE("clearSparseInfo=0");
m_rowNonZeroElements1.resize(m_rows);
m_colNonZeroElements.resize(m_cols);
for (int i=0;i<m_rows;i++)
m_rowNonZeroElements1[i].resize(0);
for (int j=0;j<m_cols;j++)
m_colNonZeroElements[j].resize(0);
}
void setZero()
{
@ -162,12 +276,21 @@ struct btMatrixX
//for (int i=0;i<m_storage.size();i++)
// m_storage[i]=0;
}
}
void setIdentity()
{
btAssert(rows() == cols());
setZero();
for (int row=0;row<rows();row++)
{
BT_PROFILE("clearSparseInfo=0");
clearSparseInfo();
setElem(row,row,1);
}
}
void printMatrix(const char* msg)
{
printf("%s ---------------------\n",msg);
@ -182,28 +305,9 @@ struct btMatrixX
printf("\n---------------------\n");
}
void printNumZeros(const char* msg)
{
printf("%s: ",msg);
int numZeros = 0;
for (int i=0;i<m_storage.size();i++)
if (m_storage[i]==0)
numZeros++;
int total = m_cols*m_rows;
int computedNonZero = total-numZeros;
int nonZero = 0;
for (int i=0;i<m_colNonZeroElements.size();i++)
nonZero += m_colNonZeroElements[i].size();
btAssert(computedNonZero==nonZero);
if(computedNonZero!=nonZero)
{
printf("Error: computedNonZero=%d, but nonZero=%d\n",computedNonZero,nonZero);
}
//printf("%d numZeros out of %d (%f)\n",numZeros,m_cols*m_rows,numZeros/(m_cols*m_rows));
printf("total %d, %d rows, %d cols, %d non-zeros (%f %)\n", total, rows(),cols(), nonZero,100.f*(T)nonZero/T(total));
}
/*
void rowComputeNonZeroElements()
void rowComputeNonZeroElements() const
{
m_rowNonZeroElements1.resize(rows());
for (int i=0;i<rows();i++)
@ -218,13 +322,11 @@ struct btMatrixX
}
}
}
*/
btMatrixX transpose() const
{
//transpose is optimized for sparse matrices
btMatrixX tr(m_cols,m_rows);
tr.setZero();
#if 0
for (int i=0;i<m_cols;i++)
for (int j=0;j<m_rows;j++)
{
@ -234,37 +336,13 @@ struct btMatrixX
tr.setElem(i,j,v);
}
}
#else
for (int i=0;i<m_colNonZeroElements.size();i++)
for (int h=0;h<m_colNonZeroElements[i].size();h++)
{
int j = m_colNonZeroElements[i][h];
T v = (*this)(j,i);
tr.setElem(i,j,v);
}
#endif
return tr;
}
void sortRowIndexArrays()
{
for (int i=0;i<m_rowNonZeroElements1[i].size();i++)
{
m_rowNonZeroElements1[i].quickSort(btIntSortPredicate());
}
}
void sortColIndexArrays()
{
for (int i=0;i<m_colNonZeroElements[i].size();i++)
{
m_colNonZeroElements[i].quickSort(btIntSortPredicate());
}
}
btMatrixX operator*(const btMatrixX& other)
{
//btMatrixX*btMatrixX implementation, optimized for sparse matrices
//btMatrixX*btMatrixX implementation, brute force
btAssert(cols() == other.rows());
btMatrixX res(rows(),other.cols());
@ -272,76 +350,18 @@ struct btMatrixX
// BT_PROFILE("btMatrixX mul");
for (int j=0; j < res.cols(); ++j)
{
//int numZero=other.m_colNonZeroElements[j].size();
//if (numZero)
{
for (int i=0; i < res.rows(); ++i)
//for (int g = 0;g<m_colNonZeroElements[j].size();g++)
{
T dotProd=0;
T dotProd2=0;
int waste=0,waste2=0;
bool doubleWalk = false;
if (doubleWalk)
{
int numRows = m_rowNonZeroElements1[i].size();
int numOtherCols = other.m_colNonZeroElements[j].size();
for (int ii=0;ii<numRows;ii++)
{
int vThis=m_rowNonZeroElements1[i][ii];
}
for (int ii=0;ii<numOtherCols;ii++)
{
int vOther = other.m_colNonZeroElements[j][ii];
}
int indexRow = 0;
int indexOtherCol = 0;
while (indexRow < numRows && indexOtherCol < numOtherCols)
{
int vThis=m_rowNonZeroElements1[i][indexRow];
int vOther = other.m_colNonZeroElements[j][indexOtherCol];
if (vOther==vThis)
{
dotProd += (*this)(i,vThis) * other(vThis,j);
}
if (vThis<vOther)
{
indexRow++;
} else
{
indexOtherCol++;
}
}
} else
{
bool useOtherCol = true;
if (other.m_colNonZeroElements[j].size() <m_rowNonZeroElements1[i].size())
{
useOtherCol=true;
}
if (!useOtherCol )
{
for (int q=0;q<other.m_colNonZeroElements[j].size();q++)
for (int v=0;v<rows();v++)
{
int v = other.m_colNonZeroElements[j][q];
T w = (*this)(i,v);
if (w!=0.f)
{
dotProd+=w*other(v,j);
}
}
}
else
{
for (int q=0;q<m_rowNonZeroElements1[i].size();q++)
{
int v=m_rowNonZeroElements1[i][q];
T w = (*this)(i,v);
if (other(v,j)!=0.f)
{
@ -404,73 +424,61 @@ struct btMatrixX
bb += 8;
}
}
};
template <typename T>
struct btVectorX
{
btAlignedObjectArray<T> m_storage;
btVectorX()
void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const T value)
{
int numRows = rowend+1-rowstart;
int numCols = colend+1-colstart;
for (int row=0;row<numRows;row++)
{
for (int col=0;col<numCols;col++)
{
setElem(rowstart+row,colstart+col,value);
}
}
}
btVectorX(int numRows)
void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btMatrixX& block)
{
m_storage.resize(numRows);
btAssert(rowend+1-rowstart == block.rows());
btAssert(colend+1-colstart == block.cols());
for (int row=0;row<block.rows();row++)
{
for (int col=0;col<block.cols();col++)
{
setElem(rowstart+row,colstart+col,block(row,col));
}
}
}
void resize(int rows)
void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btVectorX<T>& block)
{
m_storage.resize(rows);
btAssert(rowend+1-rowstart == block.rows());
btAssert(colend+1-colstart == block.cols());
for (int row=0;row<block.rows();row++)
{
for (int col=0;col<block.cols();col++)
{
setElem(rowstart+row,colstart+col,block[row]);
}
}
}
int cols() const
btMatrixX negative()
{
return 1;
}
int rows() const
{
return m_storage.size();
}
int size() const
{
return rows();
}
void setZero()
{
// for (int i=0;i<m_storage.size();i++)
// m_storage[i]=0;
//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
btSetZero(&m_storage[0],m_storage.size());
}
const T& operator[] (int index) const
{
return m_storage[index];
}
T& operator[] (int index)
{
return m_storage[index];
}
T* getBufferPointerWritable()
{
return m_storage.size() ? &m_storage[0] : 0;
}
const T* getBufferPointer() const
{
return m_storage.size() ? &m_storage[0] : 0;
btMatrixX neg(rows(),cols());
for (int i=0;i<rows();i++)
for (int j=0;j<cols();j++)
{
T v = (*this)(i,j);
neg.setElem(i,j,-v);
}
return neg;
}
};
/*
template <typename T>
void setElem(btMatrixX<T>& mat, int row, int col, T val)
{
mat.setElem(row,col,val);
}
*/
typedef btMatrixX<float> btMatrixXf;
@ -480,6 +488,47 @@ typedef btMatrixX<double> btMatrixXd;
typedef btVectorX<double> btVectorXd;
#ifdef BT_DEBUG_OSTREAM
template <typename T>
std::ostream& operator<< (std::ostream& os, const btMatrixX<T>& mat)
{
os << " [";
//printf("%s ---------------------\n",msg);
for (int i=0;i<mat.rows();i++)
{
for (int j=0;j<mat.cols();j++)
{
os << std::setw(12) << mat(i,j);
}
if (i!=mat.rows()-1)
os << std::endl << " ";
}
os << " ]";
//printf("\n---------------------\n");
return os;
}
template <typename T>
std::ostream& operator<< (std::ostream& os, const btVectorX<T>& mat)
{
os << " [";
//printf("%s ---------------------\n",msg);
for (int i=0;i<mat.rows();i++)
{
os << std::setw(12) << mat[i];
if (i!=mat.rows()-1)
os << std::endl << " ";
}
os << " ]";
//printf("\n---------------------\n");
return os;
}
#endif //BT_DEBUG_OSTREAM
inline void setElem(btMatrixXd& mat, int row, int col, double val)
{

11
extern/bullet2/src/LinearMath/btScalar.h vendored
View File

@ -284,6 +284,10 @@ static int btNanMask = 0x7F800001;
#ifndef BT_INFINITY
static int btInfinityMask = 0x7F800000;
#define BT_INFINITY (*(float*)&btInfinityMask)
inline int btGetInfinityMask()//suppress stupid compiler warning
{
return btInfinityMask;
}
#endif
//use this, in case there are clashes (such as xnamath.h)
@ -336,6 +340,10 @@ inline __m128 operator * (const __m128 A, const __m128 B)
#ifndef BT_INFINITY
static int btInfinityMask = 0x7F800000;
#define BT_INFINITY (*(float*)&btInfinityMask)
inline int btGetInfinityMask()//suppress stupid compiler warning
{
return btInfinityMask;
}
#endif
#endif//BT_USE_NEON
@ -432,7 +440,7 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
#endif
#define SIMD_PI btScalar(3.1415926535897932384626433832795029)
#define SIMD_2_PI btScalar(2.0) * SIMD_PI
#define SIMD_2_PI (btScalar(2.0) * SIMD_PI)
#define SIMD_HALF_PI (SIMD_PI * btScalar(0.5))
#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
#define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI)
@ -728,4 +736,5 @@ template <typename T>T* btAlignPointer(T* unalignedPtr, size_t alignment)
return converter.ptr;
}
#endif //BT_SCALAR_H

8
extern/bullet2/src/LinearMath/btVector3.cpp vendored
View File

@ -821,6 +821,7 @@ long _mindot_large( const float *vv, const float *vec, unsigned long count, floa
#elif defined BT_USE_NEON
#define ARM_NEON_GCC_COMPATIBILITY 1
#include <arm_neon.h>
#include <sys/types.h>
@ -884,7 +885,12 @@ static long _mindot_large_sel( const float *vv, const float *vec, unsigned long
#define vld1q_f32_aligned_postincrement( _ptr ) ({ float32x4_t _r; asm( "vld1.f32 {%0}, [%1, :128]!\n" : "=w" (_r), "+r" (_ptr) ); /*return*/ _r; })
#if defined __arm__
# define vld1q_f32_aligned_postincrement( _ptr ) ({ float32x4_t _r; asm( "vld1.f32 {%0}, [%1, :128]!\n" : "=w" (_r), "+r" (_ptr) ); /*return*/ _r; })
#else
//support 64bit arm
# define vld1q_f32_aligned_postincrement( _ptr) ({ float32x4_t _r = ((float32x4_t*)(_ptr))[0]; (_ptr) = (const float*) ((const char*)(_ptr) + 16L); /*return*/ _r; })
#endif
long _maxdot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult )

17
extern/bullet2/src/LinearMath/btVector3.h vendored
View File

@ -297,7 +297,7 @@ public:
SIMD_FORCE_INLINE btVector3& normalize()
{
btAssert(length() != btScalar(0));
btAssert(!fuzzyZero());
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
// dot product first
@ -685,9 +685,10 @@ public:
return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
}
SIMD_FORCE_INLINE bool fuzzyZero() const
{
return length2() < SIMD_EPSILON;
return length2() < SIMD_EPSILON*SIMD_EPSILON;
}
SIMD_FORCE_INLINE void serialize(struct btVector3Data& dataOut) const;
@ -950,9 +951,9 @@ SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
{
btVector3 norm = *this;
btVector3 nrm = *this;
return norm.normalize();
return nrm.normalize();
}
SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const
@ -1010,21 +1011,21 @@ SIMD_FORCE_INLINE long btVector3::maxDot( const btVector3 *array, long arra
if( array_count < scalar_cutoff )
#endif
{
btScalar maxDot = -SIMD_INFINITY;
btScalar maxDot1 = -SIMD_INFINITY;
int i = 0;
int ptIndex = -1;
for( i = 0; i < array_count; i++ )
{
btScalar dot = array[i].dot(*this);
if( dot > maxDot )
if( dot > maxDot1 )
{
maxDot = dot;
maxDot1 = dot;
ptIndex = i;
}
}
dotOut = maxDot;
dotOut = maxDot1;
return ptIndex;
}
#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)