Nixyri/godot
1
0
Fork 0
mirror of https://github.com/godotengine/godot.git synced 2024-11-10 22:23:07 +00:00
Code Issues Actions 1 Packages Projects Releases Wiki Activity

Update bullet to Master 12409f1118a7c7a266f9071350c70789dfe73bb9

Browse Source
This commit is contained in:
Andrea Catania 2018-09-07 16:11:04 +02:00
parent 5307043751
commit 6142448417
119 changed files with 11716 additions and 2440 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
modules/bullet/SCsub
View File

@ -68,11 +68,13 @@ if env['builtin_bullet']:
, "BulletCollision/CollisionShapes/btEmptyShape.cpp"
, "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp"
, "BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp"
, "BulletCollision/CollisionShapes/btMiniSDF.cpp"
, "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp"
, "BulletCollision/CollisionShapes/btMultiSphereShape.cpp"
, "BulletCollision/CollisionShapes/btOptimizedBvh.cpp"
, "BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp"
, "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp"
, "BulletCollision/CollisionShapes/btSdfCollisionShape.cpp"
, "BulletCollision/CollisionShapes/btShapeHull.cpp"
, "BulletCollision/CollisionShapes/btSphereShape.cpp"
, "BulletCollision/CollisionShapes/btStaticPlaneShape.cpp"

2
thirdparty/README.md vendored
View File

@ -19,7 +19,7 @@ comments.
## bullet
- Upstream: https://github.com/bulletphysics/bullet3
- Version: git (d05ad4b, 2017)
- Version: git (12409f1118a7c7a266f9071350c70789dfe73bb9, Commits on Sep 6, 2018 )
- License: zlib
Files extracted from upstream source:

8
thirdparty/bullet/Bullet3Common/b3AlignedObjectArray.h vendored
View File

@ -528,6 +528,14 @@ protected:
otherArray.copy(0, otherSize, m_data);
}
void removeAtIndex(int index)
{
if (index<size())
{
swap( index,size()-1);
pop_back();
}
}
};
#endif //B3_OBJECT_ARRAY__

2
thirdparty/bullet/Bullet3Common/b3FileUtils.h vendored
View File

@ -36,7 +36,7 @@ struct b3FileUtils
for (int i=0;!f && i<numPrefixes;i++)
{
#ifdef _WIN32
#ifdef _MSC_VER
sprintf_s(relativeFileName,maxRelativeFileNameMaxLen,"%s%s",prefix[i],orgFileName);
#else
sprintf(relativeFileName,"%s%s",prefix[i],orgFileName);

4
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp vendored
View File

@ -599,8 +599,8 @@ int b3Generic6DofConstraint::get_limit_motor_info2(
tag_vel,
info->fps * limot->m_stopERP);
info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
info->m_upperLimit[srow] = limot->m_maxMotorForce;
info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
}
}
if(limit)

2
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h vendored
View File

@ -69,7 +69,7 @@ public:
{
m_accumulatedImpulse = 0.f;
m_targetVelocity = 0;
m_maxMotorForce = 0.1f;
m_maxMotorForce = 6.0f;
m_maxLimitForce = 300.0f;
m_loLimit = 1.0f;
m_hiLimit = -1.0f;

2
thirdparty/bullet/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp vendored
View File

@ -619,7 +619,7 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
strippedName = strip2(clFileNameForCaching,"\\");
strippedName = strip2(strippedName,"/");
#ifdef _MSVC_VER
#ifdef _MSC_VER
sprintf_s(binaryFileName,B3_MAX_STRING_LENGTH,"%s/%s.%s.%s.bin",sCachedBinaryPath,strippedName, deviceName,driverVersion );
#else
sprintf(binaryFileName,"%s/%s.%s.%s.bin",sCachedBinaryPath,strippedName, deviceName,driverVersion );

6
thirdparty/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h vendored
View File

@ -628,7 +628,6 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool(btDispatcher* /*dispatcher*
}
extern int gOverlappingPairs;
//#include <stdio.h>
template <typename BP_FP_INT_TYPE>
@ -695,10 +694,9 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatche
pair.m_pProxy0 = 0;
pair.m_pProxy1 = 0;
m_invalidPair++;
gOverlappingPairs--;
}
}
}
}
///if you don't like to skip the invalid pairs in the array, execute following code:
#define CLEAN_INVALID_PAIRS 1

1
thirdparty/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h vendored
View File

@ -66,6 +66,7 @@ CONCAVE_SHAPES_START_HERE,
EMPTY_SHAPE_PROXYTYPE,
STATIC_PLANE_PROXYTYPE,
CUSTOM_CONCAVE_SHAPE_TYPE,
SDF_SHAPE_PROXYTYPE=CUSTOM_CONCAVE_SHAPE_TYPE,
CONCAVE_SHAPES_END_HERE,
COMPOUND_SHAPE_PROXYTYPE,

11
thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp vendored
View File

@ -17,7 +17,7 @@ subject to the following restrictions:
#include "btDbvtBroadphase.h"
#include "LinearMath/btThreads.h"
btScalar gDbvtMargin = btScalar(0.05);
//
// Profiling
//
@ -332,12 +332,9 @@ void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy,
if(delta[0]<0) velocity[0]=-velocity[0];
if(delta[1]<0) velocity[1]=-velocity[1];
if(delta[2]<0) velocity[2]=-velocity[2];
if (
#ifdef DBVT_BP_MARGIN
m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
#else
m_sets[0].update(proxy->leaf,aabb,velocity)
#endif
if (
m_sets[0].update(proxy->leaf, aabb, velocity, gDbvtMargin)
)
{
++m_updates_done;

3
thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h vendored
View File

@ -29,7 +29,8 @@ subject to the following restrictions:
#define DBVT_BP_PREVENTFALSEUPDATE 0
#define DBVT_BP_ACCURATESLEEPING 0
#define DBVT_BP_ENABLE_BENCHMARK 0
#define DBVT_BP_MARGIN (btScalar)0.05
//#define DBVT_BP_MARGIN (btScalar)0.05
extern btScalar gDbvtMargin;
#if DBVT_BP_PROFILE
#define DBVT_BP_PROFILING_RATE 256

4
thirdparty/bullet/BulletCollision/BroadphaseCollision/btDispatcher.h vendored
View File

@ -46,7 +46,8 @@ struct btDispatcherInfo
m_useEpa(true),
m_allowedCcdPenetration(btScalar(0.04)),
m_useConvexConservativeDistanceUtil(false),
m_convexConservativeDistanceThreshold(0.0f)
m_convexConservativeDistanceThreshold(0.0f),
m_deterministicOverlappingPairs(false)
{
}
@ -62,6 +63,7 @@ struct btDispatcherInfo
btScalar m_allowedCcdPenetration;
bool m_useConvexConservativeDistanceUtil;
btScalar m_convexConservativeDistanceThreshold;
bool m_deterministicOverlappingPairs;
};
enum ebtDispatcherQueryType

81
thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp vendored
View File

@ -23,11 +23,6 @@ subject to the following restrictions:
#include <stdio.h>
int gOverlappingPairs = 0;
int gRemovePairs =0;
int gAddedPairs =0;
int gFindPairs =0;
@ -134,7 +129,6 @@ void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroad
btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
{
gFindPairs++;
if(proxy0->m_uniqueId>proxy1->m_uniqueId)
btSwap(proxy0,proxy1);
int proxyId1 = proxy0->getUid();
@ -271,7 +265,6 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx
void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
{
gRemovePairs++;
if(proxy0->m_uniqueId>proxy1->m_uniqueId)
btSwap(proxy0,proxy1);
int proxyId1 = proxy0->getUid();
@ -386,8 +379,6 @@ void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback*
if (callback->processOverlap(*pair))
{
removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher);
gOverlappingPairs--;
} else
{
i++;
@ -395,6 +386,70 @@ void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback*
}
}
struct MyPairIndex
{
int m_orgIndex;
int m_uidA0;
int m_uidA1;
};
class MyPairIndeSortPredicate
{
public:
bool operator() ( const MyPairIndex& a, const MyPairIndex& b ) const
{
const int uidA0 = a.m_uidA0;
const int uidB0 = b.m_uidA0;
const int uidA1 = a.m_uidA1;
const int uidB1 = b.m_uidA1;
return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1);
}
};
void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo)
{
if (dispatchInfo.m_deterministicOverlappingPairs)
{
btBroadphasePairArray& pa = getOverlappingPairArray();
btAlignedObjectArray<MyPairIndex> indices;
{
BT_PROFILE("sortOverlappingPairs");
indices.resize(pa.size());
for (int i=0;i<indices.size();i++)
{
const btBroadphasePair& p = pa[i];
const int uidA0 = p.m_pProxy0 ? p.m_pProxy0->m_uniqueId : -1;
const int uidA1 = p.m_pProxy1 ? p.m_pProxy1->m_uniqueId : -1;
indices[i].m_uidA0 = uidA0;
indices[i].m_uidA1 = uidA1;
indices[i].m_orgIndex = i;
}
indices.quickSort(MyPairIndeSortPredicate());
}
{
BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
int i;
for (i=0;i<indices.size();)
{
btBroadphasePair* pair = &pa[indices[i].m_orgIndex];
if (callback->processOverlap(*pair))
{
removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher);
} else
{
i++;
}
}
}
} else
{
processAllOverlappingPairs(callback, dispatcher);
}
}
void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
{
///need to keep hashmap in sync with pair address, so rebuild all
@ -435,7 +490,6 @@ void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
if (findIndex < m_overlappingPairArray.size())
{
gOverlappingPairs--;
btBroadphasePair& pair = m_overlappingPairArray[findIndex];
void* userData = pair.m_internalInfo1;
cleanOverlappingPair(pair,dispatcher);
@ -469,10 +523,7 @@ btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseP
void* mem = &m_overlappingPairArray.expandNonInitializing();
btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
gOverlappingPairs++;
gAddedPairs++;
if (m_ghostPairCallback)
if (m_ghostPairCallback)
m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
return pair;
@ -526,7 +577,6 @@ void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback*
pair->m_pProxy1 = 0;
m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
m_overlappingPairArray.pop_back();
gOverlappingPairs--;
} else
{
i++;
@ -559,7 +609,6 @@ void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,b
pair.m_algorithm->~btCollisionAlgorithm();
dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
pair.m_algorithm=0;
gRemovePairs--;
}
}
}

12
thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h vendored
View File

@ -49,10 +49,6 @@ struct btOverlapFilterCallback
extern int gRemovePairs;
extern int gAddedPairs;
extern int gFindPairs;
const int BT_NULL_PAIR=0xffffffff;
///The btOverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases.
@ -78,6 +74,10 @@ public:
virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0;
virtual void processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo)
{
processAllOverlappingPairs(callback, dispatcher);
}
virtual btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0;
virtual bool hasDeferredRemoval() = 0;
@ -129,8 +129,6 @@ public:
// no new pair is created and the old one is returned.
virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
{
gAddedPairs++;
if (!needsBroadphaseCollision(proxy0,proxy1))
return 0;
@ -144,6 +142,8 @@ public:
virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
virtual void processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo);
virtual btBroadphasePair* getOverlappingPairArrayPtr()
{
return &m_overlappingPairArray[0];

3
thirdparty/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp vendored
View File

@ -24,8 +24,6 @@ subject to the following restrictions:
#include <new>
extern int gOverlappingPairs;
void btSimpleBroadphase::validate()
{
for (int i=0;i<m_numHandles;i++)
@ -315,7 +313,6 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
pair.m_pProxy0 = 0;
pair.m_pProxy1 = 0;
m_invalidPair++;
gOverlappingPairs--;
}
}

6
thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp vendored
View File

@ -132,6 +132,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
else {
// Could be inside one of the contact capsules
btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
btScalar minDistSqr = contactCapsuleRadiusSqr;
btVector3 nearestOnEdge;
for (int i = 0; i < m_triangle->getNumEdges(); i++) {
@ -141,8 +142,9 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
m_triangle->getEdge(i, pa, pb);
btScalar distanceSqr = SegmentSqrDistance(pa, pb, sphereCenter, nearestOnEdge);
if (distanceSqr < contactCapsuleRadiusSqr) {
// Yep, we're inside a capsule
if (distanceSqr < minDistSqr) {
// Yep, we're inside a capsule, and record the capsule with smallest distance
minDistSqr = distanceSqr;
hasContact = true;
contactPoint = nearestOnEdge;
}

8
thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp vendored
View File

@ -151,8 +151,8 @@ static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1
int index = 0;
btScalar minDot = BT_LARGE_FLOAT;
if( count2 > 0 )
index = (int) normal1.minDot( vertices2, count2, minDot);
if( count2 > 0 )
index = (int) normal1.minDot( vertices2, count2, minDot);
btVector3 v1 = b2Mul(xf1, vertices1[edge1]);
btVector3 v2 = b2Mul(xf2, vertices2[index]);
@ -175,8 +175,8 @@ static btScalar FindMaxSeparation(int* edgeIndex,
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
btScalar maxDot;
if( count1 > 0 )
edge = (int) dLocal1.maxDot( normals1, count1, maxDot);
if( count1 > 0 )
edge = (int) dLocal1.maxDot( normals1, count1, maxDot);
// Get the separation for the edge normal.
btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);

11
thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp vendored
View File

@ -27,8 +27,6 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
int gNumManifold = 0;
#ifdef BT_DEBUG
#include <stdio.h>
#endif
@ -77,8 +75,6 @@ btCollisionDispatcher::~btCollisionDispatcher()
btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObject* body0,const btCollisionObject* body1)
{
gNumManifold++;
//btAssert(gNumManifold < 65535);
@ -121,7 +117,6 @@ void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold)
void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
{
gNumManifold--;
//printf("releaseManifold: gNumManifold %d\n",gNumManifold);
clearManifold(manifold);
@ -246,13 +241,17 @@ public:
void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher)
{
//m_blockedForChanges = true;
btCollisionPairCallback collisionCallback(dispatchInfo,this);
pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher);
{
BT_PROFILE("processAllOverlappingPairs");
pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher, dispatchInfo);
}
//m_blockedForChanges = false;

19
thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp vendored
View File

@ -16,6 +16,7 @@ subject to the following restrictions:
#include "btCollisionObject.h"
#include "LinearMath/btSerializer.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
btCollisionObject::btCollisionObject()
: m_interpolationLinearVelocity(0.f, 0.f, 0.f),
@ -38,7 +39,7 @@ btCollisionObject::btCollisionObject()
m_rollingFriction(0.0f),
m_spinningFriction(0.f),
m_contactDamping(.1),
m_contactStiffness(1e4),
m_contactStiffness(BT_LARGE_FLOAT),
m_internalType(CO_COLLISION_OBJECT),
m_userObjectPointer(0),
m_userIndex2(-1),
@ -114,10 +115,18 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius;
dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold;
dataOut->m_checkCollideWith = m_checkCollideWith;
// Fill padding with zeros to appease msan.
memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding));
if (m_broadphaseHandle)
{
dataOut->m_collisionFilterGroup = m_broadphaseHandle->m_collisionFilterGroup;
dataOut->m_collisionFilterMask = m_broadphaseHandle->m_collisionFilterMask;
dataOut->m_uniqueId = m_broadphaseHandle->m_uniqueId;
}
else
{
dataOut->m_collisionFilterGroup = 0;
dataOut->m_collisionFilterMask = 0;
dataOut->m_uniqueId = -1;
}
return btCollisionObjectDataName;
}

13
thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h vendored
View File

@ -621,7 +621,6 @@ struct btCollisionObjectDoubleData
double m_hitFraction;
double m_ccdSweptSphereRadius;
double m_ccdMotionThreshold;
int m_hasAnisotropicFriction;
int m_collisionFlags;
int m_islandTag1;
@ -629,8 +628,9 @@ struct btCollisionObjectDoubleData
int m_activationState1;
int m_internalType;
int m_checkCollideWith;
char m_padding[4];
int m_collisionFilterGroup;
int m_collisionFilterMask;
int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
@ -650,13 +650,12 @@ struct btCollisionObjectFloatData
float m_deactivationTime;
float m_friction;
float m_rollingFriction;
float m_contactDamping;
float m_contactDamping;
float m_contactStiffness;
float m_restitution;
float m_hitFraction;
float m_ccdSweptSphereRadius;
float m_ccdMotionThreshold;
int m_hasAnisotropicFriction;
int m_collisionFlags;
int m_islandTag1;
@ -664,7 +663,9 @@ struct btCollisionObjectFloatData
int m_activationState1;
int m_internalType;
int m_checkCollideWith;
char m_padding[4];
int m_collisionFilterGroup;
int m_collisionFilterMask;
int m_uniqueId;
};

26
thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp vendored
View File

@ -1646,7 +1646,7 @@ void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
for (i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
if ((colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) || (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK))
if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
{
colObj->serializeSingleObject(serializer);
}
@ -1654,6 +1654,28 @@ void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
}
void btCollisionWorld::serializeContactManifolds(btSerializer* serializer)
{
if (serializer->getSerializationFlags() & BT_SERIALIZE_CONTACT_MANIFOLDS)
{
int numManifolds = getDispatcher()->getNumManifolds();
for (int i = 0; i < numManifolds; i++)
{
const btPersistentManifold* manifold = getDispatcher()->getInternalManifoldPointer()[i];
//don't serialize empty manifolds, they just take space
//(may have to do it anyway if it destroys determinism)
if (manifold->getNumContacts() == 0)
continue;
btChunk* chunk = serializer->allocate(manifold->calculateSerializeBufferSize(), 1);
const char* structType = manifold->serialize(manifold, chunk->m_oldPtr, serializer);
serializer->finalizeChunk(chunk, structType, BT_CONTACTMANIFOLD_CODE, (void*)manifold);
}
}
}
void btCollisionWorld::serialize(btSerializer* serializer)
{
@ -1661,6 +1683,8 @@ void btCollisionWorld::serialize(btSerializer* serializer)
serializeCollisionObjects(serializer);
serializeContactManifolds(serializer);
serializer->finishSerialization();
}

3
thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h vendored
View File

@ -107,6 +107,9 @@ protected:
void serializeCollisionObjects(btSerializer* serializer);
void serializeContactManifolds(btSerializer* serializer);
public:
//this constructor doesn't own the dispatcher and paircache/broadphase

12
thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp vendored
View File

@ -156,10 +156,12 @@ public:
btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index);
btCollisionAlgorithm* algo = 0;
bool allocatedAlgorithm = false;
if (m_resultOut->m_closestPointDistanceThreshold > 0)
{
algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
allocatedAlgorithm = true;
}
else
{
@ -204,7 +206,11 @@ public:
{
m_resultOut->setBody1Wrap(tmpWrap);
}
if(allocatedAlgorithm)
{
algo->~btCollisionAlgorithm();
m_dispatcher->freeCollisionAlgorithm(algo);
}
}
}
void Process(const btDbvtNode* leaf)
@ -253,8 +259,8 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
m_compoundShapeRevision = compoundShape->getUpdateRevision();
}
if (m_childCollisionAlgorithms.size()==0)
return;
if (m_childCollisionAlgorithms.size()==0)
return;
const btDbvt* tree = compoundShape->getDynamicAabbTree();
//use a dynamic aabb tree to cull potential child-overlaps

21
thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp vendored
View File

@ -181,11 +181,12 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1);
bool removePair = false;
btCollisionAlgorithm* colAlgo = 0;
if (m_resultOut->m_closestPointDistanceThreshold > 0)
{
colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS);
removePair = true;
}
else
{
@ -223,7 +224,11 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
m_resultOut->setBody0Wrap(tmpWrap0);
m_resultOut->setBody1Wrap(tmpWrap1);
if (removePair)
{
colAlgo->~btCollisionAlgorithm();
m_dispatcher->freeCollisionAlgorithm(colAlgo);
}
}
}
@ -396,32 +401,24 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
{
btTransform orgTrans0;
const btCollisionShape* childShape0 = 0;
btTransform newChildWorldTrans0;
btTransform orgInterpolationTrans0;
childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA);
orgTrans0 = col0ObjWrap->getWorldTransform();
orgInterpolationTrans0 = col0ObjWrap->getWorldTransform();
const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA);
newChildWorldTrans0 = orgTrans0*childTrans0 ;
newChildWorldTrans0 = col0ObjWrap->getWorldTransform()*childTrans0 ;
childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
}
btVector3 thresholdVec(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
aabbMin0 -= thresholdVec;
aabbMax0 += thresholdVec;
{
btTransform orgInterpolationTrans1;
const btCollisionShape* childShape1 = 0;
btTransform orgTrans1;
btTransform newChildWorldTrans1;
childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB);
orgTrans1 = col1ObjWrap->getWorldTransform();
orgInterpolationTrans1 = col1ObjWrap->getWorldTransform();
const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB);
newChildWorldTrans1 = orgTrans1*childTrans1 ;
newChildWorldTrans1 = col1ObjWrap->getWorldTransform()*childTrans1 ;
childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
}

101
thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp vendored
View File

@ -27,6 +27,7 @@ subject to the following restrictions:
#include "LinearMath/btIDebugDraw.h"
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
#include "BulletCollision/CollisionShapes/btSdfCollisionShape.h"
btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
@ -163,7 +164,7 @@ partId, int triangleIndex)
void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
{
m_convexBodyWrap = convexBodyWrap;
m_triBodyWrap = triBodyWrap;
@ -177,10 +178,10 @@ void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr
convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform();
const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
btScalar extraMargin = collisionMarginTriangle+ resultOut->m_closestPointDistanceThreshold;
convexShape->getAabb(convexInTriangleSpace, m_aabbMin, m_aabbMax);
btScalar extraMargin = collisionMarginTriangle + resultOut->m_closestPointDistanceThreshold;
btVector3 extra(extraMargin,extraMargin,extraMargin);
btVector3 extra(extraMargin, extraMargin, extraMargin);
m_aabbMax += extra;
m_aabbMin -= extra;
@ -193,7 +194,7 @@ void btConvexConcaveCollisionAlgorithm::clearCache()
}
void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision");
@ -202,26 +203,90 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec
if (triBodyWrap->getCollisionShape()->isConcave())
{
const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape());
if (convexBodyWrap->getCollisionShape()->isConvex())
if (triBodyWrap->getCollisionShape()->getShapeType() == SDF_SHAPE_PROXYTYPE)
{
btScalar collisionMarginTriangle = concaveShape->getMargin();
btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape();
if (convexBodyWrap->getCollisionShape()->isConvex())
{
resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape();
btAlignedObjectArray<btVector3> queryVertices;
m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
if (convex->isPolyhedral())
{
btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex;
for (int v = 0; v < poly->getNumVertices(); v++)
{
btVector3 vtx;
poly->getVertex(v, vtx);
queryVertices.push_back(vtx);
}
}
btScalar maxDist = SIMD_EPSILON;
concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
{
queryVertices.push_back(btVector3(0, 0, 0));
btSphereShape* sphere = (btSphereShape*)convex;
maxDist = sphere->getRadius() + SIMD_EPSILON;
resultOut->refreshContactPoints();
}
if (queryVertices.size())
{
resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
//m_btConvexTriangleCallback.m_manifoldPtr->clearManifold();
m_btConvexTriangleCallback.clearWrapperData();
btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex;
for (int v = 0; v < queryVertices.size(); v++)
{
const btVector3& vtx = queryVertices[v];
btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform()*vtx;
btVector3 vtxInSdf = triBodyWrap->getWorldTransform().invXform(vtxWorldSpace);
btVector3 normalLocal;
btScalar dist;
if (sdfShape->queryPoint(vtxInSdf, dist, normalLocal))
{
if (dist <= maxDist)
{
normalLocal.safeNormalize();
btVector3 normal = triBodyWrap->getWorldTransform().getBasis()*normalLocal;
if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
{
btSphereShape* sphere = (btSphereShape*)convex;
dist -= sphere->getRadius();
vtxWorldSpace -= sphere->getRadius()*normal;
}
resultOut->addContactPoint(normal,vtxWorldSpace-normal*dist, dist);
}
}
}
resultOut->refreshContactPoints();
}
}
} else
{
const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape());
if (convexBodyWrap->getCollisionShape()->isConvex())
{
btScalar collisionMarginTriangle = concaveShape->getMargin();
resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
resultOut->refreshContactPoints();
m_btConvexTriangleCallback.clearWrapperData();
}
}
}

150
thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp vendored
View File

@ -28,6 +28,7 @@ subject to the following restrictions:
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
@ -259,7 +260,7 @@ struct btPerturbedContactResult : public btManifoldResult
btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
newDepth = (endPt - pointInWorld).dot(normalOnBInWorld);
startPt = endPt+normalOnBInWorld*newDepth;
startPt = endPt - normalOnBInWorld*newDepth;
} else
{
endPt = pointInWorld + normalOnBInWorld*orgDepth;
@ -442,10 +443,26 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
{
btVector3 m_normalOnBInWorld;
btVector3 m_pointInWorld;
btScalar m_depth;
bool m_hasContact;
btDummyResult()
: m_hasContact(false)
{
}
virtual void setShapeIdentifiersA(int partId0,int index0){}
virtual void setShapeIdentifiersB(int partId1,int index1){}
virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
{
m_hasContact = true;
m_normalOnBInWorld = normalOnBInWorld;
m_pointInWorld = pointInWorld;
m_depth = depth;
}
};
@ -560,15 +577,18 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
} else
{
//we can also deal with convex versus triangle (without connectivity data)
if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
if (dispatchInfo.m_enableSatConvex && polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
{
btVertexArray vertices;
btVertexArray worldSpaceVertices;
btTriangleShape* tri = (btTriangleShape*)polyhedronB;
vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
//tri->initializePolyhedralFeatures();
@ -579,17 +599,99 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
btScalar maxDist = threshold;
bool foundSepAxis = false;
if (0)
bool useSatSepNormal = true;
if (useSatSepNormal)
{
polyhedronB->initializePolyhedralFeatures();
#if 0
if (0)
{
//initializePolyhedralFeatures performs a convex hull computation, not needed for a single triangle
polyhedronB->initializePolyhedralFeatures();
} else
#endif
{
btVector3 uniqueEdges[3] = {tri->m_vertices1[1]-tri->m_vertices1[0],
tri->m_vertices1[2]-tri->m_vertices1[1],
tri->m_vertices1[0]-tri->m_vertices1[2]};
uniqueEdges[0].normalize();
uniqueEdges[1].normalize();
uniqueEdges[2].normalize();
btConvexPolyhedron polyhedron;
polyhedron.m_vertices.push_back(tri->m_vertices1[2]);
polyhedron.m_vertices.push_back(tri->m_vertices1[0]);
polyhedron.m_vertices.push_back(tri->m_vertices1[1]);
{
btFace combinedFaceA;
combinedFaceA.m_indices.push_back(0);
combinedFaceA.m_indices.push_back(1);
combinedFaceA.m_indices.push_back(2);
btVector3 faceNormal = uniqueEdges[0].cross(uniqueEdges[1]);
faceNormal.normalize();
btScalar planeEq=1e30f;
for (int v=0;v<combinedFaceA.m_indices.size();v++)
{
btScalar eq = tri->m_vertices1[combinedFaceA.m_indices[v]].dot(faceNormal);
if (planeEq>eq)
{
planeEq=eq;
}
}
combinedFaceA.m_plane[0] = faceNormal[0];
combinedFaceA.m_plane[1] = faceNormal[1];
combinedFaceA.m_plane[2] = faceNormal[2];
combinedFaceA.m_plane[3] = -planeEq;
polyhedron.m_faces.push_back(combinedFaceA);
}
{
btFace combinedFaceB;
combinedFaceB.m_indices.push_back(0);
combinedFaceB.m_indices.push_back(2);
combinedFaceB.m_indices.push_back(1);
btVector3 faceNormal = -uniqueEdges[0].cross(uniqueEdges[1]);
faceNormal.normalize();
btScalar planeEq=1e30f;
for (int v=0;v<combinedFaceB.m_indices.size();v++)
{
btScalar eq = tri->m_vertices1[combinedFaceB.m_indices[v]].dot(faceNormal);
if (planeEq>eq)
{
planeEq=eq;
}
}
combinedFaceB.m_plane[0] = faceNormal[0];
combinedFaceB.m_plane[1] = faceNormal[1];
combinedFaceB.m_plane[2] = faceNormal[2];
combinedFaceB.m_plane[3] = -planeEq;
polyhedron.m_faces.push_back(combinedFaceB);
}
polyhedron.m_uniqueEdges.push_back(uniqueEdges[0]);
polyhedron.m_uniqueEdges.push_back(uniqueEdges[1]);
polyhedron.m_uniqueEdges.push_back(uniqueEdges[2]);
polyhedron.initialize2();
polyhedronB->setPolyhedralFeatures(polyhedron);
}
foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
body0Wrap->getWorldTransform(),
body1Wrap->getWorldTransform(),
sepNormalWorldSpace,*resultOut);
*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
body0Wrap->getWorldTransform(),
body1Wrap->getWorldTransform(),
sepNormalWorldSpace,*resultOut);
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
} else
}
else
{
#ifdef ZERO_MARGIN
gjkPairDetector.setIgnoreMargin(true);
@ -598,6 +700,24 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN
if (dummy.m_hasContact && dummy.m_depth<0)
{
if (foundSepAxis)
{
if (dummy.m_normalOnBInWorld.dot(sepNormalWorldSpace)<0.99)
{
printf("?\n");
}
} else
{
printf("!\n");
}
sepNormalWorldSpace.setValue(0,0,1);// = dummy.m_normalOnBInWorld;
//minDist = dummy.m_depth;
foundSepAxis = true;
}
#if 0
btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
if (l2>SIMD_EPSILON)
{
@ -607,6 +727,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
foundSepAxis = true;
}
#endif
}
@ -614,7 +735,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
{
worldVertsB2.resize(0);
btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(),
body0Wrap->getWorldTransform(), vertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
body0Wrap->getWorldTransform(), worldSpaceVertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
}
@ -626,6 +747,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
return;
}
}

7
thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp vendored
View File

@ -20,11 +20,12 @@ subject to the following restrictions:
#include <stdio.h>
#ifdef BT_DEBUG_COLLISION_PAIRS
int gOverlappingSimplePairs = 0;
int gRemoveSimplePairs =0;
int gAddedSimplePairs =0;
int gFindSimplePairs =0;
#endif //BT_DEBUG_COLLISION_PAIRS
@ -61,7 +62,9 @@ void btHashedSimplePairCache::removeAllPairs()
btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB)
{
#ifdef BT_DEBUG_COLLISION_PAIRS
gFindSimplePairs++;
#endif
/*if (indexA > indexB)
@ -172,7 +175,9 @@ btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB)
void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB)
{
#ifdef BT_DEBUG_COLLISION_PAIRS
gRemoveSimplePairs++;
#endif
/*if (indexA > indexB)

6
thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h vendored
View File

@ -43,12 +43,12 @@ struct btSimplePair
typedef btAlignedObjectArray<btSimplePair> btSimplePairArray;
#ifdef BT_DEBUG_COLLISION_PAIRS
extern int gOverlappingSimplePairs;
extern int gRemoveSimplePairs;
extern int gAddedSimplePairs;
extern int gFindSimplePairs;
#endif //BT_DEBUG_COLLISION_PAIRS
@ -75,7 +75,9 @@ public:
// no new pair is created and the old one is returned.
virtual btSimplePair* addOverlappingPair(int indexA,int indexB)
{
#ifdef BT_DEBUG_COLLISION_PAIRS
gAddedSimplePairs++;
#endif
return internalAddPair(indexA,indexB);
}

17
thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp vendored
View File

@ -455,11 +455,20 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWr
btBvhTriangleMeshShape* trimesh = 0;
if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE )
trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
else
trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
{
trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
}
else
{
if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
}
}
if (trimesh==0)
return;
btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
if (!triangleInfoMapPtr)
return;

40
thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp vendored
View File

@ -199,6 +199,22 @@ class btPersistentManifoldSortPredicate
}
};
class btPersistentManifoldSortPredicateDeterministic
{
public:
SIMD_FORCE_INLINE bool operator() (const btPersistentManifold* lhs, const btPersistentManifold* rhs) const
{
return (
(getIslandId(lhs) < getIslandId(rhs))
|| ((getIslandId(lhs) == getIslandId(rhs)) && lhs->getBody0()->getBroadphaseHandle()->m_uniqueId < rhs->getBody0()->getBroadphaseHandle()->m_uniqueId)
||((getIslandId(lhs) == getIslandId(rhs)) && (lhs->getBody0()->getBroadphaseHandle()->m_uniqueId == rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) &&
(lhs->getBody1()->getBroadphaseHandle()->m_uniqueId < rhs->getBody1()->getBroadphaseHandle()->m_uniqueId))
);
}
};
void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
{
@ -245,13 +261,11 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
if (colObj0->getActivationState()== ACTIVE_TAG)
{
allSleeping = false;
}
if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
if (colObj0->getActivationState()== ACTIVE_TAG ||
colObj0->getActivationState()== DISABLE_DEACTIVATION)
{
allSleeping = false;
break;
}
}
}
@ -318,6 +332,11 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
for (i=0;i<maxNumManifolds ;i++)
{
btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
{
if (manifold->getNumContacts() == 0)
continue;
}
const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
@ -379,7 +398,16 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
//tried a radix sort, but quicksort/heapsort seems still faster
//@todo rewrite island management
m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
//btPersistentManifoldSortPredicateDeterministic sorts contact manifolds based on islandid,
//but also based on object0 unique id and object1 unique id
if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
{
m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic());
} else
{
m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
}
//m_islandmanifold.heapSort(btPersistentManifoldSortPredicate());
//now process all active islands (sets of manifolds for now)

1
thirdparty/bullet/BulletCollision/CollisionShapes/btCapsuleShape.h vendored
View File

@ -49,6 +49,7 @@ public:
virtual void setMargin(btScalar collisionMargin)
{
//don't override the margin for capsules, their entire radius == margin
(void)collisionMargin;
}
virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const

2
thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp vendored
View File

@ -213,7 +213,7 @@ void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co
void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const
void btCompoundShape::calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const
{
int n = m_children.size();

2
thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.h vendored
View File

@ -160,7 +160,7 @@ public:
///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound
///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
///of the collision object by the principal transform.
void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const;
void calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const;
int getUpdateRevision() const
{

12
thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp vendored
View File

@ -104,9 +104,9 @@ void btConvexPolyhedron::initialize()
btHashMap<btInternalVertexPair,btInternalEdge> edges;
btScalar TotalArea = 0.0f;
m_localCenter.setValue(0, 0, 0);
for(int i=0;i<m_faces.size();i++)
{
int numVertices = m_faces[i].m_indices.size();
@ -172,6 +172,13 @@ void btConvexPolyhedron::initialize()
}
#endif//USE_CONNECTED_FACES
initialize2();
}
void btConvexPolyhedron::initialize2()
{
m_localCenter.setValue(0, 0, 0);
btScalar TotalArea = 0.0f;
for(int i=0;i<m_faces.size();i++)
{
int numVertices = m_faces[i].m_indices.size();
@ -274,7 +281,6 @@ void btConvexPolyhedron::initialize()
}
#endif
}
void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
{
minProj = FLT_MAX;

1
thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.h vendored
View File

@ -54,6 +54,7 @@ ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron
btVector3 mE;
void initialize();
void initialize2();
bool testContainment() const;
void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;

8
thirdparty/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp vendored
View File

@ -118,9 +118,13 @@ static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector
return supVec;
#else
btScalar maxDot;
long ptIndex = vec.maxDot( points, numPoints, maxDot);
btScalar maxDot;
long ptIndex = vec.maxDot( points, numPoints, maxDot);
btAssert(ptIndex >= 0);
if (ptIndex<0)
{
ptIndex = 0;
}
btVector3 supVec = points[ptIndex] * localScaling;
return supVec;
#endif //__SPU__

532
thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.cpp vendored Normal file
View File

@ -0,0 +1,532 @@
#include "btMiniSDF.h"
//
//Based on code from DiscreGrid, https://github.com/InteractiveComputerGraphics/Discregrid
//example:
//GenerateSDF.exe -r "32 32 32" -d "-1.6 -1.6 -.6 1.6 1.6 .6" concave_box.obj
//The MIT License (MIT)
//
//Copyright (c) 2017 Dan Koschier
//
#include <limits.h>
#include <string.h> //memcpy
struct btSdfDataStream
{
const char* m_data;
int m_size;
int m_currentOffset;
btSdfDataStream(const char* data, int size)
:m_data(data),
m_size(size),
m_currentOffset(0)
{
}
template<class T> bool read(T& val)
{
int bytes = sizeof(T);
if (m_currentOffset+bytes<=m_size)
{
char* dest = (char*)&val;
memcpy(dest,&m_data[m_currentOffset],bytes);
m_currentOffset+=bytes;
return true;
}
btAssert(0);
return false;
}
};
bool btMiniSDF::load(const char* data, int size)
{
int fileSize = -1;
btSdfDataStream ds(data,size);
{
double buf[6];
ds.read(buf);
m_domain.m_min[0] = buf[0];
m_domain.m_min[1] = buf[1];
m_domain.m_min[2] = buf[2];
m_domain.m_min[3] = 0;
m_domain.m_max[0] = buf[3];
m_domain.m_max[1] = buf[4];
m_domain.m_max[2] = buf[5];
m_domain.m_max[3] = 0;
}
{
unsigned int buf2[3];
ds.read(buf2);
m_resolution[0] = buf2[0];
m_resolution[1] = buf2[1];
m_resolution[2] = buf2[2];
}
{
double buf[3];
ds.read(buf);
m_cell_size[0] = buf[0];
m_cell_size[1] = buf[1];
m_cell_size[2] = buf[2];
}
{
double buf[3];
ds.read(buf);
m_inv_cell_size[0] = buf[0];
m_inv_cell_size[1] = buf[1];
m_inv_cell_size[2] = buf[2];
}
{
unsigned long long int cells;
ds.read(cells);
m_n_cells = cells;
}
{
unsigned long long int fields;
ds.read(fields);
m_n_fields = fields;
}
unsigned long long int nodes0;
std::size_t n_nodes0;
ds.read(nodes0);
n_nodes0 = nodes0;
if (n_nodes0 > 1024 * 1024 * 1024)
{
return m_isValid;
}
m_nodes.resize(n_nodes0);
for (unsigned int i=0;i<n_nodes0;i++)
{
unsigned long long int n_nodes1;
ds.read(n_nodes1);
btAlignedObjectArray<double>& nodes = m_nodes[i];
nodes.resize(n_nodes1);
for ( int j=0;j<nodes.size();j++)
{
double& node = nodes[j];
ds.read(node);
}
}
unsigned long long int n_cells0;
ds.read(n_cells0);
m_cells.resize(n_cells0);
for (int i=0;i<n_cells0;i++)
{
unsigned long long int n_cells1;
btAlignedObjectArray<btCell32 >& cells = m_cells[i];
ds.read(n_cells1);
cells.resize(n_cells1);
for (int j=0;j<n_cells1;j++)
{
btCell32& cell = cells[j];
ds.read(cell);
}
}
{
unsigned long long int n_cell_maps0;
ds.read(n_cell_maps0);
m_cell_map.resize(n_cell_maps0);
for (int i=0;i<n_cell_maps0;i++)
{
unsigned long long int n_cell_maps1;
btAlignedObjectArray<unsigned int>& cell_maps = m_cell_map[i];
ds.read(n_cell_maps1);
cell_maps.resize(n_cell_maps1);
for (int j=0;j<n_cell_maps1;j++)
{
unsigned int& cell_map = cell_maps[j];
ds.read(cell_map);
}
}
}
m_isValid = (ds.m_currentOffset == ds.m_size);
return m_isValid;
}
unsigned int btMiniSDF::multiToSingleIndex(btMultiIndex const & ijk) const
{
return m_resolution[1] * m_resolution[0] * ijk.ijk[2] + m_resolution[0] * ijk.ijk[1] + ijk.ijk[0];
}
btAlignedBox3d
btMiniSDF::subdomain(btMultiIndex const& ijk) const
{
btAssert(m_isValid);
btVector3 tmp;
tmp.m_floats[0] = m_cell_size[0]*(double)ijk.ijk[0];
tmp.m_floats[1] = m_cell_size[1]*(double)ijk.ijk[1];
tmp.m_floats[2] = m_cell_size[2]*(double)ijk.ijk[2];
btVector3 origin = m_domain.min() + tmp;
btAlignedBox3d box = btAlignedBox3d (origin, origin + m_cell_size);
return box;
}
btMultiIndex
btMiniSDF::singleToMultiIndex(unsigned int l) const
{
btAssert(m_isValid);
unsigned int n01 = m_resolution[0] * m_resolution[1];
unsigned int k = l / n01;
unsigned int temp = l % n01;
unsigned int j = temp / m_resolution[0];
unsigned int i = temp % m_resolution[0];
btMultiIndex mi;
mi.ijk[0] = i;
mi.ijk[1] = j;
mi.ijk[2] = k;
return mi;
}
btAlignedBox3d
btMiniSDF::subdomain(unsigned int l) const
{
btAssert(m_isValid);
return subdomain(singleToMultiIndex(l));
}
btShapeMatrix
btMiniSDF::shape_function_(btVector3 const& xi, btShapeGradients* gradient) const
{
btAssert(m_isValid);
btShapeMatrix res;
btScalar x = xi[0];
btScalar y = xi[1];
btScalar z = xi[2];
btScalar x2 = x*x;
btScalar y2 = y*y;
btScalar z2 = z*z;
btScalar _1mx = 1.0 - x;
btScalar _1my = 1.0 - y;
btScalar _1mz = 1.0 - z;
btScalar _1px = 1.0 + x;
btScalar _1py = 1.0 + y;
btScalar _1pz = 1.0 + z;
btScalar _1m3x = 1.0 - 3.0 * x;
btScalar _1m3y = 1.0 - 3.0 * y;
btScalar _1m3z = 1.0 - 3.0 * z;
btScalar _1p3x = 1.0 + 3.0 * x;
btScalar _1p3y = 1.0 + 3.0 * y;
btScalar _1p3z = 1.0 + 3.0 * z;
btScalar _1mxt1my = _1mx * _1my;
btScalar _1mxt1py = _1mx * _1py;
btScalar _1pxt1my = _1px * _1my;
btScalar _1pxt1py = _1px * _1py;
btScalar _1mxt1mz = _1mx * _1mz;
btScalar _1mxt1pz = _1mx * _1pz;
btScalar _1pxt1mz = _1px * _1mz;
btScalar _1pxt1pz = _1px * _1pz;
btScalar _1myt1mz = _1my * _1mz;
btScalar _1myt1pz = _1my * _1pz;
btScalar _1pyt1mz = _1py * _1mz;
btScalar _1pyt1pz = _1py * _1pz;
btScalar _1mx2 = 1.0 - x2;
btScalar _1my2 = 1.0 - y2;
btScalar _1mz2 = 1.0 - z2;
// Corner nodes.
btScalar fac = 1.0 / 64.0 * (9.0 * (x2 + y2 + z2) - 19.0);
res[0] = fac * _1mxt1my * _1mz;
res[1] = fac * _1pxt1my * _1mz;
res[2] = fac * _1mxt1py * _1mz;
res[3] = fac * _1pxt1py * _1mz;
res[4] = fac * _1mxt1my * _1pz;
res[5] = fac * _1pxt1my * _1pz;
res[6] = fac * _1mxt1py * _1pz;
res[7] = fac * _1pxt1py * _1pz;
// Edge nodes.
fac = 9.0 / 64.0 * _1mx2;
btScalar fact1m3x = fac * _1m3x;
btScalar fact1p3x = fac * _1p3x;
res[ 8] = fact1m3x * _1myt1mz;
res[ 9] = fact1p3x * _1myt1mz;
res[10] = fact1m3x * _1myt1pz;
res[11] = fact1p3x * _1myt1pz;
res[12] = fact1m3x * _1pyt1mz;
res[13] = fact1p3x * _1pyt1mz;
res[14] = fact1m3x * _1pyt1pz;
res[15] = fact1p3x * _1pyt1pz;
fac = 9.0 / 64.0 * _1my2;
btScalar fact1m3y = fac * _1m3y;
btScalar fact1p3y = fac * _1p3y;
res[16] = fact1m3y * _1mxt1mz;
res[17] = fact1p3y * _1mxt1mz;
res[18] = fact1m3y * _1pxt1mz;
res[19] = fact1p3y * _1pxt1mz;
res[20] = fact1m3y * _1mxt1pz;
res[21] = fact1p3y * _1mxt1pz;
res[22] = fact1m3y * _1pxt1pz;
res[23] = fact1p3y * _1pxt1pz;
fac = 9.0 / 64.0 * _1mz2;
btScalar fact1m3z = fac * _1m3z;
btScalar fact1p3z = fac * _1p3z;
res[24] = fact1m3z * _1mxt1my;
res[25] = fact1p3z * _1mxt1my;
res[26] = fact1m3z * _1mxt1py;
res[27] = fact1p3z * _1mxt1py;
res[28] = fact1m3z * _1pxt1my;
res[29] = fact1p3z * _1pxt1my;
res[30] = fact1m3z * _1pxt1py;
res[31] = fact1p3z * _1pxt1py;
if (gradient)
{
btShapeGradients& dN = *gradient;
btScalar _9t3x2py2pz2m19 = 9.0 * (3.0 * x2 + y2 + z2) - 19.0;
btScalar _9tx2p3y2pz2m19 = 9.0 * (x2 + 3.0 * y2 + z2) - 19.0;
btScalar _9tx2py2p3z2m19 = 9.0 * (x2 + y2 + 3.0 * z2) - 19.0;
btScalar _18x = 18.0 * x;
btScalar _18y = 18.0 * y;
btScalar _18z = 18.0 * z;
btScalar _3m9x2 = 3.0 - 9.0 * x2;
btScalar _3m9y2 = 3.0 - 9.0 * y2;
btScalar _3m9z2 = 3.0 - 9.0 * z2;
btScalar _2x = 2.0 * x;
btScalar _2y = 2.0 * y;
btScalar _2z = 2.0 * z;
btScalar _18xm9t3x2py2pz2m19 = _18x - _9t3x2py2pz2m19;
btScalar _18xp9t3x2py2pz2m19 = _18x + _9t3x2py2pz2m19;
btScalar _18ym9tx2p3y2pz2m19 = _18y - _9tx2p3y2pz2m19;
btScalar _18yp9tx2p3y2pz2m19 = _18y + _9tx2p3y2pz2m19;
btScalar _18zm9tx2py2p3z2m19 = _18z - _9tx2py2p3z2m19;
btScalar _18zp9tx2py2p3z2m19 = _18z + _9tx2py2p3z2m19;
dN(0,0) =_18xm9t3x2py2pz2m19 * _1myt1mz;
dN(0,1) =_1mxt1mz * _18ym9tx2p3y2pz2m19;
dN(0,2) =_1mxt1my * _18zm9tx2py2p3z2m19;
dN(1,0) =_18xp9t3x2py2pz2m19 * _1myt1mz;
dN(1,1) =_1pxt1mz * _18ym9tx2p3y2pz2m19;
dN(1,2) =_1pxt1my * _18zm9tx2py2p3z2m19;
dN(2,0) =_18xm9t3x2py2pz2m19 * _1pyt1mz;
dN(2,1) =_1mxt1mz * _18yp9tx2p3y2pz2m19;
dN(2,2) =_1mxt1py * _18zm9tx2py2p3z2m19;
dN(3,0) =_18xp9t3x2py2pz2m19 * _1pyt1mz;
dN(3,1) =_1pxt1mz * _18yp9tx2p3y2pz2m19;
dN(3,2) =_1pxt1py * _18zm9tx2py2p3z2m19;
dN(4,0) =_18xm9t3x2py2pz2m19 * _1myt1pz;
dN(4,1) =_1mxt1pz * _18ym9tx2p3y2pz2m19;
dN(4,2) =_1mxt1my * _18zp9tx2py2p3z2m19;
dN(5,0) =_18xp9t3x2py2pz2m19 * _1myt1pz;
dN(5,1) =_1pxt1pz * _18ym9tx2p3y2pz2m19;
dN(5,2) =_1pxt1my * _18zp9tx2py2p3z2m19;
dN(6,0) =_18xm9t3x2py2pz2m19 * _1pyt1pz;
dN(6,1) =_1mxt1pz * _18yp9tx2p3y2pz2m19;
dN(6,2) =_1mxt1py * _18zp9tx2py2p3z2m19;
dN(7,0) =_18xp9t3x2py2pz2m19 * _1pyt1pz;
dN(7,1) =_1pxt1pz * _18yp9tx2p3y2pz2m19;
dN(7,2) =_1pxt1py * _18zp9tx2py2p3z2m19;
dN.topRowsDivide(8, 64.0);
btScalar _m3m9x2m2x = -_3m9x2 - _2x;
btScalar _p3m9x2m2x = _3m9x2 - _2x;
btScalar _1mx2t1m3x = _1mx2 * _1m3x;
btScalar _1mx2t1p3x = _1mx2 * _1p3x;
dN( 8,0) = _m3m9x2m2x * _1myt1mz,
dN( 8,1) = -_1mx2t1m3x * _1mz,
dN( 8,2) = -_1mx2t1m3x * _1my;
dN( 9,0) = _p3m9x2m2x * _1myt1mz,
dN( 9,1) = -_1mx2t1p3x * _1mz,
dN( 9,2) = -_1mx2t1p3x * _1my;
dN(10,0) = _m3m9x2m2x * _1myt1pz,
dN(10,1) = -_1mx2t1m3x * _1pz,
dN(10,2) = _1mx2t1m3x * _1my;
dN(11,0) = _p3m9x2m2x * _1myt1pz,
dN(11,1) = -_1mx2t1p3x * _1pz,
dN(11,2) = _1mx2t1p3x * _1my;
dN(12,0) = _m3m9x2m2x * _1pyt1mz,
dN(12,1) = _1mx2t1m3x * _1mz,
dN(12,2) = -_1mx2t1m3x * _1py;
dN(13,0) = _p3m9x2m2x * _1pyt1mz,
dN(13,1) = _1mx2t1p3x * _1mz,
dN(13,2) = -_1mx2t1p3x * _1py;
dN(14,0) = _m3m9x2m2x * _1pyt1pz,
dN(14,1) = _1mx2t1m3x * _1pz,
dN(14,2) = _1mx2t1m3x * _1py;
dN(15,0) = _p3m9x2m2x * _1pyt1pz,
dN(15,1) = _1mx2t1p3x * _1pz,
dN(15,2) = _1mx2t1p3x * _1py;
btScalar _m3m9y2m2y = -_3m9y2 - _2y;
btScalar _p3m9y2m2y = _3m9y2 - _2y;
btScalar _1my2t1m3y = _1my2 * _1m3y;
btScalar _1my2t1p3y = _1my2 * _1p3y;
dN(16,0) = -_1my2t1m3y * _1mz,
dN(16,1) = _m3m9y2m2y * _1mxt1mz,
dN(16,2) = -_1my2t1m3y * _1mx;
dN(17,0) = -_1my2t1p3y * _1mz,
dN(17,1) = _p3m9y2m2y * _1mxt1mz,
dN(17,2) = -_1my2t1p3y * _1mx;
dN(18,0) = _1my2t1m3y * _1mz,
dN(18,1) = _m3m9y2m2y * _1pxt1mz,
dN(18,2) = -_1my2t1m3y * _1px;
dN(19,0) = _1my2t1p3y * _1mz,
dN(19,1) = _p3m9y2m2y * _1pxt1mz,
dN(19,2) = -_1my2t1p3y * _1px;
dN(20,0) = -_1my2t1m3y * _1pz,
dN(20,1) = _m3m9y2m2y * _1mxt1pz,
dN(20,2) = _1my2t1m3y * _1mx;
dN(21,0) = -_1my2t1p3y * _1pz,
dN(21,1) = _p3m9y2m2y * _1mxt1pz,
dN(21,2) = _1my2t1p3y * _1mx;
dN(22,0) = _1my2t1m3y * _1pz,
dN(22,1) = _m3m9y2m2y * _1pxt1pz,
dN(22,2) = _1my2t1m3y * _1px;
dN(23,0) = _1my2t1p3y * _1pz,
dN(23,1) = _p3m9y2m2y * _1pxt1pz,
dN(23,2) = _1my2t1p3y * _1px;
btScalar _m3m9z2m2z = -_3m9z2 - _2z;
btScalar _p3m9z2m2z = _3m9z2 - _2z;
btScalar _1mz2t1m3z = _1mz2 * _1m3z;
btScalar _1mz2t1p3z = _1mz2 * _1p3z;
dN(24,0) = -_1mz2t1m3z * _1my,
dN(24,1) = -_1mz2t1m3z * _1mx,
dN(24,2) = _m3m9z2m2z * _1mxt1my;
dN(25,0) = -_1mz2t1p3z * _1my,
dN(25,1) = -_1mz2t1p3z * _1mx,
dN(25,2) = _p3m9z2m2z * _1mxt1my;
dN(26,0) = -_1mz2t1m3z * _1py,
dN(26,1) = _1mz2t1m3z * _1mx,
dN(26,2) = _m3m9z2m2z * _1mxt1py;
dN(27,0) = -_1mz2t1p3z * _1py,
dN(27,1) = _1mz2t1p3z * _1mx,
dN(27,2) = _p3m9z2m2z * _1mxt1py;
dN(28,0) = _1mz2t1m3z * _1my,
dN(28,1) = -_1mz2t1m3z * _1px,
dN(28,2) = _m3m9z2m2z * _1pxt1my;
dN(29,0) = _1mz2t1p3z * _1my,
dN(29,1) = -_1mz2t1p3z * _1px,
dN(29,2) = _p3m9z2m2z * _1pxt1my;
dN(30,0) = _1mz2t1m3z * _1py,
dN(30,1) = _1mz2t1m3z * _1px,
dN(30,2) = _m3m9z2m2z * _1pxt1py;
dN(31,0) = _1mz2t1p3z * _1py,
dN(31,1) = _1mz2t1p3z * _1px,
dN(31,2) = _p3m9z2m2z * _1pxt1py;
dN.bottomRowsMul(32u - 8u, 9.0 / 64.0);
}
return res;
}
bool btMiniSDF::interpolate(unsigned int field_id, double& dist, btVector3 const& x,
btVector3* gradient) const
{
btAssert(m_isValid);
if (!m_isValid)
return false;
if (!m_domain.contains(x))
return false;
btVector3 tmpmi = ((x - m_domain.min())*(m_inv_cell_size));//.cast<unsigned int>().eval();
unsigned int mi[3] = {(unsigned int )tmpmi[0],(unsigned int )tmpmi[1],(unsigned int )tmpmi[2]};
if (mi[0] >= m_resolution[0])
mi[0] = m_resolution[0]-1;
if (mi[1] >= m_resolution[1])
mi[1] = m_resolution[1]-1;
if (mi[2] >= m_resolution[2])
mi[2] = m_resolution[2]-1;
btMultiIndex mui;
mui.ijk[0] = mi[0];
mui.ijk[1] = mi[1];
mui.ijk[2] = mi[2];
int i = multiToSingleIndex(mui);
unsigned int i_ = m_cell_map[field_id][i];
if (i_ == UINT_MAX)
return false;
btAlignedBox3d sd = subdomain(i);
i = i_;
btVector3 d = sd.m_max-sd.m_min;//.diagonal().eval();
btVector3 denom = (sd.max() - sd.min());
btVector3 c0 = btVector3(2.0,2.0,2.0)/denom;
btVector3 c1 = (sd.max() + sd.min())/denom;
btVector3 xi = (c0*x - c1);
btCell32 const& cell = m_cells[field_id][i];
if (!gradient)
{
//auto phi = m_coefficients[field_id][i].dot(shape_function_(xi, 0));
double phi = 0.0;
btShapeMatrix N = shape_function_(xi, 0);
for (unsigned int j = 0u; j < 32u; ++j)
{
unsigned int v = cell.m_cells[j];
double c = m_nodes[field_id][v];
if (c == DBL_MAX)
{
return false;;
}
phi += c * N[j];
}
dist = phi;
return true;
}
btShapeGradients dN;
btShapeMatrix N = shape_function_(xi, &dN);
double phi = 0.0;
gradient->setZero();
for (unsigned int j = 0u; j < 32u; ++j)
{
unsigned int v = cell.m_cells[j];
double c = m_nodes[field_id][v];
if (c == DBL_MAX)
{
gradient->setZero();
return false;
}
phi += c * N[j];
(*gradient)[0] += c * dN(j, 0);
(*gradient)[1] += c * dN(j, 1);
(*gradient)[2] += c * dN(j, 2);
}
(*gradient) *= c0;
dist = phi;
return true;
}

134
thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.h vendored Normal file
View File

@ -0,0 +1,134 @@
#ifndef MINISDF_H
#define MINISDF_H
#include "LinearMath/btVector3.h"
#include "LinearMath/btAabbUtil2.h"
#include "LinearMath/btAlignedObjectArray.h"
struct btMultiIndex
{
unsigned int ijk[3];
};
struct btAlignedBox3d
{
btVector3 m_min;
btVector3 m_max;
const btVector3& min() const
{
return m_min;
}
const btVector3& max() const
{
return m_max;
}
bool contains(const btVector3& x) const
{
return TestPointAgainstAabb2(m_min, m_max, x);
}
btAlignedBox3d(const btVector3& mn, const btVector3& mx)
:m_min(mn),
m_max(mx)
{
}
btAlignedBox3d()
{
}
};
struct btShapeMatrix
{
double m_vec[32];
inline double& operator[](int i)
{
return m_vec[i];
}
inline const double& operator[](int i) const
{
return m_vec[i];
}
};
struct btShapeGradients
{
btVector3 m_vec[32];
void topRowsDivide(int row, double denom)
{
for (int i=0;i<row;i++)
{
m_vec[i] /= denom;
}
}
void bottomRowsMul(int row, double val)
{
for (int i=32-row;i<32;i++)
{
m_vec[i] *= val;
}
}
inline btScalar& operator()(int i, int j)
{
return m_vec[i][j];
}
};
struct btCell32
{
unsigned int m_cells[32];
};
struct btMiniSDF
{
btAlignedBox3d m_domain;
unsigned int m_resolution[3];
btVector3 m_cell_size;
btVector3 m_inv_cell_size;
std::size_t m_n_cells;
std::size_t m_n_fields;
bool m_isValid;
btAlignedObjectArray<btAlignedObjectArray<double> > m_nodes;
btAlignedObjectArray<btAlignedObjectArray<btCell32 > > m_cells;
btAlignedObjectArray<btAlignedObjectArray<unsigned int> > m_cell_map;
btMiniSDF()
:m_isValid(false)
{
}
bool load(const char* data, int size);
bool isValid() const
{
return m_isValid;
}
unsigned int multiToSingleIndex(btMultiIndex const & ijk) const;
btAlignedBox3d subdomain(btMultiIndex const& ijk) const;
btMultiIndex singleToMultiIndex(unsigned int l) const;
btAlignedBox3d subdomain(unsigned int l) const;
btShapeMatrix
shape_function_(btVector3 const& xi, btShapeGradients* gradient = 0) const;
bool interpolate(unsigned int field_id, double& dist, btVector3 const& x, btVector3* gradient) const;
};
#endif //MINISDF_H

77
thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp vendored
View File

@ -39,6 +39,17 @@ btPolyhedralConvexShape::~btPolyhedralConvexShape()
}
}
void btPolyhedralConvexShape::setPolyhedralFeatures(btConvexPolyhedron& polyhedron)
{
if (m_polyhedron)
{
*m_polyhedron = polyhedron;
} else
{
void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
m_polyhedron = new (mem) btConvexPolyhedron(polyhedron);
}
}
bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin)
{
@ -87,7 +98,71 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);
}
#ifndef BT_RECONSTRUCT_FACES
int numVertices = conv.vertices.size();
m_polyhedron->m_vertices.resize(numVertices);
for (int p=0;p<numVertices;p++)
{
m_polyhedron->m_vertices[p] = conv.vertices[p];
}
int v0, v1;
for (int j = 0; j < conv.faces.size(); j++)
{
btVector3 edges[3];
int numEdges = 0;
btFace combinedFace;
const btConvexHullComputer::Edge* edge = &conv.edges[conv.faces[j]];
v0 = edge->getSourceVertex();
int prevVertex=v0;
combinedFace.m_indices.push_back(v0);
v1 = edge->getTargetVertex();
while (v1 != v0)
{
btVector3 wa = conv.vertices[prevVertex];
btVector3 wb = conv.vertices[v1];
btVector3 newEdge = wb-wa;
newEdge.normalize();
if (numEdges<2)
edges[numEdges++] = newEdge;
//face->addIndex(v1);
combinedFace.m_indices.push_back(v1);
edge = edge->getNextEdgeOfFace();
prevVertex = v1;
int v01 = edge->getSourceVertex();
v1 = edge->getTargetVertex();
}
btAssert(combinedFace.m_indices.size() > 2);
btVector3 faceNormal = edges[0].cross(edges[1]);
faceNormal.normalize();
btScalar planeEq=1e30f;
for (int v=0;v<combinedFace.m_indices.size();v++)
{
btScalar eq = m_polyhedron->m_vertices[combinedFace.m_indices[v]].dot(faceNormal);
if (planeEq>eq)
{
planeEq=eq;
}
}
combinedFace.m_plane[0] = faceNormal.getX();
combinedFace.m_plane[1] = faceNormal.getY();
combinedFace.m_plane[2] = faceNormal.getZ();
combinedFace.m_plane[3] = -planeEq;
m_polyhedron->m_faces.push_back(combinedFace);
}
#else//BT_RECONSTRUCT_FACES
btAlignedObjectArray<btVector3> faceNormals;
int numFaces = conv.faces.size();
@ -312,6 +387,8 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
}
#endif //BT_RECONSTRUCT_FACES
m_polyhedron->initialize();
return true;

3
thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h vendored
View File

@ -43,6 +43,9 @@ public:
///experimental/work-in-progress
virtual bool initializePolyhedralFeatures(int shiftVerticesByMargin=0);
virtual void setPolyhedralFeatures(btConvexPolyhedron& polyhedron);
const btConvexPolyhedron* getConvexPolyhedron() const
{
return m_polyhedron;

99
thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp vendored Normal file
View File

@ -0,0 +1,99 @@
#include "btSdfCollisionShape.h"
#include "btMiniSDF.h"
#include "LinearMath/btAabbUtil2.h"
struct btSdfCollisionShapeInternalData
{
btVector3 m_localScaling;
btScalar m_margin;
btMiniSDF m_sdf;
btSdfCollisionShapeInternalData()
:m_localScaling(1,1,1),
m_margin(0)
{
}
};
bool btSdfCollisionShape::initializeSDF(const char* sdfData, int sizeInBytes)
{
bool valid = m_data->m_sdf.load(sdfData, sizeInBytes);
return valid;
}
btSdfCollisionShape::btSdfCollisionShape()
{
m_shapeType = SDF_SHAPE_PROXYTYPE;
m_data = new btSdfCollisionShapeInternalData();
//"E:/develop/bullet3/data/toys/ground_hole64_64_8.cdf");//ground_cube.cdf");
/*unsigned int field_id=0;
Eigen::Vector3d x (1,10,1);
Eigen::Vector3d gradient;
double dist = m_data->m_sdf.interpolate(field_id, x, &gradient);
printf("dist=%g\n", dist);
*/
}
btSdfCollisionShape::~btSdfCollisionShape()
{
delete m_data;
}
void btSdfCollisionShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
{
btAssert(m_data->m_sdf.isValid());
btVector3 localAabbMin = m_data->m_sdf.m_domain.m_min;
btVector3 localAabbMax = m_data->m_sdf.m_domain.m_max;
btScalar margin(0);
btTransformAabb(localAabbMin,localAabbMax,margin,t,aabbMin,aabbMax);
}
void btSdfCollisionShape::setLocalScaling(const btVector3& scaling)
{
m_data->m_localScaling = scaling;
}
const btVector3& btSdfCollisionShape::getLocalScaling() const
{
return m_data->m_localScaling;
}
void btSdfCollisionShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
{
inertia.setValue(0,0,0);
}
const char* btSdfCollisionShape::getName()const
{
return "btSdfCollisionShape";
}
void btSdfCollisionShape::setMargin(btScalar margin)
{
m_data->m_margin = margin;
}
btScalar btSdfCollisionShape::getMargin() const
{
return m_data->m_margin;
}
void btSdfCollisionShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
{
//not yet
}
bool btSdfCollisionShape::queryPoint(const btVector3& ptInSDF, btScalar& distOut, btVector3& normal)
{
int field = 0;
btVector3 grad;
double dist;
bool hasResult = m_data->m_sdf.interpolate(field,dist, ptInSDF,&grad);
if (hasResult)
{
normal.setValue(grad[0],grad[1],grad[2]);
distOut= dist;
}
return hasResult;
}

30
thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.h vendored Normal file
View File

@ -0,0 +1,30 @@
#ifndef BT_SDF_COLLISION_SHAPE_H
#define BT_SDF_COLLISION_SHAPE_H
#include "btConcaveShape.h"
class btSdfCollisionShape : public btConcaveShape
{
struct btSdfCollisionShapeInternalData* m_data;
public:
btSdfCollisionShape();
virtual ~btSdfCollisionShape();
bool initializeSDF(const char* sdfData, int sizeInBytes);
virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
virtual void setLocalScaling(const btVector3& scaling);
virtual const btVector3& getLocalScaling() const;
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
virtual const char* getName()const;
virtual void setMargin(btScalar margin);
virtual btScalar getMargin() const;
virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
bool queryPoint(const btVector3& ptInSDF, btScalar& distOut, btVector3& normal);
};
#endif //BT_SDF_COLLISION_SHAPE_H

277
thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.cpp vendored
View File

@ -20,6 +20,8 @@ subject to the following restrictions:
#include "LinearMath/btConvexHull.h"
#define NUM_UNITSPHERE_POINTS 42
#define NUM_UNITSPHERE_POINTS_HIGHRES 256
btShapeHull::btShapeHull (const btConvexShape* shape)
{
@ -36,9 +38,9 @@ btShapeHull::~btShapeHull ()
}
bool
btShapeHull::buildHull (btScalar /*margin*/)
btShapeHull::buildHull (btScalar /*margin*/, int highres)
{
int numSampleDirections = NUM_UNITSPHERE_POINTS;
int numSampleDirections = highres? NUM_UNITSPHERE_POINTS_HIGHRES:NUM_UNITSPHERE_POINTS;
{
int numPDA = m_shape->getNumPreferredPenetrationDirections();
if (numPDA)
@ -47,17 +49,17 @@ btShapeHull::buildHull (btScalar /*margin*/)
{
btVector3 norm;
m_shape->getPreferredPenetrationDirection(i,norm);
getUnitSpherePoints()[numSampleDirections] = norm;
getUnitSpherePoints(highres)[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
btVector3 supportPoints[NUM_UNITSPHERE_POINTS_HIGHRES+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
int i;
for (i = 0; i < numSampleDirections; i++)
{
supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints()[i]);
supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints(highres)[i]);
}
HullDesc hd;
@ -118,9 +120,268 @@ btShapeHull::numIndices () const
}
btVector3* btShapeHull::getUnitSpherePoints()
btVector3* btShapeHull::getUnitSpherePoints(int highres)
{
static btVector3 sUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
static btVector3 sUnitSpherePointsHighres[NUM_UNITSPHERE_POINTS_HIGHRES + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] =
{
btVector3(btScalar(0.997604), btScalar(0.067004), btScalar(0.017144)),
btVector3(btScalar(0.984139), btScalar(-0.086784), btScalar(-0.154427)),
btVector3(btScalar(0.971065), btScalar(0.124164), btScalar(-0.203224)),
btVector3(btScalar(0.955844), btScalar(0.291173), btScalar(-0.037704)),
btVector3(btScalar(0.957405), btScalar(0.212238), btScalar(0.195157)),
btVector3(btScalar(0.971650), btScalar(-0.012709), btScalar(0.235561)),
btVector3(btScalar(0.984920), btScalar(-0.161831), btScalar(0.059695)),
btVector3(btScalar(0.946673), btScalar(-0.299288), btScalar(-0.117536)),
btVector3(btScalar(0.922670), btScalar(-0.219186), btScalar(-0.317019)),
btVector3(btScalar(0.928134), btScalar(-0.007265), btScalar(-0.371867)),
btVector3(btScalar(0.875642), btScalar(0.198434), btScalar(-0.439988)),
btVector3(btScalar(0.908035), btScalar(0.325975), btScalar(-0.262562)),
btVector3(btScalar(0.864519), btScalar(0.488706), btScalar(-0.116755)),
btVector3(btScalar(0.893009), btScalar(0.428046), btScalar(0.137185)),
btVector3(btScalar(0.857494), btScalar(0.362137), btScalar(0.364776)),
btVector3(btScalar(0.900815), btScalar(0.132524), btScalar(0.412987)),
btVector3(btScalar(0.934964), btScalar(-0.241739), btScalar(0.259179)),
btVector3(btScalar(0.894570), btScalar(-0.103504), btScalar(0.434263)),
btVector3(btScalar(0.922085), btScalar(-0.376668), btScalar(0.086241)),
btVector3(btScalar(0.862177), btScalar(-0.499154), btScalar(-0.085330)),
btVector3(btScalar(0.861982), btScalar(-0.420218), btScalar(-0.282861)),
btVector3(btScalar(0.818076), btScalar(-0.328256), btScalar(-0.471804)),
btVector3(btScalar(0.762657), btScalar(-0.179329), btScalar(-0.621124)),
btVector3(btScalar(0.826857), btScalar(0.019760), btScalar(-0.561786)),
btVector3(btScalar(0.731434), btScalar(0.206599), btScalar(-0.649817)),
btVector3(btScalar(0.769486), btScalar(0.379052), btScalar(-0.513770)),
btVector3(btScalar(0.796806), btScalar(0.507176), btScalar(-0.328145)),
btVector3(btScalar(0.679722), btScalar(0.684101), btScalar(-0.264123)),
btVector3(btScalar(0.786854), btScalar(0.614886), btScalar(0.050912)),
btVector3(btScalar(0.769486), btScalar(0.571141), btScalar(0.285139)),
btVector3(btScalar(0.707432), btScalar(0.492789), btScalar(0.506288)),
btVector3(btScalar(0.774560), btScalar(0.268037), btScalar(0.572652)),
btVector3(btScalar(0.796220), btScalar(0.031230), btScalar(0.604077)),
btVector3(btScalar(0.837395), btScalar(-0.320285), btScalar(0.442461)),
btVector3(btScalar(0.848127), btScalar(-0.450548), btScalar(0.278307)),
btVector3(btScalar(0.775536), btScalar(-0.206354), btScalar(0.596465)),
btVector3(btScalar(0.816320), btScalar(-0.567007), btScalar(0.109469)),
btVector3(btScalar(0.741191), btScalar(-0.668690), btScalar(-0.056832)),
btVector3(btScalar(0.755632), btScalar(-0.602975), btScalar(-0.254949)),
btVector3(btScalar(0.720311), btScalar(-0.521318), btScalar(-0.457165)),
btVector3(btScalar(0.670746), btScalar(-0.386583), btScalar(-0.632835)),
btVector3(btScalar(0.587031), btScalar(-0.219769), btScalar(-0.778836)),
btVector3(btScalar(0.676015), btScalar(-0.003182), btScalar(-0.736676)),
btVector3(btScalar(0.566932), btScalar(0.186963), btScalar(-0.802064)),
btVector3(btScalar(0.618254), btScalar(0.398105), btScalar(-0.677533)),
btVector3(btScalar(0.653964), btScalar(0.575224), btScalar(-0.490933)),
btVector3(btScalar(0.525367), btScalar(0.743205), btScalar(-0.414028)),
btVector3(btScalar(0.506439), btScalar(0.836528), btScalar(-0.208885)),
btVector3(btScalar(0.651427), btScalar(0.756426), btScalar(-0.056247)),
btVector3(btScalar(0.641670), btScalar(0.745149), btScalar(0.180908)),
btVector3(btScalar(0.602643), btScalar(0.687211), btScalar(0.405180)),
btVector3(btScalar(0.516586), btScalar(0.596999), btScalar(0.613447)),
btVector3(btScalar(0.602252), btScalar(0.387801), btScalar(0.697573)),
btVector3(btScalar(0.646549), btScalar(0.153911), btScalar(0.746956)),
btVector3(btScalar(0.650842), btScalar(-0.087756), btScalar(0.753983)),
btVector3(btScalar(0.740411), btScalar(-0.497404), btScalar(0.451830)),
btVector3(btScalar(0.726946), btScalar(-0.619890), btScalar(0.295093)),
btVector3(btScalar(0.637768), btScalar(-0.313092), btScalar(0.703624)),
btVector3(btScalar(0.678942), btScalar(-0.722934), btScalar(0.126645)),
btVector3(btScalar(0.489072), btScalar(-0.867195), btScalar(-0.092942)),
btVector3(btScalar(0.622742), btScalar(-0.757541), btScalar(-0.194636)),
btVector3(btScalar(0.596788), btScalar(-0.693576), btScalar(-0.403098)),
btVector3(btScalar(0.550150), btScalar(-0.582172), btScalar(-0.598287)),
btVector3(btScalar(0.474436), btScalar(-0.429745), btScalar(-0.768101)),
btVector3(btScalar(0.372574), btScalar(-0.246016), btScalar(-0.894583)),
btVector3(btScalar(0.480095), btScalar(-0.026513), btScalar(-0.876626)),
btVector3(btScalar(0.352474), btScalar(0.177242), btScalar(-0.918787)),
btVector3(btScalar(0.441848), btScalar(0.374386), btScalar(-0.814946)),
btVector3(btScalar(0.492389), btScalar(0.582223), btScalar(-0.646693)),
btVector3(btScalar(0.343498), btScalar(0.866080), btScalar(-0.362693)),
btVector3(btScalar(0.362036), btScalar(0.745149), btScalar(-0.559639)),
btVector3(btScalar(0.334131), btScalar(0.937044), btScalar(-0.099774)),
btVector3(btScalar(0.486925), btScalar(0.871718), btScalar(0.052473)),
btVector3(btScalar(0.452776), btScalar(0.845665), btScalar(0.281820)),
btVector3(btScalar(0.399503), btScalar(0.771785), btScalar(0.494576)),
btVector3(btScalar(0.296469), btScalar(0.673018), btScalar(0.677469)),
btVector3(btScalar(0.392088), btScalar(0.479179), btScalar(0.785213)),
btVector3(btScalar(0.452190), btScalar(0.252094), btScalar(0.855286)),
btVector3(btScalar(0.478339), btScalar(0.013149), btScalar(0.877928)),
btVector3(btScalar(0.481656), btScalar(-0.219380), btScalar(0.848259)),
btVector3(btScalar(0.615327), btScalar(-0.494293), btScalar(0.613837)),
btVector3(btScalar(0.594642), btScalar(-0.650414), btScalar(0.472325)),
btVector3(btScalar(0.562249), btScalar(-0.771345), btScalar(0.297631)),
btVector3(btScalar(0.467411), btScalar(-0.437133), btScalar(0.768231)),
btVector3(btScalar(0.519513), btScalar(-0.847947), btScalar(0.103808)),
btVector3(btScalar(0.297640), btScalar(-0.938159), btScalar(-0.176288)),
btVector3(btScalar(0.446727), btScalar(-0.838615), btScalar(-0.311359)),
btVector3(btScalar(0.331790), btScalar(-0.942437), btScalar(0.040762)),
btVector3(btScalar(0.413358), btScalar(-0.748403), btScalar(-0.518259)),
btVector3(btScalar(0.347596), btScalar(-0.621640), btScalar(-0.701737)),
btVector3(btScalar(0.249831), btScalar(-0.456186), btScalar(-0.853984)),
btVector3(btScalar(0.131772), btScalar(-0.262931), btScalar(-0.955678)),
btVector3(btScalar(0.247099), btScalar(-0.042261), btScalar(-0.967975)),
btVector3(btScalar(0.113624), btScalar(0.165965), btScalar(-0.979491)),
btVector3(btScalar(0.217438), btScalar(0.374580), btScalar(-0.901220)),
btVector3(btScalar(0.307983), btScalar(0.554615), btScalar(-0.772786)),
btVector3(btScalar(0.166702), btScalar(0.953181), btScalar(-0.252021)),
btVector3(btScalar(0.172751), btScalar(0.844499), btScalar(-0.506743)),
btVector3(btScalar(0.177630), btScalar(0.711125), btScalar(-0.679876)),
btVector3(btScalar(0.120064), btScalar(0.992260), btScalar(-0.030482)),
btVector3(btScalar(0.289640), btScalar(0.949098), btScalar(0.122546)),
btVector3(btScalar(0.239879), btScalar(0.909047), btScalar(0.340377)),
btVector3(btScalar(0.181142), btScalar(0.821363), btScalar(0.540641)),
btVector3(btScalar(0.066986), btScalar(0.719097), btScalar(0.691327)),
btVector3(btScalar(0.156750), btScalar(0.545478), btScalar(0.823079)),
btVector3(btScalar(0.236172), btScalar(0.342306), btScalar(0.909353)),
btVector3(btScalar(0.277541), btScalar(0.112693), btScalar(0.953856)),
btVector3(btScalar(0.295299), btScalar(-0.121974), btScalar(0.947415)),
btVector3(btScalar(0.287883), btScalar(-0.349254), btScalar(0.891591)),
btVector3(btScalar(0.437165), btScalar(-0.634666), btScalar(0.636869)),
btVector3(btScalar(0.407113), btScalar(-0.784954), btScalar(0.466664)),
btVector3(btScalar(0.375111), btScalar(-0.888193), btScalar(0.264839)),
btVector3(btScalar(0.275394), btScalar(-0.560591), btScalar(0.780723)),
btVector3(btScalar(0.122015), btScalar(-0.992209), btScalar(-0.024821)),
btVector3(btScalar(0.087866), btScalar(-0.966156), btScalar(-0.241676)),
btVector3(btScalar(0.239489), btScalar(-0.885665), btScalar(-0.397437)),
btVector3(btScalar(0.167287), btScalar(-0.965184), btScalar(0.200817)),
btVector3(btScalar(0.201632), btScalar(-0.776789), btScalar(-0.596335)),
btVector3(btScalar(0.122015), btScalar(-0.637971), btScalar(-0.760098)),
btVector3(btScalar(0.008054), btScalar(-0.464741), btScalar(-0.885214)),
btVector3(btScalar(-0.116054), btScalar(-0.271096), btScalar(-0.955482)),
btVector3(btScalar(-0.000727), btScalar(-0.056065), btScalar(-0.998424)),
btVector3(btScalar(-0.134007), btScalar(0.152939), btScalar(-0.978905)),
btVector3(btScalar(-0.025900), btScalar(0.366026), btScalar(-0.930108)),
btVector3(btScalar(0.081231), btScalar(0.557337), btScalar(-0.826072)),
btVector3(btScalar(-0.002874), btScalar(0.917213), btScalar(-0.398023)),
btVector3(btScalar(-0.050683), btScalar(0.981761), btScalar(-0.182534)),
btVector3(btScalar(-0.040536), btScalar(0.710153), btScalar(-0.702713)),
btVector3(btScalar(-0.139081), btScalar(0.827973), btScalar(-0.543048)),
btVector3(btScalar(-0.101029), btScalar(0.994010), btScalar(0.041152)),
btVector3(btScalar(0.069328), btScalar(0.978067), btScalar(0.196133)),
btVector3(btScalar(0.023860), btScalar(0.911380), btScalar(0.410645)),
btVector3(btScalar(-0.153521), btScalar(0.736789), btScalar(0.658145)),
btVector3(btScalar(-0.070002), btScalar(0.591750), btScalar(0.802780)),
btVector3(btScalar(0.002590), btScalar(0.312948), btScalar(0.949562)),
btVector3(btScalar(0.090988), btScalar(-0.020680), btScalar(0.995627)),
btVector3(btScalar(0.088842), btScalar(-0.250099), btScalar(0.964006)),
btVector3(btScalar(0.083378), btScalar(-0.470185), btScalar(0.878318)),
btVector3(btScalar(0.240074), btScalar(-0.749764), btScalar(0.616374)),
btVector3(btScalar(0.210803), btScalar(-0.885860), btScalar(0.412987)),
btVector3(btScalar(0.077524), btScalar(-0.660524), btScalar(0.746565)),
btVector3(btScalar(-0.096736), btScalar(-0.990070), btScalar(-0.100945)),
btVector3(btScalar(-0.052634), btScalar(-0.990264), btScalar(0.127426)),
btVector3(btScalar(-0.106102), btScalar(-0.938354), btScalar(-0.328340)),
btVector3(btScalar(0.013323), btScalar(-0.863112), btScalar(-0.504596)),
btVector3(btScalar(-0.002093), btScalar(-0.936993), btScalar(0.349161)),
btVector3(btScalar(-0.106297), btScalar(-0.636610), btScalar(-0.763612)),
btVector3(btScalar(-0.229430), btScalar(-0.463769), btScalar(-0.855546)),
btVector3(btScalar(-0.245236), btScalar(-0.066175), btScalar(-0.966999)),
btVector3(btScalar(-0.351587), btScalar(-0.270513), btScalar(-0.896145)),
btVector3(btScalar(-0.370906), btScalar(0.133108), btScalar(-0.918982)),
btVector3(btScalar(-0.264360), btScalar(0.346000), btScalar(-0.900049)),
btVector3(btScalar(-0.151375), btScalar(0.543728), btScalar(-0.825291)),
btVector3(btScalar(-0.218697), btScalar(0.912741), btScalar(-0.344346)),
btVector3(btScalar(-0.274507), btScalar(0.953764), btScalar(-0.121635)),
btVector3(btScalar(-0.259677), btScalar(0.692266), btScalar(-0.673044)),
btVector3(btScalar(-0.350416), btScalar(0.798810), btScalar(-0.488786)),
btVector3(btScalar(-0.320170), btScalar(0.941127), btScalar(0.108297)),
btVector3(btScalar(-0.147667), btScalar(0.952792), btScalar(0.265034)),
btVector3(btScalar(-0.188061), btScalar(0.860636), btScalar(0.472910)),
btVector3(btScalar(-0.370906), btScalar(0.739900), btScalar(0.560941)),
btVector3(btScalar(-0.297143), btScalar(0.585334), btScalar(0.754178)),
btVector3(btScalar(-0.189622), btScalar(0.428241), btScalar(0.883393)),
btVector3(btScalar(-0.091272), btScalar(0.098695), btScalar(0.990747)),
btVector3(btScalar(-0.256945), btScalar(0.228375), btScalar(0.938827)),
btVector3(btScalar(-0.111761), btScalar(-0.133251), btScalar(0.984696)),
btVector3(btScalar(-0.118006), btScalar(-0.356253), btScalar(0.926725)),
btVector3(btScalar(-0.119372), btScalar(-0.563896), btScalar(0.817029)),
btVector3(btScalar(0.041228), btScalar(-0.833949), btScalar(0.550010)),
btVector3(btScalar(-0.121909), btScalar(-0.736543), btScalar(0.665172)),
btVector3(btScalar(-0.307681), btScalar(-0.931160), btScalar(-0.195026)),
btVector3(btScalar(-0.283679), btScalar(-0.957990), btScalar(0.041348)),
btVector3(btScalar(-0.227284), btScalar(-0.935243), btScalar(0.270890)),
btVector3(btScalar(-0.293436), btScalar(-0.858252), btScalar(-0.420860)),
btVector3(btScalar(-0.175767), btScalar(-0.780677), btScalar(-0.599262)),
btVector3(btScalar(-0.170108), btScalar(-0.858835), btScalar(0.482865)),
btVector3(btScalar(-0.332854), btScalar(-0.635055), btScalar(-0.696857)),
btVector3(btScalar(-0.447791), btScalar(-0.445299), btScalar(-0.775128)),
btVector3(btScalar(-0.470622), btScalar(-0.074146), btScalar(-0.879164)),
btVector3(btScalar(-0.639417), btScalar(-0.340505), btScalar(-0.689049)),
btVector3(btScalar(-0.598438), btScalar(0.104722), btScalar(-0.794256)),
btVector3(btScalar(-0.488575), btScalar(0.307699), btScalar(-0.816313)),
btVector3(btScalar(-0.379882), btScalar(0.513592), btScalar(-0.769077)),
btVector3(btScalar(-0.425740), btScalar(0.862775), btScalar(-0.272516)),
btVector3(btScalar(-0.480769), btScalar(0.875412), btScalar(-0.048439)),
btVector3(btScalar(-0.467890), btScalar(0.648716), btScalar(-0.600043)),
btVector3(btScalar(-0.543799), btScalar(0.730956), btScalar(-0.411881)),
btVector3(btScalar(-0.516284), btScalar(0.838277), btScalar(0.174076)),
btVector3(btScalar(-0.353343), btScalar(0.876384), btScalar(0.326519)),
btVector3(btScalar(-0.572875), btScalar(0.614497), btScalar(0.542007)),
btVector3(btScalar(-0.503600), btScalar(0.497261), btScalar(0.706161)),
btVector3(btScalar(-0.530920), btScalar(0.754870), btScalar(0.384685)),
btVector3(btScalar(-0.395884), btScalar(0.366414), btScalar(0.841818)),
btVector3(btScalar(-0.300656), btScalar(0.001678), btScalar(0.953661)),
btVector3(btScalar(-0.461060), btScalar(0.146912), btScalar(0.875000)),
btVector3(btScalar(-0.315486), btScalar(-0.232212), btScalar(0.919893)),
btVector3(btScalar(-0.323682), btScalar(-0.449187), btScalar(0.832644)),
btVector3(btScalar(-0.318999), btScalar(-0.639527), btScalar(0.699134)),
btVector3(btScalar(-0.496771), btScalar(-0.866029), btScalar(-0.055271)),
btVector3(btScalar(-0.496771), btScalar(-0.816257), btScalar(-0.294377)),
btVector3(btScalar(-0.456377), btScalar(-0.869528), btScalar(0.188130)),
btVector3(btScalar(-0.380858), btScalar(-0.827144), btScalar(0.412792)),
btVector3(btScalar(-0.449352), btScalar(-0.727405), btScalar(-0.518259)),
btVector3(btScalar(-0.570533), btScalar(-0.551064), btScalar(-0.608632)),
btVector3(btScalar(-0.656394), btScalar(-0.118280), btScalar(-0.744874)),
btVector3(btScalar(-0.756696), btScalar(-0.438105), btScalar(-0.484882)),
btVector3(btScalar(-0.801773), btScalar(-0.204798), btScalar(-0.561005)),
btVector3(btScalar(-0.785186), btScalar(0.038618), btScalar(-0.617805)),
btVector3(btScalar(-0.709082), btScalar(0.262399), btScalar(-0.654306)),
btVector3(btScalar(-0.583412), btScalar(0.462265), btScalar(-0.667383)),
btVector3(btScalar(-0.616001), btScalar(0.761286), btScalar(-0.201272)),
btVector3(btScalar(-0.660687), btScalar(0.750204), btScalar(0.020072)),
btVector3(btScalar(-0.744987), btScalar(0.435823), btScalar(-0.504791)),
btVector3(btScalar(-0.713765), btScalar(0.605554), btScalar(-0.351373)),
btVector3(btScalar(-0.686251), btScalar(0.687600), btScalar(0.236927)),
btVector3(btScalar(-0.680201), btScalar(0.429407), btScalar(0.593732)),
btVector3(btScalar(-0.733474), btScalar(0.546450), btScalar(0.403814)),
btVector3(btScalar(-0.591023), btScalar(0.292923), btScalar(0.751445)),
btVector3(btScalar(-0.500283), btScalar(-0.080757), btScalar(0.861922)),
btVector3(btScalar(-0.643710), btScalar(0.070115), btScalar(0.761985)),
btVector3(btScalar(-0.506332), btScalar(-0.308425), btScalar(0.805122)),
btVector3(btScalar(-0.503015), btScalar(-0.509847), btScalar(0.697573)),
btVector3(btScalar(-0.482525), btScalar(-0.682105), btScalar(0.549229)),
btVector3(btScalar(-0.680396), btScalar(-0.716323), btScalar(-0.153451)),
btVector3(btScalar(-0.658346), btScalar(-0.746264), btScalar(0.097562)),
btVector3(btScalar(-0.653272), btScalar(-0.646915), btScalar(-0.392948)),
btVector3(btScalar(-0.590828), btScalar(-0.732655), btScalar(0.337645)),
btVector3(btScalar(-0.819140), btScalar(-0.518013), btScalar(-0.246166)),
btVector3(btScalar(-0.900513), btScalar(-0.282178), btScalar(-0.330487)),
btVector3(btScalar(-0.914953), btScalar(-0.028652), btScalar(-0.402122)),
btVector3(btScalar(-0.859924), btScalar(0.220209), btScalar(-0.459898)),
btVector3(btScalar(-0.777185), btScalar(0.613720), btScalar(-0.137836)),
btVector3(btScalar(-0.805285), btScalar(0.586889), btScalar(0.082728)),
btVector3(btScalar(-0.872413), btScalar(0.406077), btScalar(-0.271735)),
btVector3(btScalar(-0.859339), btScalar(0.448072), btScalar(0.246101)),
btVector3(btScalar(-0.757671), btScalar(0.216320), btScalar(0.615594)),
btVector3(btScalar(-0.826165), btScalar(0.348139), btScalar(0.442851)),
btVector3(btScalar(-0.671810), btScalar(-0.162803), btScalar(0.722557)),
btVector3(btScalar(-0.796504), btScalar(-0.004543), btScalar(0.604468)),
btVector3(btScalar(-0.676298), btScalar(-0.378223), btScalar(0.631794)),
btVector3(btScalar(-0.668883), btScalar(-0.558258), btScalar(0.490673)),
btVector3(btScalar(-0.821287), btScalar(-0.570118), btScalar(0.006994)),
btVector3(btScalar(-0.767428), btScalar(-0.587810), btScalar(0.255470)),
btVector3(btScalar(-0.933296), btScalar(-0.349837), btScalar(-0.079865)),
btVector3(btScalar(-0.982667), btScalar(-0.100393), btScalar(-0.155208)),
btVector3(btScalar(-0.961396), btScalar(0.160910), btScalar(-0.222938)),
btVector3(btScalar(-0.934858), btScalar(0.354555), btScalar(-0.006864)),
btVector3(btScalar(-0.941687), btScalar(0.229736), btScalar(0.245711)),
btVector3(btScalar(-0.884317), btScalar(0.131552), btScalar(0.447536)),
btVector3(btScalar(-0.810359), btScalar(-0.219769), btScalar(0.542788)),
btVector3(btScalar(-0.915929), btScalar(-0.210048), btScalar(0.341743)),
btVector3(btScalar(-0.816799), btScalar(-0.407192), btScalar(0.408303)),
btVector3(btScalar(-0.903050), btScalar(-0.392416), btScalar(0.174076)),
btVector3(btScalar(-0.980325), btScalar(-0.170969), btScalar(0.096586)),
btVector3(btScalar(-0.995936), btScalar(0.084891), btScalar(0.029441)),
btVector3(btScalar(-0.960031), btScalar(0.002650), btScalar(0.279283)),
};
static btVector3 sUnitSpherePoints[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] =
{
btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
@ -165,6 +426,8 @@ btVector3* btShapeHull::getUnitSpherePoints()
btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
};
if (highres)
return sUnitSpherePointsHighres;
return sUnitSpherePoints;
}

4
thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.h vendored
View File

@ -34,7 +34,7 @@ protected:
unsigned int m_numIndices;
const btConvexShape* m_shape;
static btVector3* getUnitSpherePoints();
static btVector3* getUnitSpherePoints(int highres=0);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
@ -42,7 +42,7 @@ public:
btShapeHull (const btConvexShape* shape);
~btShapeHull ();
bool buildHull (btScalar margin);
bool buildHull (btScalar margin, int highres=0);
int numTriangles () const;
int numVertices () const;

2
thirdparty/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp vendored
View File

@ -69,8 +69,6 @@ void btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback,const
//tangentDir0/tangentDir1 can be precalculated
btPlaneSpace1(m_planeNormal,tangentDir0,tangentDir1);
btVector3 supVertex0,supVertex1;
btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal;
btVector3 triangle[3];

2
thirdparty/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h vendored
View File

@ -63,7 +63,7 @@ typedef btAlignedObjectArray<btIndexedMesh> IndexedMeshArray;
///The btTriangleIndexVertexArray allows to access multiple triangle meshes, by indexing into existing triangle/index arrays.
///Additional meshes can be added using addIndexedMesh
///No duplcate is made of the vertex/index data, it only indexes into external vertex/index arrays.
///No duplicate is made of the vertex/index data, it only indexes into external vertex/index arrays.
///So keep those arrays around during the lifetime of this btTriangleIndexVertexArray.
ATTRIBUTE_ALIGNED16( class) btTriangleIndexVertexArray : public btStridingMeshInterface
{

23
thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp vendored
View File

@ -113,12 +113,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact(
if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f)
return false;
btScalar lambda = btScalar(0.);
btVector3 v(1,0,0);
int maxIter = MAX_ITERATIONS;
btVector3 n;
n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
@ -138,7 +133,6 @@ bool btContinuousConvexCollision::calcTimeOfImpact(
btPointCollector pointCollector1;
{
computeClosestPoints(fromA,fromB,pointCollector1);
hasResult = pointCollector1.m_hasResult;
@ -172,28 +166,20 @@ bool btContinuousConvexCollision::calcTimeOfImpact(
dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
lambda += dLambda;
lambda = lambda + dLambda;
if (lambda > btScalar(1.))
if (lambda > btScalar(1.) || lambda < btScalar(0.))
return false;
if (lambda < btScalar(0.))
return false;
//todo: next check with relative epsilon
if (lambda <= lastLambda)
{
return false;
//n.setValue(0,0,0);
break;
//break;
}
lastLambda = lambda;
//interpolate to next lambda
btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
@ -223,7 +209,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact(
}
numIter++;
if (numIter > maxIter)
if (numIter > MAX_ITERATIONS)
{
result.reportFailure(-2, numIter);
return false;
@ -237,6 +223,5 @@ bool btContinuousConvexCollision::calcTimeOfImpact(
}
return false;
}

2
thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h vendored
View File

@ -25,7 +25,7 @@ class btStaticPlaneShape;
/// btContinuousConvexCollision implements angular and linear time of impact for convex objects.
/// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).
/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.
/// Algorithm operates in worldspace, in order to keep in between motion globally consistent.
/// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops
class btContinuousConvexCollision : public btConvexCast
{

66
thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp vendored
View File

@ -21,46 +21,64 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver,
const btConvexShape* pConvexA, const btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
class btIDebugDraw* debugDraw)
bool btGjkEpaPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
const btConvexShape* pConvexA, const btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
class btIDebugDraw* debugDraw)
{
(void)debugDraw;
(void)v;
(void)simplexSolver;
// const btScalar radialmargin(btScalar(0.));
btVector3 guessVectors[] = {
btVector3(transformB.getOrigin() - transformA.getOrigin()).normalized(),
btVector3(transformA.getOrigin() - transformB.getOrigin()).normalized(),
btVector3(0, 0, 1),
btVector3(0, 1, 0),
btVector3(1, 0, 0),
btVector3(1, 1, 0),
btVector3(1, 1, 1),
btVector3(0, 1, 1),
btVector3(1, 0, 1),
};
btVector3 guessVector(transformB.getOrigin()-transformA.getOrigin());
btGjkEpaSolver2::sResults results;
int numVectors = sizeof(guessVectors) / sizeof(btVector3);
if(btGjkEpaSolver2::Penetration(pConvexA,transformA,
pConvexB,transformB,
guessVector,results))
{
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
wWitnessOnA = results.witnesses[0];
wWitnessOnB = results.witnesses[1];
v = results.normal;
return true;
} else
for (int i = 0; i < numVectors; i++)
{
if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results))
simplexSolver.reset();
btVector3 guessVector = guessVectors[i];
btGjkEpaSolver2::sResults results;
if (btGjkEpaSolver2::Penetration(pConvexA, transformA,
pConvexB, transformB,
guessVector, results))
{
wWitnessOnA = results.witnesses[0];
wWitnessOnB = results.witnesses[1];
v = results.normal;
return false;
return true;
}
else
{
if (btGjkEpaSolver2::Distance(pConvexA, transformA, pConvexB, transformB, guessVector, results))
{
wWitnessOnA = results.witnesses[0];
wWitnessOnB = results.witnesses[1];
v = results.normal;
return false;
}
}
}
//failed to find a distance/penetration
wWitnessOnA.setValue(0, 0, 0);
wWitnessOnB.setValue(0, 0, 0);
v.setValue(0, 0, 0);
return false;
}

1144
thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp vendored
View File

File diff suppressed because it is too large Load Diff

151
thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp vendored
View File

@ -16,7 +16,13 @@ subject to the following restrictions:
#include "btPersistentManifold.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btSerializer.h"
#ifdef BT_USE_DOUBLE_PRECISION
#define btCollisionObjectData btCollisionObjectDoubleData
#else
#define btCollisionObjectData btCollisionObjectFloatData
#endif
btScalar gContactBreakingThreshold = btScalar(0.02);
ContactDestroyedCallback gContactDestroyedCallback = 0;
@ -33,6 +39,8 @@ btPersistentManifold::btPersistentManifold()
m_body0(0),
m_body1(0),
m_cachedPoints (0),
m_companionIdA(0),
m_companionIdB(0),
m_index1a(0)
{
}
@ -303,6 +311,149 @@ void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btT
}
int btPersistentManifold::calculateSerializeBufferSize() const
{
return sizeof(btPersistentManifoldData);
}
const char* btPersistentManifold::serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const
{
btPersistentManifoldData* dataOut = (btPersistentManifoldData*)dataBuffer;
memset(dataOut, 0, sizeof(btPersistentManifoldData));
dataOut->m_body0 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody0());
dataOut->m_body1 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody1());
dataOut->m_contactBreakingThreshold = manifold->getContactBreakingThreshold();
dataOut->m_contactProcessingThreshold = manifold->getContactProcessingThreshold();
dataOut->m_numCachedPoints = manifold->getNumContacts();
dataOut->m_companionIdA = manifold->m_companionIdA;
dataOut->m_companionIdB = manifold->m_companionIdB;
dataOut->m_index1a = manifold->m_index1a;
dataOut->m_objectType = manifold->m_objectType;
for (int i = 0; i < this->getNumContacts(); i++)
{
const btManifoldPoint& pt = manifold->getContactPoint(i);
dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse;
dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1;
dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2;
pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]);
pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]);
pt.m_normalWorldOnB.serialize(dataOut->m_pointCacheNormalWorldOnB[i]);
dataOut->m_pointCacheDistance[i] = pt.m_distance1;
dataOut->m_pointCacheCombinedContactDamping1[i] = pt.m_combinedContactDamping1;
dataOut->m_pointCacheCombinedContactStiffness1[i] = pt.m_combinedContactStiffness1;
dataOut->m_pointCacheLifeTime[i] = pt.m_lifeTime;
dataOut->m_pointCacheFrictionCFM[i] = pt.m_frictionCFM;
dataOut->m_pointCacheContactERP[i] = pt.m_contactERP;
dataOut->m_pointCacheContactCFM[i] = pt.m_contactCFM;
dataOut->m_pointCacheContactPointFlags[i] = pt.m_contactPointFlags;
dataOut->m_pointCacheIndex0[i] = pt.m_index0;
dataOut->m_pointCacheIndex1[i] = pt.m_index1;
dataOut->m_pointCachePartId0[i] = pt.m_partId0;
dataOut->m_pointCachePartId1[i] = pt.m_partId1;
pt.m_positionWorldOnA.serialize(dataOut->m_pointCachePositionWorldOnA[i]);
pt.m_positionWorldOnB.serialize(dataOut->m_pointCachePositionWorldOnB[i]);
dataOut->m_pointCacheCombinedFriction[i] = pt.m_combinedFriction;
pt.m_lateralFrictionDir1.serialize(dataOut->m_pointCacheLateralFrictionDir1[i]);
pt.m_lateralFrictionDir2.serialize(dataOut->m_pointCacheLateralFrictionDir2[i]);
dataOut->m_pointCacheCombinedRollingFriction[i] = pt.m_combinedRollingFriction;
dataOut->m_pointCacheCombinedSpinningFriction[i] = pt.m_combinedSpinningFriction;
dataOut->m_pointCacheCombinedRestitution[i] = pt.m_combinedRestitution;
dataOut->m_pointCacheContactMotion1[i] = pt.m_contactMotion1;
dataOut->m_pointCacheContactMotion2[i] = pt.m_contactMotion2;
}
return btPersistentManifoldDataName;
}
void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr)
{
m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold;
m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold;
m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
m_companionIdA = manifoldDataPtr->m_companionIdA;
m_companionIdB = manifoldDataPtr->m_companionIdB;
//m_index1a = manifoldDataPtr->m_index1a;
m_objectType = manifoldDataPtr->m_objectType;
for (int i = 0; i < this->getNumContacts(); i++)
{
btManifoldPoint& pt = m_pointCache[i];
pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
pt.m_localPointA.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointA[i]);
pt.m_localPointB.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointB[i]);
pt.m_normalWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]);
pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i];
pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i];
pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
pt.m_positionWorldOnA.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnA[i]);
pt.m_positionWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnB[i]);
pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
pt.m_lateralFrictionDir1.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]);
pt.m_lateralFrictionDir2.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]);
pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i];
pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i];
pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i];
pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
}
}
void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr)
{
m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold;
m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold;
m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
m_companionIdA = manifoldDataPtr->m_companionIdA;
m_companionIdB = manifoldDataPtr->m_companionIdB;
//m_index1a = manifoldDataPtr->m_index1a;
m_objectType = manifoldDataPtr->m_objectType;
for (int i = 0; i < this->getNumContacts(); i++)
{
btManifoldPoint& pt = m_pointCache[i];
pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]);
pt.m_localPointB.deSerialize(manifoldDataPtr->m_pointCacheLocalPointB[i]);
pt.m_normalWorldOnB.deSerialize(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]);
pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i];
pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i];
pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
pt.m_positionWorldOnA.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnA[i]);
pt.m_positionWorldOnB.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnB[i]);
pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
pt.m_lateralFrictionDir1.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]);
pt.m_lateralFrictionDir2.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]);
pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i];
pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i];
pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i];
pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
}
}

116
thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h vendored
View File

@ -24,6 +24,8 @@ class btCollisionObject;
#include "LinearMath/btAlignedAllocator.h"
struct btCollisionResult;
struct btCollisionObjectDoubleData;
struct btCollisionObjectFloatData;
///maximum contact breaking and merging threshold
extern btScalar gContactBreakingThreshold;
@ -95,7 +97,10 @@ public:
: btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
m_body0(body0),m_body1(body1),m_cachedPoints(0),
m_contactBreakingThreshold(contactBreakingThreshold),
m_contactProcessingThreshold(contactProcessingThreshold)
m_contactProcessingThreshold(contactProcessingThreshold),
m_companionIdA(0),
m_companionIdB(0),
m_index1a(0)
{
}
@ -256,10 +261,115 @@ public:
m_cachedPoints = 0;
}
int calculateSerializeBufferSize() const;
const char* serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const;
void deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr);
void deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr);
}
;
};
struct btPersistentManifoldDoubleData
{
btVector3DoubleData m_pointCacheLocalPointA[4];
btVector3DoubleData m_pointCacheLocalPointB[4];
btVector3DoubleData m_pointCachePositionWorldOnA[4];
btVector3DoubleData m_pointCachePositionWorldOnB[4];
btVector3DoubleData m_pointCacheNormalWorldOnB[4];
btVector3DoubleData m_pointCacheLateralFrictionDir1[4];
btVector3DoubleData m_pointCacheLateralFrictionDir2[4];
double m_pointCacheDistance[4];
double m_pointCacheAppliedImpulse[4];
double m_pointCacheCombinedFriction[4];
double m_pointCacheCombinedRollingFriction[4];
double m_pointCacheCombinedSpinningFriction[4];
double m_pointCacheCombinedRestitution[4];
int m_pointCachePartId0[4];
int m_pointCachePartId1[4];
int m_pointCacheIndex0[4];
int m_pointCacheIndex1[4];
int m_pointCacheContactPointFlags[4];
double m_pointCacheAppliedImpulseLateral1[4];
double m_pointCacheAppliedImpulseLateral2[4];
double m_pointCacheContactMotion1[4];
double m_pointCacheContactMotion2[4];
double m_pointCacheContactCFM[4];
double m_pointCacheCombinedContactStiffness1[4];
double m_pointCacheContactERP[4];
double m_pointCacheCombinedContactDamping1[4];
double m_pointCacheFrictionCFM[4];
int m_pointCacheLifeTime[4];
int m_numCachedPoints;
int m_companionIdA;
int m_companionIdB;
int m_index1a;
int m_objectType;
double m_contactBreakingThreshold;
double m_contactProcessingThreshold;
int m_padding;
btCollisionObjectDoubleData *m_body0;
btCollisionObjectDoubleData *m_body1;
};
struct btPersistentManifoldFloatData
{
btVector3FloatData m_pointCacheLocalPointA[4];
btVector3FloatData m_pointCacheLocalPointB[4];
btVector3FloatData m_pointCachePositionWorldOnA[4];
btVector3FloatData m_pointCachePositionWorldOnB[4];
btVector3FloatData m_pointCacheNormalWorldOnB[4];
btVector3FloatData m_pointCacheLateralFrictionDir1[4];
btVector3FloatData m_pointCacheLateralFrictionDir2[4];
float m_pointCacheDistance[4];
float m_pointCacheAppliedImpulse[4];
float m_pointCacheCombinedFriction[4];
float m_pointCacheCombinedRollingFriction[4];
float m_pointCacheCombinedSpinningFriction[4];
float m_pointCacheCombinedRestitution[4];
int m_pointCachePartId0[4];
int m_pointCachePartId1[4];
int m_pointCacheIndex0[4];
int m_pointCacheIndex1[4];
int m_pointCacheContactPointFlags[4];
float m_pointCacheAppliedImpulseLateral1[4];
float m_pointCacheAppliedImpulseLateral2[4];
float m_pointCacheContactMotion1[4];
float m_pointCacheContactMotion2[4];
float m_pointCacheContactCFM[4];
float m_pointCacheCombinedContactStiffness1[4];
float m_pointCacheContactERP[4];
float m_pointCacheCombinedContactDamping1[4];
float m_pointCacheFrictionCFM[4];
int m_pointCacheLifeTime[4];
int m_numCachedPoints;
int m_companionIdA;
int m_companionIdB;
int m_index1a;
int m_objectType;
float m_contactBreakingThreshold;
float m_contactProcessingThreshold;
int m_padding;
btCollisionObjectFloatData *m_body0;
btCollisionObjectFloatData *m_body1;
};
#ifdef BT_USE_DOUBLE_PRECISION
#define btPersistentManifoldData btPersistentManifoldDoubleData
#define btPersistentManifoldDataName "btPersistentManifoldDoubleData"
#else
#define btPersistentManifoldData btPersistentManifoldFloatData
#define btPersistentManifoldDataName "btPersistentManifoldFloatData"
#endif //BT_USE_DOUBLE_PRECISION

9
thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp vendored
View File

@ -72,11 +72,18 @@ bool btSubsimplexConvexCast::calcTimeOfImpact(
btScalar dist2 = v.length2();
#ifdef BT_USE_DOUBLE_PRECISION
btScalar epsilon = btScalar(0.0001);
btScalar epsilon = SIMD_EPSILON * 10;
#else
//todo: epsilon kept for backward compatibility of unit tests.
//will need to digg deeper to make the algorithm more robust
//since, a large epsilon can cause an early termination with false
//positive results (ray intersections that shouldn't be there)
btScalar epsilon = btScalar(0.0001);
#endif //BT_USE_DOUBLE_PRECISION
btVector3 w,p;
btScalar VdotR;

1128
thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

66
thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.h vendored Normal file
View File

@ -0,0 +1,66 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_BATCHED_CONSTRAINTS_H
#define BT_BATCHED_CONSTRAINTS_H
#include "LinearMath/btThreads.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
#include "BulletDynamics/ConstraintSolver/btSolverConstraint.h"
class btIDebugDraw;
struct btBatchedConstraints
{
enum BatchingMethod
{
BATCHING_METHOD_SPATIAL_GRID_2D,
BATCHING_METHOD_SPATIAL_GRID_3D,
BATCHING_METHOD_COUNT
};
struct Range
{
int begin;
int end;
Range() : begin( 0 ), end( 0 ) {}
Range( int _beg, int _end ) : begin( _beg ), end( _end ) {}
};
btAlignedObjectArray<int> m_constraintIndices;
btAlignedObjectArray<Range> m_batches; // each batch is a range of indices in the m_constraintIndices array
btAlignedObjectArray<Range> m_phases; // each phase is range of indices in the m_batches array
btAlignedObjectArray<char> m_phaseGrainSize; // max grain size for each phase
btAlignedObjectArray<int> m_phaseOrder; // phases can be done in any order, so we can randomize the order here
btIDebugDraw* m_debugDrawer;
static bool s_debugDrawBatches;
btBatchedConstraints() {m_debugDrawer=NULL;}
void setup( btConstraintArray* constraints,
const btAlignedObjectArray<btSolverBody>& bodies,
BatchingMethod batchingMethod,
int minBatchSize,
int maxBatchSize,
btAlignedObjectArray<char>* scratchMemory
);
bool validate( btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies ) const;
};
#endif // BT_BATCHED_CONSTRAINTS_H

3
thirdparty/bullet/BulletDynamics/ConstraintSolver/btConstraintSolver.h vendored
View File

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

3
thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h vendored
View File

@ -29,7 +29,8 @@ enum btSolverMode
SOLVER_CACHE_FRIENDLY = 128,
SOLVER_SIMD = 256,
SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024
SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024,
SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048
};
struct btContactSolverInfoData

4
thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp vendored
View File

@ -855,8 +855,8 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
tag_vel,
info->fps * limot->m_stopERP);
info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
info->m_upperLimit[srow] = limot->m_maxMotorForce;
info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
}
}
if(limit)

2
thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h vendored
View File

@ -77,7 +77,7 @@ public:
{
m_accumulatedImpulse = 0.f;
m_targetVelocity = 0;
m_maxMotorForce = 0.1f;
m_maxMotorForce = 6.0f;
m_maxLimitForce = 300.0f;
m_loLimit = 1.0f;
m_hiLimit = -1.0f;

27
thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp vendored
View File

@ -719,8 +719,8 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
tag_vel,
info->fps * limot->m_motorERP);
info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity;
info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
info->m_upperLimit[srow] = limot->m_maxMotorForce;
info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
info->cfm[srow] = limot->m_motorCFM;
srow += info->rowskip;
++count;
@ -769,8 +769,8 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
mot_fact = 0;
}
info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1);
info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
info->m_upperLimit[srow] = limot->m_maxMotorForce;
info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
info->cfm[srow] = limot->m_motorCFM;
srow += info->rowskip;
++count;
@ -797,6 +797,12 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
btScalar cfm = BT_ZERO;
btScalar mA = BT_ONE / m_rbA.getInvMass();
btScalar mB = BT_ONE / m_rbB.getInvMass();
if (rotational) {
btScalar rrA = (m_calculatedTransformA.getOrigin() - transA.getOrigin()).length2();
btScalar rrB = (m_calculatedTransformB.getOrigin() - transB.getOrigin()).length2();
if (m_rbA.getInvMass()) mA = mA * rrA + 1 / (m_rbA.getInvInertiaTensorWorld() * ax1).length();
if (m_rbB.getInvMass()) mB = mB * rrB + 1 / (m_rbB.getInvInertiaTensorWorld() * ax1).length();
}
btScalar m = mA > mB ? mB : mA;
btScalar angularfreq = sqrt(ks / m);
@ -815,7 +821,18 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
btScalar f = (fs+fd);
info->m_constraintError[srow] = (vel + f * (rotational ? -1 : 1)) ;
// after the spring force affecting the body(es) the new velocity will be
// vel + f / m * (rotational ? -1 : 1)
// so in theory this should be set here for m_constraintError
// (with m_constraintError we set a desired velocity for the affected body(es))
// however in practice any value is fine as long as it is greater then the "proper" velocity,
// because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force
// so it is much simpler (and more robust) just to simply use inf (with the proper sign)
// you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration)
// will we not request a velocity with the wrong direction ?
// and the answare is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
// so the sign of the force that is really matters
info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
btScalar minf = f < fd ? f : fd;
btScalar maxf = f < fd ? fd : f;

2
thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h vendored
View File

@ -107,7 +107,7 @@ public:
m_motorCFM = 0.f;
m_enableMotor = false;
m_targetVelocity = 0;
m_maxMotorForce = 0.1f;
m_maxMotorForce = 6.0f;
m_servoMotor = false;
m_servoTarget = 0;
m_enableSpring = false;

4
thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp vendored
View File

@ -131,7 +131,7 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf
btScalar force = delta * m_springStiffness[i];
btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations);
m_linearLimits.m_targetVelocity[i] = velFactor * force;
m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps;
m_linearLimits.m_maxMotorForce[i] = btFabs(force);
}
}
for(i = 0; i < 3; i++)
@ -146,7 +146,7 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf
btScalar force = -delta * m_springStiffness[i+3];
btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations);
m_angularLimits[i].m_targetVelocity = velFactor * force;
m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
m_angularLimits[i].m_maxMotorForce = btFabs(force);
}
}
}

757
thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp vendored
View File

File diff suppressed because it is too large Load Diff

24
thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h vendored
View File

@ -27,7 +27,7 @@ class btCollisionObject;
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
typedef btSimdScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
typedef btScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver
@ -95,13 +95,17 @@ protected:
void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
virtual void convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal);
void convertJoint(btSolverConstraint* currentConstraintRow, btTypedConstraint* constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal);
btSimdScalar resolveSplitPenetrationSIMD(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
virtual void convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
btScalar resolveSplitPenetrationSIMD(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
{
return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
}
btSimdScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
btScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
{
return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
}
@ -110,18 +114,20 @@ protected:
int getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);
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);
btSimdScalar resolveSplitPenetrationImpulse(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
btScalar resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btScalar resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btScalar resolveSplitPenetrationImpulse(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
{
return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
}
protected:
void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);

1621
thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

154
thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h vendored Normal file
View File

@ -0,0 +1,154 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
#define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
#include "btSequentialImpulseConstraintSolver.h"
#include "btBatchedConstraints.h"
#include "LinearMath/btThreads.h"
///
/// btSequentialImpulseConstraintSolverMt
///
/// A multithreaded variant of the sequential impulse constraint solver. The constraints to be solved are grouped into
/// batches and phases where each batch of constraints within a given phase can be solved in parallel with the rest.
/// Ideally we want as few phases as possible, and each phase should have many batches, and all of the batches should
/// have about the same number of constraints.
/// This method works best on a large island of many constraints.
///
/// Supports all of the features of the normal sequential impulse solver such as:
/// - split penetration impulse
/// - rolling friction
/// - interleaving constraints
/// - warmstarting
/// - 2 friction directions
/// - randomized constraint ordering
/// - early termination when leastSquaresResidualThreshold is satisfied
///
/// When the SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS flag is enabled, unlike the normal SequentialImpulse solver,
/// the rolling friction is interleaved as well.
/// Interleaving the contact penetration constraints with friction reduces the number of parallel loops that need to be done,
/// which reduces threading overhead so it can be a performance win, however, it does seem to produce a less stable simulation,
/// at least on stacks of blocks.
///
/// When the SOLVER_RANDMIZE_ORDER flag is enabled, the ordering of phases, and the ordering of constraints within each batch
/// is randomized, however it does not swap constraints between batches.
/// This is to avoid regenerating the batches for each solver iteration which would be quite costly in performance.
///
/// Note that a non-zero leastSquaresResidualThreshold could possibly affect the determinism of the simulation
/// if the task scheduler's parallelSum operation is non-deterministic. The parallelSum operation can be non-deterministic
/// because floating point addition is not associative due to rounding errors.
/// The task scheduler can and should ensure that the result of any parallelSum operation is deterministic.
///
ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolverMt : public btSequentialImpulseConstraintSolver
{
public:
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE;
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE;
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE;
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
// temp struct used to collect info from persistent manifolds into a cache-friendly struct using multiple threads
struct btContactManifoldCachedInfo
{
static const int MAX_NUM_CONTACT_POINTS = 4;
int numTouchingContacts;
int solverBodyIds[ 2 ];
int contactIndex;
int rollingFrictionIndex;
bool contactHasRollingFriction[ MAX_NUM_CONTACT_POINTS ];
btManifoldPoint* contactPoints[ MAX_NUM_CONTACT_POINTS ];
};
// temp struct used for setting up joint constraints in parallel
struct JointParams
{
int m_solverConstraint;
int m_solverBodyA;
int m_solverBodyB;
};
void internalInitMultipleJoints(btTypedConstraint** constraints, int iBegin, int iEnd);
void internalConvertMultipleJoints( const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal );
// parameters to control batching
static bool s_allowNestedParallelForLoops; // whether to allow nested parallel operations
static int s_minimumContactManifoldsForBatching; // don't even try to batch if fewer manifolds than this
static btBatchedConstraints::BatchingMethod s_contactBatchingMethod;
static btBatchedConstraints::BatchingMethod s_jointBatchingMethod;
static int s_minBatchSize; // desired number of constraints per batch
static int s_maxBatchSize;
protected:
static const int CACHE_LINE_SIZE = 64;
btBatchedConstraints m_batchedContactConstraints;
btBatchedConstraints m_batchedJointConstraints;
int m_numFrictionDirections;
bool m_useBatching;
bool m_useObsoleteJointConstraints;
btAlignedObjectArray<btContactManifoldCachedInfo> m_manifoldCachedInfoArray;
btAlignedObjectArray<int> m_rollingFrictionIndexTable; // lookup table mapping contact index to rolling friction index
btSpinMutex m_bodySolverArrayMutex;
char m_antiFalseSharingPadding[CACHE_LINE_SIZE]; // padding to keep mutexes in separate cachelines
btSpinMutex m_kinematicBodyUniqueIdToSolverBodyTableMutex;
btAlignedObjectArray<char> m_scratchMemory;
virtual void randomizeConstraintOrdering( int iteration, int numIterations );
virtual btScalar resolveAllJointConstraints( int iteration );
virtual btScalar resolveAllContactConstraints();
virtual btScalar resolveAllContactFrictionConstraints();
virtual btScalar resolveAllContactConstraintsInterleaved();
virtual btScalar resolveAllRollingFrictionConstraints();
virtual void setupBatchedContactConstraints();
virtual void setupBatchedJointConstraints();
virtual void convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
virtual void convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
int getOrInitSolverBodyThreadsafe(btCollisionObject& body, btScalar timeStep);
void allocAllContactConstraints(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
void setupAllContactConstraints(const btContactSolverInfo& infoGlobal);
void randomizeBatchedConstraintOrdering( btBatchedConstraints* batchedConstraints );
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btSequentialImpulseConstraintSolverMt();
virtual ~btSequentialImpulseConstraintSolverMt();
btScalar resolveMultipleJointConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration );
btScalar resolveMultipleContactConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
btScalar resolveMultipleContactSplitPenetrationImpulseConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
btScalar resolveMultipleContactFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
btScalar resolveMultipleContactRollingFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
btScalar resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray<int>& contactIndices, int batchBegin, int batchEnd );
void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
void internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds);
void internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal);
void internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
};
#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H

0
thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp vendored Normal file → Executable file
View File

0
thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h vendored Normal file → Executable file
View File

14
thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp vendored
View File

@ -842,6 +842,9 @@ public:
btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
if(!m_dispatcher->needsCollision(m_me, otherObj))
return false;
//call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
if (m_dispatcher->needsResponse(m_me,otherObj))
{
@ -1342,9 +1345,12 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
btVector3 axis = tr.getBasis().getColumn(0);
btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
if (minTh <= maxTh)
{
btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
}
axis = tr.getBasis().getColumn(1);
btScalar ay = p6DOF->getAngle(1);
btScalar az = p6DOF->getAngle(2);
@ -1533,6 +1539,8 @@ void btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
serializeRigidBodies(serializer);
serializeContactManifolds(serializer);
serializer->finishSerialization();
}

99
thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp vendored
View File

@ -50,63 +50,6 @@ subject to the following restrictions:
#include "LinearMath/btSerializer.h"
struct InplaceSolverIslandCallbackMt : public btSimulationIslandManagerMt::IslandCallback
{
btContactSolverInfo* m_solverInfo;
btConstraintSolver* m_solver;
btIDebugDraw* m_debugDrawer;
btDispatcher* m_dispatcher;
InplaceSolverIslandCallbackMt(
btConstraintSolver* solver,
btStackAlloc* stackAlloc,
btDispatcher* dispatcher)
:m_solverInfo(NULL),
m_solver(solver),
m_debugDrawer(NULL),
m_dispatcher(dispatcher)
{
}
InplaceSolverIslandCallbackMt& operator=(InplaceSolverIslandCallbackMt& other)
{
btAssert(0);
(void)other;
return *this;
}
SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btIDebugDraw* debugDrawer)
{
btAssert(solverInfo);
m_solverInfo = solverInfo;
m_debugDrawer = debugDrawer;
}
virtual void processIsland( btCollisionObject** bodies,
int numBodies,
btPersistentManifold** manifolds,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
int islandId
)
{
m_solver->solveGroup( bodies,
numBodies,
manifolds,
numManifolds,
constraints,
numConstraints,
*m_solverInfo,
m_debugDrawer,
m_dispatcher
);
}
};
///
/// btConstraintSolverPoolMt
@ -209,7 +152,12 @@ void btConstraintSolverPoolMt::reset()
/// btDiscreteDynamicsWorldMt
///
btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolverPoolMt* constraintSolver, btCollisionConfiguration* collisionConfiguration)
btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
btBroadphaseInterface* pairCache,
btConstraintSolverPoolMt* constraintSolver,
btConstraintSolver* constraintSolverMt,
btCollisionConfiguration* collisionConfiguration
)
: btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
{
if (m_ownsIslandManager)
@ -217,31 +165,18 @@ btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, b
m_islandManager->~btSimulationIslandManager();
btAlignedFree( m_islandManager);
}
{
void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallbackMt),16);
m_solverIslandCallbackMt = new (mem) InplaceSolverIslandCallbackMt (m_constraintSolver, 0, dispatcher);
}
{
void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt),16);
btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt();
im->setMinimumSolverBatchSize( m_solverInfo.m_minimumSolverBatchSize );
m_islandManager = im;
}
m_constraintSolverMt = constraintSolverMt;
}
btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt()
{
if (m_solverIslandCallbackMt)
{
m_solverIslandCallbackMt->~InplaceSolverIslandCallbackMt();
btAlignedFree(m_solverIslandCallbackMt);
}
if (m_ownsConstraintSolver)
{
m_constraintSolver->~btConstraintSolver();
btAlignedFree(m_constraintSolver);
}
}
@ -249,12 +184,17 @@ void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo
{
BT_PROFILE("solveConstraints");
m_solverIslandCallbackMt->setup(&solverInfo, getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
/// solve all the constraints for this island
btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager);
im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, m_solverIslandCallbackMt );
btSimulationIslandManagerMt::SolverParams solverParams;
solverParams.m_solverPool = m_constraintSolver;
solverParams.m_solverMt = m_constraintSolverMt;
solverParams.m_solverInfo = &solverInfo;
solverParams.m_debugDrawer = m_debugDrawer;
solverParams.m_dispatcher = getCollisionWorld()->getDispatcher();
im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, solverParams );
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
}
@ -325,3 +265,14 @@ void btDiscreteDynamicsWorldMt::integrateTransforms( btScalar timeStep )
}
}
int btDiscreteDynamicsWorldMt::stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep )
{
int numSubSteps = btDiscreteDynamicsWorld::stepSimulation(timeStep, maxSubSteps, fixedTimeStep);
if (btITaskScheduler* scheduler = btGetTaskScheduler())
{
// tell Bullet's threads to sleep, so other threads can run
scheduler->sleepWorkerThreadsHint();
}
return numSubSteps;
}

6
thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h vendored
View File

@ -21,7 +21,6 @@ subject to the following restrictions:
#include "btSimulationIslandManagerMt.h"
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
struct InplaceSolverIslandCallbackMt;
///
/// btConstraintSolverPoolMt - masquerades as a constraint solver, but really it is a threadsafe pool of them.
@ -88,7 +87,7 @@ private:
ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld
{
protected:
InplaceSolverIslandCallbackMt* m_solverIslandCallbackMt;
btConstraintSolver* m_constraintSolverMt;
virtual void solveConstraints(btContactSolverInfo& solverInfo) BT_OVERRIDE;
@ -126,9 +125,12 @@ public:
btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
btBroadphaseInterface* pairCache,
btConstraintSolverPoolMt* constraintSolver, // Note this should be a solver-pool for multi-threading
btConstraintSolver* constraintSolverMt, // single multi-threaded solver for large islands (or NULL)
btCollisionConfiguration* collisionConfiguration
);
virtual ~btDiscreteDynamicsWorldMt();
virtual int stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep ) BT_OVERRIDE;
};
#endif //BT_DISCRETE_DYNAMICS_WORLD_H

143
thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp vendored
View File

@ -22,6 +22,7 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h" // for s_minimumContactManifoldsForBatching
//#include <stdio.h>
#include "LinearMath/btQuickprof.h"
@ -275,7 +276,7 @@ btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::allocateIsland
void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld )
{
BT_PROFILE("islandUnionFindAndQuickSort");
BT_PROFILE("buildIslands");
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
@ -314,13 +315,11 @@ void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btColl
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
if (colObj0->getActivationState()== ACTIVE_TAG)
{
allSleeping = false;
}
if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
if (colObj0->getActivationState()== ACTIVE_TAG ||
colObj0->getActivationState()== DISABLE_DEACTIVATION)
{
allSleeping = false;
break;
}
}
}
@ -546,59 +545,103 @@ void btSimulationIslandManagerMt::mergeIslands()
}
void btSimulationIslandManagerMt::serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback )
void btSimulationIslandManagerMt::solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams)
{
btPersistentManifold** manifolds = island.manifoldArray.size() ? &island.manifoldArray[ 0 ] : NULL;
btTypedConstraint** constraintsPtr = island.constraintArray.size() ? &island.constraintArray[ 0 ] : NULL;
solver->solveGroup( &island.bodyArray[ 0 ],
island.bodyArray.size(),
manifolds,
island.manifoldArray.size(),
constraintsPtr,
island.constraintArray.size(),
*solverParams.m_solverInfo,
solverParams.m_debugDrawer,
solverParams.m_dispatcher
);
}
void btSimulationIslandManagerMt::serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams )
{
BT_PROFILE( "serialIslandDispatch" );
// serial dispatch
btAlignedObjectArray<Island*>& islands = *islandsPtr;
btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool;
for ( int i = 0; i < islands.size(); ++i )
{
Island* island = islands[ i ];
btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL;
btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL;
callback->processIsland( &island->bodyArray[ 0 ],
island->bodyArray.size(),
manifolds,
island->manifoldArray.size(),
constraintsPtr,
island->constraintArray.size(),
island->id
);
solveIsland(solver, *islands[ i ], solverParams);
}
}
struct UpdateIslandDispatcher : public btIParallelForBody
{
btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr;
btSimulationIslandManagerMt::IslandCallback* callback;
btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& m_islandsPtr;
const btSimulationIslandManagerMt::SolverParams& m_solverParams;
UpdateIslandDispatcher(btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& islandsPtr, const btSimulationIslandManagerMt::SolverParams& solverParams)
: m_islandsPtr(islandsPtr), m_solverParams(solverParams)
{}
void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
{
btConstraintSolver* solver = m_solverParams.m_solverPool;
for ( int i = iBegin; i < iEnd; ++i )
{
btSimulationIslandManagerMt::Island* island = ( *islandsPtr )[ i ];
btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL;
btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL;
callback->processIsland( &island->bodyArray[ 0 ],
island->bodyArray.size(),
manifolds,
island->manifoldArray.size(),
constraintsPtr,
island->constraintArray.size(),
island->id
);
btSimulationIslandManagerMt::Island* island = m_islandsPtr[ i ];
btSimulationIslandManagerMt::solveIsland( solver, *island, m_solverParams );
}
}
};
void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback )
void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams )
{
BT_PROFILE( "parallelIslandDispatch" );
int grainSize = 1; // iterations per task
UpdateIslandDispatcher dispatcher;
dispatcher.islandsPtr = islandsPtr;
dispatcher.callback = callback;
btParallelFor( 0, islandsPtr->size(), grainSize, dispatcher );
//
// if there are islands with many contacts, it may be faster to submit these
// large islands *serially* to a single parallel constraint solver, and then later
// submit the remaining smaller islands in parallel to multiple sequential solvers.
//
// Some task schedulers do not deal well with nested parallelFor loops. One implementation
// of OpenMP was actually slower than doing everything single-threaded. Intel TBB
// on the other hand, seems to do a pretty respectable job with it.
//
// When solving islands in parallel, the worst case performance happens when there
// is one very large island and then perhaps a smattering of very small
// islands -- one worker thread takes the large island and the remaining workers
// tear through the smaller islands and then sit idle waiting for the first worker
// to finish. Solving islands in parallel works best when there are numerous small
// islands, roughly equal in size.
//
// By contrast, the other approach -- the parallel constraint solver -- is only
// able to deliver a worthwhile speedup when the island is large. For smaller islands,
// it is difficult to extract a useful amount of parallelism -- the overhead of grouping
// the constraints into batches and sending the batches to worker threads can nullify
// any gains from parallelism.
//
UpdateIslandDispatcher dispatcher(*islandsPtr, solverParams);
// We take advantage of the fact the islands are sorted in order of decreasing size
int iBegin = 0;
if (solverParams.m_solverMt)
{
while ( iBegin < islandsPtr->size() )
{
btSimulationIslandManagerMt::Island* island = ( *islandsPtr )[ iBegin ];
if ( island->manifoldArray.size() < btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching )
{
// OK to submit the rest of the array in parallel
break;
}
// serial dispatch to parallel solver for large islands (if any)
solveIsland(solverParams.m_solverMt, *island, solverParams);
++iBegin;
}
}
// parallel dispatch to sequential solvers for rest
btParallelFor( iBegin, islandsPtr->size(), 1, dispatcher );
}
@ -606,15 +649,14 @@ void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray<I
void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatcher,
btCollisionWorld* collisionWorld,
btAlignedObjectArray<btTypedConstraint*>& constraints,
IslandCallback* callback
const SolverParams& solverParams
)
{
BT_PROFILE("buildAndProcessIslands");
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
buildIslands(dispatcher,collisionWorld);
BT_PROFILE("processIslands");
if(!getSplitIslands())
{
btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer();
@ -646,14 +688,17 @@ void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatch
}
}
btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[ 0 ] : NULL;
callback->processIsland(&collisionObjects[0],
collisionObjects.size(),
manifolds,
maxNumManifolds,
constraintsPtr,
constraints.size(),
-1
);
btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool;
solver->solveGroup(&collisionObjects[0],
collisionObjects.size(),
manifolds,
maxNumManifolds,
constraintsPtr,
constraints.size(),
*solverParams.m_solverInfo,
solverParams.m_debugDrawer,
solverParams.m_dispatcher
);
}
else
{
@ -673,6 +718,6 @@ void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatch
mergeIslands();
}
// dispatch islands to solver
m_islandDispatch( &m_activeIslands, callback );
m_islandDispatch( &m_activeIslands, solverParams );
}
}

36
thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h vendored
View File

@ -19,7 +19,9 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
class btTypedConstraint;
class btConstraintSolver;
struct btContactSolverInfo;
class btIDebugDraw;
///
/// SimulationIslandManagerMt -- Multithread capable version of SimulationIslandManager
@ -45,22 +47,19 @@ public:
void append( const Island& other ); // add bodies, manifolds, constraints to my own
};
struct IslandCallback
struct SolverParams
{
virtual ~IslandCallback() {};
virtual void processIsland( btCollisionObject** bodies,
int numBodies,
btPersistentManifold** manifolds,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
int islandId
) = 0;
btConstraintSolver* m_solverPool;
btConstraintSolver* m_solverMt;
btContactSolverInfo* m_solverInfo;
btIDebugDraw* m_debugDrawer;
btDispatcher* m_dispatcher;
};
typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray<Island*>* islands, IslandCallback* callback );
static void serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback );
static void parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback );
static void solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams);
typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray<Island*>* islands, const SolverParams& solverParams );
static void serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams );
static void parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams );
protected:
btAlignedObjectArray<Island*> m_allocatedIslands; // owner of all Islands
btAlignedObjectArray<Island*> m_activeIslands; // islands actively in use
@ -83,7 +82,11 @@ public:
btSimulationIslandManagerMt();
virtual ~btSimulationIslandManagerMt();
virtual void buildAndProcessIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld, btAlignedObjectArray<btTypedConstraint*>& constraints, IslandCallback* callback );
virtual void buildAndProcessIslands( btDispatcher* dispatcher,
btCollisionWorld* collisionWorld,
btAlignedObjectArray<btTypedConstraint*>& constraints,
const SolverParams& solverParams
);
virtual void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
@ -106,5 +109,6 @@ public:
}
};
#endif //BT_SIMULATION_ISLAND_MANAGER_H

115
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp vendored
View File

@ -112,14 +112,15 @@ btMultiBody::btMultiBody(int n_links,
m_userObjectPointer(0),
m_userIndex2(-1),
m_userIndex(-1),
m_companionId(-1),
m_linearDamping(0.04f),
m_angularDamping(0.04f),
m_useGyroTerm(true),
m_maxAppliedImpulse(1000.f),
m_maxAppliedImpulse(1000.f),
m_maxCoordinateVelocity(100.f),
m_hasSelfCollision(true),
m_hasSelfCollision(true),
__posUpdated(false),
m_dofCount(0),
m_dofCount(0),
m_posVarCnt(0),
m_useRK4(false),
m_useGlobalVelocities(false),
@ -136,6 +137,9 @@ btMultiBody::btMultiBody(int n_links,
m_baseForce.setValue(0, 0, 0);
m_baseTorque.setValue(0, 0, 0);
clearConstraintForces();
clearForcesAndTorques();
}
btMultiBody::~btMultiBody()
@ -740,13 +744,13 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
btVector3 base_vel = getBaseVel();
btVector3 base_omega = getBaseOmega();
const btVector3 base_vel = getBaseVel();
const btVector3 base_omega = getBaseOmega();
// Temporary matrices/vectors -- use scratch space from caller
// so that we don't have to keep reallocating every frame
scratch_r.resize(2*m_dofCount + 6); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
scratch_r.resize(2*m_dofCount + 7); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
scratch_v.resize(8*num_links + 6);
scratch_m.resize(4*num_links + 4);
@ -815,13 +819,13 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
}
else
{
btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
const btVector3& baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
const btVector3& baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
//external forces
zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
//adding damping terms (only)
btScalar linDampMult = 1., angDampMult = 1.;
const btScalar linDampMult = 1., angDampMult = 1.;
zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
@ -963,16 +967,15 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
//
Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
- m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
- spatCoriolisAcc[i].dot(hDof)
;
}
- spatCoriolisAcc[i].dot(hDof);
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
}
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
{
btScalar *D_row = &D[dof * m_links[i].m_dofCount];
for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
{
btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
}
}
@ -983,14 +986,20 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
case btMultibodyLink::ePrismatic:
case btMultibodyLink::eRevolute:
{
invDi[0] = 1.0f / D[0];
if (D[0]>=SIMD_EPSILON)
{
invDi[0] = 1.0f / D[0];
} else
{
invDi[0] = 0;
}
break;
}
case btMultibodyLink::eSpherical:
case btMultibodyLink::ePlanar:
{
btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
const btMatrix3x3 invD3x3(D3x3.inverse());
//unroll the loop?
for(int row = 0; row < 3; ++row)
@ -1016,7 +1025,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
{
btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
//
spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
}
@ -1027,7 +1036,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
//determine (h*D^{-1}) * h^{T}
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
{
btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
//
dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
}
@ -1048,7 +1057,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
{
btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
//
spatForceVecTemps[0] += hDof * invD_times_Y[dof];
}
@ -1099,7 +1108,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
{
btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
//
Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);
}
@ -1159,12 +1168,12 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
}
// transform base accelerations back to the world frame.
btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
const btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
output[0] = omegadot_out[0];
output[1] = omegadot_out[1];
output[2] = omegadot_out[2];
btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
const btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
output[3] = vdot_out[0];
output[4] = vdot_out[1];
output[5] = vdot_out[2];
@ -1266,12 +1275,29 @@ void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bo
if (num_links == 0)
{
// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
result[0] = rhs_bot[0] / m_baseInertia[0];
result[1] = rhs_bot[1] / m_baseInertia[1];
if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
{
result[0] = rhs_bot[0] / m_baseInertia[0];
result[1] = rhs_bot[1] / m_baseInertia[1];
result[2] = rhs_bot[2] / m_baseInertia[2];
result[3] = rhs_top[0] / m_baseMass;
result[4] = rhs_top[1] / m_baseMass;
result[5] = rhs_top[2] / m_baseMass;
} else
{
result[0] = 0;
result[1] = 0;
result[2] = 0;
}
if (m_baseMass>=SIMD_EPSILON)
{
result[3] = rhs_top[0] / m_baseMass;
result[4] = rhs_top[1] / m_baseMass;
result[5] = rhs_top[2] / m_baseMass;
} else
{
result[3] = 0;
result[4] = 0;
result[5] = 0;
}
} else
{
if (!m_cachedInertiaValid)
@ -1322,9 +1348,21 @@ void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionV
if (num_links == 0)
{
// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
result.setAngular(rhs.getAngular() / m_baseInertia);
result.setLinear(rhs.getLinear() / m_baseMass);
} else
if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
{
result.setAngular(rhs.getAngular() / m_baseInertia);
} else
{
result.setAngular(btVector3(0,0,0));
}
if (m_baseMass>=SIMD_EPSILON)
{
result.setLinear(rhs.getLinear() / m_baseMass);
} else
{
result.setLinear(btVector3(0,0,0));
}
} else
{
/// Special routine for calculating the inverse of a spatial inertia matrix
///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
@ -1808,6 +1846,7 @@ void btMultiBody::fillConstraintJacobianMultiDof(int link,
void btMultiBody::wakeUp()
{
m_sleepTimer = 0;
m_awake = true;
}
@ -1956,7 +1995,11 @@ int btMultiBody::calculateSerializeBufferSize() const
const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const
{
btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer;
getBaseWorldTransform().serialize(mbd->m_baseWorldTransform);
getBasePos().serialize(mbd->m_baseWorldPosition);
getWorldToBaseRot().inverse().serialize(mbd->m_baseWorldOrientation);
getBaseVel().serialize(mbd->m_baseLinearVelocity);
getBaseOmega().serialize(mbd->m_baseAngularVelocity);
mbd->m_baseMass = this->getBaseMass();
getBaseInertia().serialize(mbd->m_baseInertia);
{
@ -1982,6 +2025,12 @@ const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali
memPtr->m_posVarCount = getLink(i).m_posVarCount;
getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
getLink(i).m_absFrameTotVelocity.m_topVec.serialize(memPtr->m_absFrameTotVelocityTop);
getLink(i).m_absFrameTotVelocity.m_bottomVec.serialize(memPtr->m_absFrameTotVelocityBottom);
getLink(i).m_absFrameLocVelocity.m_topVec.serialize(memPtr->m_absFrameLocVelocityTop);
getLink(i).m_absFrameLocVelocity.m_bottomVec.serialize(memPtr->m_absFrameLocVelocityBottom);
memPtr->m_linkMass = getLink(i).m_mass;
memPtr->m_parentIndex = getLink(i).m_parent;
memPtr->m_jointDamping = getLink(i).m_jointDamping;
@ -1991,7 +2040,7 @@ const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali
memPtr->m_jointMaxForce = getLink(i).m_jointMaxForce;
memPtr->m_jointMaxVelocity = getLink(i).m_jointMaxVelocity;
getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset);
getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset);
getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
btAssert(memPtr->m_dofCount<=3);

50
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h vendored
View File

@ -702,14 +702,17 @@ private:
int m_companionId;
btScalar m_linearDamping;
btScalar m_angularDamping;
bool m_useGyroTerm;
bool m_useGyroTerm;
btScalar m_maxAppliedImpulse;
btScalar m_maxCoordinateVelocity;
bool m_hasSelfCollision;
bool __posUpdated;
int m_dofCount, m_posVarCnt;
bool __posUpdated;
int m_dofCount, m_posVarCnt;
bool m_useRK4, m_useGlobalVelocities;
//for global velocities, see 8.3.2B Proposed resolution in Jakub Stepien PhD Thesis
//https://drive.google.com/file/d/0Bz3vEa19XOYGNWdZWGpMdUdqVmZ5ZVBOaEh4ZnpNaUxxZFNV/view?usp=sharing
///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
bool m_internalNeedsJointFeedback;
@ -718,12 +721,17 @@ private:
struct btMultiBodyLinkDoubleData
{
btQuaternionDoubleData m_zeroRotParentToThis;
btVector3DoubleData m_parentComToThisComOffset;
btVector3DoubleData m_parentComToThisPivotOffset;
btVector3DoubleData m_thisPivotToThisComOffset;
btVector3DoubleData m_jointAxisTop[6];
btVector3DoubleData m_jointAxisBottom[6];
btVector3DoubleData m_linkInertia; // inertia of the base (in local frame; diagonal)
btVector3DoubleData m_absFrameTotVelocityTop;
btVector3DoubleData m_absFrameTotVelocityBottom;
btVector3DoubleData m_absFrameLocVelocityTop;
btVector3DoubleData m_absFrameLocVelocityBottom;
double m_linkMass;
int m_parentIndex;
int m_jointType;
@ -751,11 +759,16 @@ struct btMultiBodyLinkDoubleData
struct btMultiBodyLinkFloatData
{
btQuaternionFloatData m_zeroRotParentToThis;
btVector3FloatData m_parentComToThisComOffset;
btVector3FloatData m_parentComToThisPivotOffset;
btVector3FloatData m_thisPivotToThisComOffset;
btVector3FloatData m_jointAxisTop[6];
btVector3FloatData m_jointAxisBottom[6];
btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal)
btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal)
btVector3FloatData m_absFrameTotVelocityTop;
btVector3FloatData m_absFrameTotVelocityBottom;
btVector3FloatData m_absFrameLocVelocityTop;
btVector3FloatData m_absFrameLocVelocityBottom;
int m_dofCount;
float m_linkMass;
int m_parentIndex;
@ -784,29 +797,38 @@ struct btMultiBodyLinkFloatData
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btMultiBodyDoubleData
{
btTransformDoubleData m_baseWorldTransform;
btVector3DoubleData m_baseWorldPosition;
btQuaternionDoubleData m_baseWorldOrientation;
btVector3DoubleData m_baseLinearVelocity;
btVector3DoubleData m_baseAngularVelocity;
btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal)
double m_baseMass;
int m_numLinks;
char m_padding[4];
char *m_baseName;
btMultiBodyLinkDoubleData *m_links;
btCollisionObjectDoubleData *m_baseCollider;
char *m_paddingPtr;
int m_numLinks;
char m_padding[4];
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btMultiBodyFloatData
{
btVector3FloatData m_baseWorldPosition;
btQuaternionFloatData m_baseWorldOrientation;
btVector3FloatData m_baseLinearVelocity;
btVector3FloatData m_baseAngularVelocity;
btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal)
float m_baseMass;
int m_numLinks;
char *m_baseName;
btMultiBodyLinkFloatData *m_links;
btCollisionObjectFloatData *m_baseCollider;
btTransformFloatData m_baseWorldTransform;
btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal)
float m_baseMass;
int m_numLinks;
};

10
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp vendored
View File

@ -253,7 +253,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
if (angConstraint) {
denom0 = rb0->getInvMass() + constraintNormalAng.dot(vec);
denom0 = constraintNormalAng.dot(solverConstraint.m_angularComponentA);
}
else {
denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec);
@ -277,7 +277,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
if (angConstraint) {
denom1 = rb1->getInvMass() + constraintNormalAng.dot(vec);
denom1 = constraintNormalAng.dot(-solverConstraint.m_angularComponentB);
}
else {
denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec);
@ -315,7 +315,8 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
}
else if(rb0)
{
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
rel_vel += rb0->getLinearVelocity().dot(solverConstraint.m_contactNormal1);
rel_vel += rb0->getAngularVelocity().dot(solverConstraint.m_relpos1CrossNormal);
}
if (multiBodyB)
{
@ -327,7 +328,8 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
}
else if(rb1)
{
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
rel_vel += rb1->getLinearVelocity().dot(solverConstraint.m_contactNormal2);
rel_vel += rb1->getAngularVelocity().dot(solverConstraint.m_relpos2CrossNormal);
}
solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;

8
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h vendored
View File

@ -119,6 +119,14 @@ public:
return m_bodyB;
}
int getLinkA() const
{
return m_linkA;
}
int getLinkB() const
{
return m_linkB;
}
void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
{
btAssert(dof>=0);

371
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp vendored
View File

@ -39,7 +39,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
btScalar residual = resolveSingleConstraintRowGeneric(constraint);
leastSquaredResidual += residual*residual;
leastSquaredResidual = btMax(leastSquaredResidual,residual*residual);
if(constraint.m_multiBodyA)
constraint.m_multiBodyA->setPosUpdated(false);
@ -60,7 +60,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
residual = resolveSingleConstraintRowGeneric(constraint);
}
leastSquaredResidual += residual*residual;
leastSquaredResidual = btMax(leastSquaredResidual,residual*residual);
if(constraint.m_multiBodyA)
constraint.m_multiBodyA->setPosUpdated(false);
@ -69,27 +69,92 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
}
//solve featherstone frictional contact
for (int j1=0;j1<this->m_multiBodyFrictionContactConstraints.size();j1++)
if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0))
{
if (iteration < infoGlobal.m_numIterations)
for (int j1 = 0; j1<this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++)
{
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))
if (iteration < infoGlobal.m_numIterations)
{
frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
leastSquaredResidual += residual*residual;
int index = j1;//iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1;
if(frictionConstraint.m_multiBodyA)
frictionConstraint.m_multiBodyA->setPosUpdated(false);
if(frictionConstraint.m_multiBodyB)
frictionConstraint.m_multiBodyB->setPosUpdated(false);
btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index];
btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
//adjust friction limits here
if (totalImpulse>btScalar(0))
{
frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
leastSquaredResidual = btMax(leastSquaredResidual , residual*residual);
if (frictionConstraint.m_multiBodyA)
frictionConstraint.m_multiBodyA->setPosUpdated(false);
if (frictionConstraint.m_multiBodyB)
frictionConstraint.m_multiBodyB->setPosUpdated(false);
}
}
}
for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++)
{
if (iteration < infoGlobal.m_numIterations)
{
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;
j1++;
int index2 = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyFrictionContactConstraints[index2];
btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex);
if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
{
frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction*totalImpulse);
frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction*totalImpulse;
btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
leastSquaredResidual = btMax(leastSquaredResidual, residual*residual);
if (frictionConstraintB.m_multiBodyA)
frictionConstraintB.m_multiBodyA->setPosUpdated(false);
if (frictionConstraintB.m_multiBodyB)
frictionConstraintB.m_multiBodyB->setPosUpdated(false);
if (frictionConstraint.m_multiBodyA)
frictionConstraint.m_multiBodyA->setPosUpdated(false);
if (frictionConstraint.m_multiBodyB)
frictionConstraint.m_multiBodyB->setPosUpdated(false);
}
}
}
}
else
{
for (int j1 = 0; j1<this->m_multiBodyFrictionContactConstraints.size(); j1++)
{
if (iteration < infoGlobal.m_numIterations)
{
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))
{
frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
leastSquaredResidual = btMax(leastSquaredResidual, residual*residual);
if (frictionConstraint.m_multiBodyA)
frictionConstraint.m_multiBodyA->setPosUpdated(false);
if (frictionConstraint.m_multiBodyB)
frictionConstraint.m_multiBodyB->setPosUpdated(false);
}
}
}
}
@ -101,6 +166,8 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb
m_multiBodyNonContactConstraints.resize(0);
m_multiBodyNormalContactConstraints.resize(0);
m_multiBodyFrictionContactConstraints.resize(0);
m_multiBodyTorsionalFrictionContactConstraints.resize(0);
m_data.m_jacobians.resize(0);
m_data.m_deltaVelocitiesUnitImpulse.resize(0);
m_data.m_deltaVelocities.resize(0);
@ -128,49 +195,51 @@ void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar im
btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
btScalar deltaVelADotn=0;
btScalar deltaVelBDotn=0;
btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
btScalar deltaVelADotn = 0;
btScalar deltaVelBDotn = 0;
btSolverBody* bodyA = 0;
btSolverBody* bodyB = 0;
int ndofA=0;
int ndofB=0;
int ndofA = 0;
int ndofB = 0;
if (c.m_multiBodyA)
{
ndofA = c.m_multiBodyA->getNumDofs() + 6;
ndofA = c.m_multiBodyA->getNumDofs() + 6;
for (int i = 0; i < ndofA; ++i)
deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
} else if(c.m_solverBodyIdA >= 0)
deltaVelADotn += m_data.m_jacobians[c.m_jacAindex + i] * m_data.m_deltaVelocities[c.m_deltaVelAindex + i];
}
else if (c.m_solverBodyIdA >= 0)
{
bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
}
if (c.m_multiBodyB)
{
ndofB = c.m_multiBodyB->getNumDofs() + 6;
ndofB = c.m_multiBodyB->getNumDofs() + 6;
for (int i = 0; i < ndofB; ++i)
deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
} else if(c.m_solverBodyIdB >= 0)
deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex + i] * m_data.m_deltaVelocities[c.m_deltaVelBindex + i];
}
else if (c.m_solverBodyIdB >= 0)
{
bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
}
deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
c.m_appliedImpulse = c.m_lowerLimit;
}
else if (sum > c.m_upperLimit)
{
deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
c.m_appliedImpulse = c.m_upperLimit;
}
else
@ -180,30 +249,213 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt
if (c.m_multiBodyA)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse);
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
} else if(c.m_solverBodyIdA >= 0)
}
else if (c.m_solverBodyIdA >= 0)
{
bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
}
if (c.m_multiBodyB)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB);
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse);
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
} else if(c.m_solverBodyIdB >= 0)
{
bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
}
return deltaImpulse;
else if (c.m_solverBodyIdB >= 0)
{
bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
}
btScalar deltaVel =deltaImpulse/c.m_jacDiagABInv;
return deltaVel;
}
btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1,const btMultiBodySolverConstraint& cB)
{
int ndofA=0;
int ndofB=0;
btSolverBody* bodyA = 0;
btSolverBody* bodyB = 0;
btScalar deltaImpulseB = 0.f;
btScalar sumB = 0.f;
{
deltaImpulseB = cB.m_rhs-btScalar(cB.m_appliedImpulse)*cB.m_cfm;
btScalar deltaVelADotn=0;
btScalar deltaVelBDotn=0;
if (cB.m_multiBodyA)
{
ndofA = cB.m_multiBodyA->getNumDofs() + 6;
for (int i = 0; i < ndofA; ++i)
deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex+i];
} else if(cB.m_solverBodyIdA >= 0)
{
bodyA = &m_tmpSolverBodyPool[cB.m_solverBodyIdA];
deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
}
if (cB.m_multiBodyB)
{
ndofB = cB.m_multiBodyB->getNumDofs() + 6;
for (int i = 0; i < ndofB; ++i)
deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex+i];
} else if(cB.m_solverBodyIdB >= 0)
{
bodyB = &m_tmpSolverBodyPool[cB.m_solverBodyIdB];
deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
}
deltaImpulseB -= deltaVelADotn*cB.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
deltaImpulseB -= deltaVelBDotn*cB.m_jacDiagABInv;
sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB;
}
btScalar deltaImpulseA = 0.f;
btScalar sumA = 0.f;
const btMultiBodySolverConstraint& cA = cA1;
{
{
deltaImpulseA = cA.m_rhs-btScalar(cA.m_appliedImpulse)*cA.m_cfm;
btScalar deltaVelADotn=0;
btScalar deltaVelBDotn=0;
if (cA.m_multiBodyA)
{
ndofA = cA.m_multiBodyA->getNumDofs() + 6;
for (int i = 0; i < ndofA; ++i)
deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex+i];
} else if(cA.m_solverBodyIdA >= 0)
{
bodyA = &m_tmpSolverBodyPool[cA.m_solverBodyIdA];
deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
}
if (cA.m_multiBodyB)
{
ndofB = cA.m_multiBodyB->getNumDofs() + 6;
for (int i = 0; i < ndofB; ++i)
deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex+i];
} else if(cA.m_solverBodyIdB >= 0)
{
bodyB = &m_tmpSolverBodyPool[cA.m_solverBodyIdB];
deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
}
deltaImpulseA -= deltaVelADotn*cA.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
deltaImpulseA -= deltaVelBDotn*cA.m_jacDiagABInv;
sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA;
}
}
if (sumA*sumA+sumB*sumB>=cA.m_lowerLimit*cB.m_lowerLimit)
{
btScalar angle = btAtan2(sumA,sumB);
btScalar sumAclipped = btFabs(cA.m_lowerLimit*btSin(angle));
btScalar sumBclipped = btFabs(cB.m_lowerLimit*btCos(angle));
if (sumA < -sumAclipped)
{
deltaImpulseA = -sumAclipped - cA.m_appliedImpulse;
cA.m_appliedImpulse = -sumAclipped;
}
else if (sumA > sumAclipped)
{
deltaImpulseA = sumAclipped - cA.m_appliedImpulse;
cA.m_appliedImpulse = sumAclipped;
}
else
{
cA.m_appliedImpulse = sumA;
}
if (sumB < -sumBclipped)
{
deltaImpulseB = -sumBclipped - cB.m_appliedImpulse;
cB.m_appliedImpulse = -sumBclipped;
}
else if (sumB > sumBclipped)
{
deltaImpulseB = sumBclipped - cB.m_appliedImpulse;
cB.m_appliedImpulse = sumBclipped;
}
else
{
cB.m_appliedImpulse = sumB;
}
//deltaImpulseA = sumAclipped-cA.m_appliedImpulse;
//cA.m_appliedImpulse = sumAclipped;
//deltaImpulseB = sumBclipped-cB.m_appliedImpulse;
//cB.m_appliedImpulse = sumBclipped;
}
else
{
cA.m_appliedImpulse = sumA;
cB.m_appliedImpulse = sumB;
}
if (cA.m_multiBodyA)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex],deltaImpulseA,cA.m_deltaVelAindex,ndofA);
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex],deltaImpulseA);
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
} else if(cA.m_solverBodyIdA >= 0)
{
bodyA->internalApplyImpulse(cA.m_contactNormal1*bodyA->internalGetInvMass(),cA.m_angularComponentA,deltaImpulseA);
}
if (cA.m_multiBodyB)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex],deltaImpulseA,cA.m_deltaVelBindex,ndofB);
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex],deltaImpulseA);
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
} else if(cA.m_solverBodyIdB >= 0)
{
bodyB->internalApplyImpulse(cA.m_contactNormal2*bodyB->internalGetInvMass(),cA.m_angularComponentB,deltaImpulseA);
}
if (cB.m_multiBodyA)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex],deltaImpulseB,cB.m_deltaVelAindex,ndofA);
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex],deltaImpulseB);
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
} else if(cB.m_solverBodyIdA >= 0)
{
bodyA->internalApplyImpulse(cB.m_contactNormal1*bodyA->internalGetInvMass(),cB.m_angularComponentA,deltaImpulseB);
}
if (cB.m_multiBodyB)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex],deltaImpulseB,cB.m_deltaVelBindex,ndofB);
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex],deltaImpulseB);
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
} else if(cB.m_solverBodyIdB >= 0)
{
bodyB->internalApplyImpulse(cB.m_contactNormal2*bodyB->internalGetInvMass(),cB.m_angularComponentB,deltaImpulseB);
}
btScalar deltaVel =deltaImpulseA/cA.m_jacDiagABInv+deltaImpulseB/cB.m_jacDiagABInv;
return deltaVel;
}
@ -908,7 +1160,10 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalF
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
{
BT_PROFILE("addMultiBodyRollingFrictionConstraint");
btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
bool useTorsionalAndConeFriction = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0));
btMultiBodySolverConstraint& solverConstraint = useTorsionalAndConeFriction? m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing() : m_multiBodyFrictionContactConstraints.expandNonInitializing();
solverConstraint.m_orgConstraint = 0;
solverConstraint.m_orgDofIndex = -1;
@ -1151,6 +1406,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
{
//printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints);
return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
}
@ -1234,27 +1490,12 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
if (c.m_multiBodyA)
{
if(c.m_multiBodyA->isMultiDof())
{
c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
}
else
{
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
}
c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
}
if (c.m_multiBodyB)
{
if(c.m_multiBodyB->isMultiDof())
{
c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
}
else
{
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
}
c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
}
#endif
@ -1416,6 +1657,8 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
{
//printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints);
//printf("solveMultiBodyGroup start\n");
m_tmpMultiBodyConstraints = multiBodyConstraints;
m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;

4
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h vendored
View File

@ -36,6 +36,7 @@ protected:
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints;
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints;
btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints;
btMultiBodyJacobianData m_data;
@ -45,6 +46,9 @@ protected:
btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
//solve 2 friction directions and clamp against the implicit friction cone
btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB);
void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);

64
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp vendored
View File

@ -277,6 +277,10 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
m_multiBodyConstraints.resize(0);
}
void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
{
m_solver = solver;
}
virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId)
{
@ -348,7 +352,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
for (i=0;i<numCurMultiBodyConstraints;i++)
m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
if ((m_multiBodyConstraints.size()+m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
{
processConstraints();
} else
@ -394,6 +398,22 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
delete m_solverMultiBodyIslandCallback;
}
void btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
{
m_multiBodyConstraintSolver = solver;
m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver);
btDiscreteDynamicsWorld::setConstraintSolver(solver);
}
void btMultiBodyDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
{
if (solver->getSolverType()==BT_MULTIBODY_SOLVER)
{
m_multiBodyConstraintSolver = (btMultiBodyConstraintSolver*)solver;
}
btDiscreteDynamicsWorld::setConstraintSolver(solver);
}
void btMultiBodyDynamicsWorld::forwardKinematics()
{
@ -411,6 +431,8 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
BT_PROFILE("solveConstraints");
clearMultiBodyConstraintForces();
m_sortedConstraints.resize( m_constraints.size());
int i;
for (i=0;i<getNumConstraints();i++)
@ -433,8 +455,6 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
{
@ -669,7 +689,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
}
}
clearMultiBodyConstraintForces();
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
m_solverMultiBodyIslandCallback->processConstraints();
@ -825,20 +847,23 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
btMultiBody* bod = m_multiBodies[b];
bod->forwardKinematics(m_scratch_world_to_local1,m_scratch_local_origin1);
getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
if (mode & btIDebugDraw::DBG_DrawFrames)
{
getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
}
for (int m = 0; m<bod->getNumLinks(); m++)
{
const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
getDebugDrawer()->drawTransform(tr, 0.1);
if (mode & btIDebugDraw::DBG_DrawFrames)
{
getDebugDrawer()->drawTransform(tr, 0.1);
}
//draw the joint axis
if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec)*0.1;
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
@ -847,7 +872,7 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
}
if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec)*0.1;
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
@ -856,7 +881,7 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
}
if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec)*0.1;
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
@ -970,6 +995,8 @@ void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
serializeCollisionObjects(serializer);
serializeContactManifolds(serializer);
serializer->finishSerialization();
}
@ -988,4 +1015,17 @@ void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
}
}
//serialize all multibody links (collision objects)
for (i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
if (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
int len = colObj->calculateSerializeBufferSize();
btChunk* chunk = serializer->allocate(len,1);
const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
serializer->finalizeChunk(chunk,structType,BT_MB_LINKCOLLIDER_CODE,colObj);
}
}
}

2
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h vendored
View File

@ -109,6 +109,8 @@ public:
virtual void applyGravity();
virtual void serialize(btSerializer* serializer);
virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver);
virtual void setConstraintSolver(btConstraintSolver* solver);
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

28
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp vendored
View File

@ -65,13 +65,16 @@ int btMultiBodyFixedConstraint::getIslandIdA() const
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
if (m_linkA < 0)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyA->getLink(m_linkA).m_collider)
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
}
}
return -1;
@ -83,16 +86,17 @@ int btMultiBodyFixedConstraint::getIslandIdB() const
return m_rigidBodyB->getIslandTag();
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
if (m_linkB < 0)
{
col = m_bodyB->getLink(i).m_collider;
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyB->getLink(m_linkB).m_collider)
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
}
}
return -1;
}

33
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp vendored
View File

@ -45,16 +45,18 @@ btMultiBodyGearConstraint::~btMultiBodyGearConstraint()
int btMultiBodyGearConstraint::getIslandIdA() const
{
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
if (m_linkA < 0)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyA->getLink(m_linkA).m_collider)
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
}
}
return -1;
@ -64,16 +66,17 @@ int btMultiBodyGearConstraint::getIslandIdB() const
{
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
if (m_linkB < 0)
{
col = m_bodyB->getLink(i).m_collider;
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyB->getLink(m_linkB).m_collider)
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
}
}
return -1;
}
@ -134,6 +137,10 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray&
if (m_erp!=0)
{
btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
if (m_gearAuxLink >= 0)
{
currentPositionA -= m_bodyA->getJointPosMultiDof(m_gearAuxLink)[dof];
}
btScalar currentPositionB = m_gearRatio*m_bodyA->getJointPosMultiDof(m_linkB)[dof];
btScalar diff = currentPositionB+currentPositionA;
btScalar desiredPositionDiff = this->m_relativePositionTarget;

34
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp vendored
View File

@ -53,17 +53,22 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
{
}
int btMultiBodyJointLimitConstraint::getIslandIdA() const
{
if(m_bodyA)
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
if (m_linkA < 0)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyA->getLink(m_linkA).m_collider)
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
}
}
return -1;
@ -71,18 +76,19 @@ int btMultiBodyJointLimitConstraint::getIslandIdA() const
int btMultiBodyJointLimitConstraint::getIslandIdB() const
{
if(m_bodyB)
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
if (m_linkB < 0)
{
col = m_bodyB->getLink(i).m_collider;
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyB->getLink(m_linkB).m_collider)
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
}
}
return -1;
}

32
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp vendored
View File

@ -74,29 +74,37 @@ btMultiBodyJointMotor::~btMultiBodyJointMotor()
int btMultiBodyJointMotor::getIslandIdA() const
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
if (this->m_linkA < 0)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyA->getLink(m_linkA).m_collider)
{
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodyJointMotor::getIslandIdB() const
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
if (m_linkB < 0)
{
col = m_bodyB->getLink(i).m_collider;
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyB->getLink(m_linkB).m_collider)
{
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
}
}
return -1;
}

2
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h vendored
View File

@ -182,6 +182,8 @@ btVector3 m_appliedConstraintForce; // In WORLD frame
m_cachedRVector.setValue(0, 0, 0);
m_appliedForce.setValue( 0, 0, 0);
m_appliedTorque.setValue(0, 0, 0);
m_appliedConstraintForce.setValue(0,0,0);
m_appliedConstraintTorque.setValue(0,0,0);
//
m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
m_jointPos[3] = 1.f; //"quat.w"

52
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h vendored
View File

@ -19,6 +19,16 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "btMultiBody.h"
#include "LinearMath/btSerializer.h"
#ifdef BT_USE_DOUBLE_PRECISION
#define btMultiBodyLinkColliderData btMultiBodyLinkColliderDoubleData
#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderDoubleData"
#else
#define btMultiBodyLinkColliderData btMultiBodyLinkColliderFloatData
#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderFloatData"
#endif
class btMultiBodyLinkCollider : public btCollisionObject
{
@ -119,7 +129,49 @@ public:
}
return true;
}
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
};
struct btMultiBodyLinkColliderFloatData
{
btCollisionObjectFloatData m_colObjData;
btMultiBodyFloatData *m_multiBody;
int m_link;
char m_padding[4];
};
struct btMultiBodyLinkColliderDoubleData
{
btCollisionObjectDoubleData m_colObjData;
btMultiBodyDoubleData *m_multiBody;
int m_link;
char m_padding[4];
};
SIMD_FORCE_INLINE int btMultiBodyLinkCollider::calculateSerializeBufferSize() const
{
return sizeof(btMultiBodyLinkColliderData);
}
SIMD_FORCE_INLINE const char* btMultiBodyLinkCollider::serialize(void* dataBuffer, class btSerializer* serializer) const
{
btMultiBodyLinkColliderData* dataOut = (btMultiBodyLinkColliderData*)dataBuffer;
btCollisionObject::serialize(&dataOut->m_colObjData,serializer);
dataOut->m_link = this->m_link;
dataOut->m_multiBody = (btMultiBodyData*)serializer->getUniquePointer(m_multiBody);
// Fill padding with zeros to appease msan.
memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding));
return btMultiBodyLinkColliderDataName;
}
#endif //BT_FEATHERSTONE_LINK_COLLIDER_H

966
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp vendored Normal file
View File

@ -0,0 +1,966 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2018 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
static bool interleaveContactAndFriction = false;
struct btJointNode
{
int jointIndex; // pointer to enclosing dxJoint object
int otherBodyIndex; // *other* body this joint is connected to
int nextJointNodeIndex; //-1 for null
int constraintRowIndex;
};
// Helper function to compute a delta velocity in the constraint space.
static btScalar computeDeltaVelocityInConstraintSpace(
const btVector3& angularDeltaVelocity,
const btVector3& contactNormal,
btScalar invMass,
const btVector3& angularJacobian,
const btVector3& linearJacobian)
{
return angularDeltaVelocity.dot(angularJacobian) + contactNormal.dot(linearJacobian) * invMass;
}
// Faster version of computeDeltaVelocityInConstraintSpace that can be used when contactNormal and linearJacobian are
// identical.
static btScalar computeDeltaVelocityInConstraintSpace(
const btVector3& angularDeltaVelocity,
btScalar invMass,
const btVector3& angularJacobian)
{
return angularDeltaVelocity.dot(angularJacobian) + invMass;
}
// Helper function to compute a delta velocity in the constraint space.
static btScalar computeDeltaVelocityInConstraintSpace(const btScalar* deltaVelocity, const btScalar* jacobian, int size)
{
btScalar result = 0;
for (int i = 0; i < size; ++i)
result += deltaVelocity[i] * jacobian[i];
return result;
}
static btScalar computeConstraintMatrixDiagElementMultiBody(
const btAlignedObjectArray<btSolverBody>& solverBodyPool,
const btMultiBodyJacobianData& data,
const btMultiBodySolverConstraint& constraint)
{
btScalar ret = 0;
const btMultiBody* multiBodyA = constraint.m_multiBodyA;
const btMultiBody* multiBodyB = constraint.m_multiBodyB;
if (multiBodyA)
{
const btScalar* jacA = &data.m_jacobians[constraint.m_jacAindex];
const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
const int ndofA = multiBodyA->getNumDofs() + 6;
ret += computeDeltaVelocityInConstraintSpace(deltaA, jacA, ndofA);
}
else
{
const int solverBodyIdA = constraint.m_solverBodyIdA;
btAssert(solverBodyIdA != -1);
const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
ret += computeDeltaVelocityInConstraintSpace(
constraint.m_relpos1CrossNormal,
invMassA,
constraint.m_angularComponentA);
}
if (multiBodyB)
{
const btScalar* jacB = &data.m_jacobians[constraint.m_jacBindex];
const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
const int ndofB = multiBodyB->getNumDofs() + 6;
ret += computeDeltaVelocityInConstraintSpace(deltaB, jacB, ndofB);
}
else
{
const int solverBodyIdB = constraint.m_solverBodyIdB;
btAssert(solverBodyIdB != -1);
const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
ret += computeDeltaVelocityInConstraintSpace(
constraint.m_relpos2CrossNormal,
invMassB,
constraint.m_angularComponentB);
}
return ret;
}
static btScalar computeConstraintMatrixOffDiagElementMultiBody(
const btAlignedObjectArray<btSolverBody>& solverBodyPool,
const btMultiBodyJacobianData& data,
const btMultiBodySolverConstraint& constraint,
const btMultiBodySolverConstraint& offDiagConstraint)
{
btScalar offDiagA = btScalar(0);
const btMultiBody* multiBodyA = constraint.m_multiBodyA;
const btMultiBody* multiBodyB = constraint.m_multiBodyB;
const btMultiBody* offDiagMultiBodyA = offDiagConstraint.m_multiBodyA;
const btMultiBody* offDiagMultiBodyB = offDiagConstraint.m_multiBodyB;
// Assumed at least one system is multibody
btAssert(multiBodyA || multiBodyB);
btAssert(offDiagMultiBodyA || offDiagMultiBodyB);
if (offDiagMultiBodyA)
{
const btScalar* offDiagJacA = &data.m_jacobians[offDiagConstraint.m_jacAindex];
if (offDiagMultiBodyA == multiBodyA)
{
const int ndofA = multiBodyA->getNumDofs() + 6;
const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacA, ndofA);
}
else if (offDiagMultiBodyA == multiBodyB)
{
const int ndofB = multiBodyB->getNumDofs() + 6;
const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacA, ndofB);
}
}
else
{
const int solverBodyIdA = constraint.m_solverBodyIdA;
const int solverBodyIdB = constraint.m_solverBodyIdB;
const int offDiagSolverBodyIdA = offDiagConstraint.m_solverBodyIdA;
btAssert(offDiagSolverBodyIdA != -1);
if (offDiagSolverBodyIdA == solverBodyIdA)
{
btAssert(solverBodyIdA != -1);
const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
offDiagA += computeDeltaVelocityInConstraintSpace(
offDiagConstraint.m_relpos1CrossNormal,
offDiagConstraint.m_contactNormal1,
invMassA, constraint.m_angularComponentA,
constraint.m_contactNormal1);
}
else if (offDiagSolverBodyIdA == solverBodyIdB)
{
btAssert(solverBodyIdB != -1);
const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
offDiagA += computeDeltaVelocityInConstraintSpace(
offDiagConstraint.m_relpos1CrossNormal,
offDiagConstraint.m_contactNormal1,
invMassB,
constraint.m_angularComponentB,
constraint.m_contactNormal2);
}
}
if (offDiagMultiBodyB)
{
const btScalar* offDiagJacB = &data.m_jacobians[offDiagConstraint.m_jacBindex];
if (offDiagMultiBodyB == multiBodyA)
{
const int ndofA = multiBodyA->getNumDofs() + 6;
const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacB, ndofA);
}
else if (offDiagMultiBodyB == multiBodyB)
{
const int ndofB = multiBodyB->getNumDofs() + 6;
const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacB, ndofB);
}
}
else
{
const int solverBodyIdA = constraint.m_solverBodyIdA;
const int solverBodyIdB = constraint.m_solverBodyIdB;
const int offDiagSolverBodyIdB = offDiagConstraint.m_solverBodyIdB;
btAssert(offDiagSolverBodyIdB != -1);
if (offDiagSolverBodyIdB == solverBodyIdA)
{
btAssert(solverBodyIdA != -1);
const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
offDiagA += computeDeltaVelocityInConstraintSpace(
offDiagConstraint.m_relpos2CrossNormal,
offDiagConstraint.m_contactNormal2,
invMassA, constraint.m_angularComponentA,
constraint.m_contactNormal1);
}
else if (offDiagSolverBodyIdB == solverBodyIdB)
{
btAssert(solverBodyIdB != -1);
const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
offDiagA += computeDeltaVelocityInConstraintSpace(
offDiagConstraint.m_relpos2CrossNormal,
offDiagConstraint.m_contactNormal2,
invMassB, constraint.m_angularComponentB,
constraint.m_contactNormal2);
}
}
return offDiagA;
}
void btMultiBodyMLCPConstraintSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
{
createMLCPFastRigidBody(infoGlobal);
createMLCPFastMultiBody(infoGlobal);
}
void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal)
{
int numContactRows = interleaveContactAndFriction ? 3 : 1;
int numConstraintRows = m_allConstraintPtrArray.size();
if (numConstraintRows == 0)
return;
int n = numConstraintRows;
{
BT_PROFILE("init b (rhs)");
m_b.resize(numConstraintRows);
m_bSplit.resize(numConstraintRows);
m_b.setZero();
m_bSplit.setZero();
for (int i = 0; i < numConstraintRows; i++)
{
btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
if (!btFuzzyZero(jacDiag))
{
btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
m_b[i] = rhs / jacDiag;
m_bSplit[i] = rhsPenetration / jacDiag;
}
}
}
// btScalar* w = 0;
// int nub = 0;
m_lo.resize(numConstraintRows);
m_hi.resize(numConstraintRows);
{
BT_PROFILE("init lo/ho");
for (int i = 0; i < numConstraintRows; i++)
{
if (0) //m_limitDependencies[i]>=0)
{
m_lo[i] = -BT_INFINITY;
m_hi[i] = BT_INFINITY;
}
else
{
m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
}
}
}
//
int m = m_allConstraintPtrArray.size();
int numBodies = m_tmpSolverBodyPool.size();
btAlignedObjectArray<int> bodyJointNodeArray;
{
BT_PROFILE("bodyJointNodeArray.resize");
bodyJointNodeArray.resize(numBodies, -1);
}
btAlignedObjectArray<btJointNode> jointNodeArray;
{
BT_PROFILE("jointNodeArray.reserve");
jointNodeArray.reserve(2 * m_allConstraintPtrArray.size());
}
btMatrixXu& J3 = m_scratchJ3;
{
BT_PROFILE("J3.resize");
J3.resize(2 * m, 8);
}
btMatrixXu& JinvM3 = m_scratchJInvM3;
{
BT_PROFILE("JinvM3.resize/setZero");
JinvM3.resize(2 * m, 8);
JinvM3.setZero();
J3.setZero();
}
int cur = 0;
int rowOffset = 0;
btAlignedObjectArray<int>& ofs = m_scratchOfs;
{
BT_PROFILE("ofs resize");
ofs.resize(0);
ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
}
{
BT_PROFILE("Compute J and JinvM");
int c = 0;
int numRows = 0;
for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
{
ofs[c] = rowOffset;
int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
if (orgBodyA)
{
{
int slotA = -1;
//find free jointNode slot for sbA
slotA = jointNodeArray.size();
jointNodeArray.expand(); //NonInitializing();
int prevSlot = bodyJointNodeArray[sbA];
bodyJointNodeArray[sbA] = slotA;
jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
jointNodeArray[slotA].jointIndex = c;
jointNodeArray[slotA].constraintRowIndex = i;
jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
}
for (int row = 0; row < numRows; row++, cur++)
{
btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass();
btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld();
for (int r = 0; r < 3; r++)
{
J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]);
J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]);
JinvM3.setElem(cur, r, normalInvMass[r]);
JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]);
}
J3.setElem(cur, 3, 0);
JinvM3.setElem(cur, 3, 0);
J3.setElem(cur, 7, 0);
JinvM3.setElem(cur, 7, 0);
}
}
else
{
cur += numRows;
}
if (orgBodyB)
{
{
int slotB = -1;
//find free jointNode slot for sbA
slotB = jointNodeArray.size();
jointNodeArray.expand(); //NonInitializing();
int prevSlot = bodyJointNodeArray[sbB];
bodyJointNodeArray[sbB] = slotB;
jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
jointNodeArray[slotB].jointIndex = c;
jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
jointNodeArray[slotB].constraintRowIndex = i;
}
for (int row = 0; row < numRows; row++, cur++)
{
btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass();
btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
for (int r = 0; r < 3; r++)
{
J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]);
J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]);
JinvM3.setElem(cur, r, normalInvMassB[r]);
JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]);
}
J3.setElem(cur, 3, 0);
JinvM3.setElem(cur, 3, 0);
J3.setElem(cur, 7, 0);
JinvM3.setElem(cur, 7, 0);
}
}
else
{
cur += numRows;
}
rowOffset += numRows;
}
}
//compute JinvM = J*invM.
const btScalar* JinvM = JinvM3.getBufferPointer();
const btScalar* Jptr = J3.getBufferPointer();
{
BT_PROFILE("m_A.resize");
m_A.resize(n, n);
}
{
BT_PROFILE("m_A.setZero");
m_A.setZero();
}
int c = 0;
{
int numRows = 0;
BT_PROFILE("Compute A");
for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
{
int row__ = ofs[c];
int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
// btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
// btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
{
int startJointNodeA = bodyJointNodeArray[sbA];
while (startJointNodeA >= 0)
{
int j0 = jointNodeArray[startJointNodeA].jointIndex;
int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
if (j0 < c)
{
int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0;
//printf("%d joint i %d and j0: %d: ",count++,i,j0);
m_A.multiplyAdd2_p8r(JinvMrow,
Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
}
startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
}
}
{
int startJointNodeB = bodyJointNodeArray[sbB];
while (startJointNodeB >= 0)
{
int j1 = jointNodeArray[startJointNodeB].jointIndex;
int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
if (j1 < c)
{
int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0;
m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows,
Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
}
startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
}
}
}
{
BT_PROFILE("compute diagonal");
// compute diagonal blocks of m_A
int row__ = 0;
int numJointRows = m_allConstraintPtrArray.size();
int jj = 0;
for (; row__ < numJointRows;)
{
//int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
// btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__;
m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__);
if (orgBodyB)
{
m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__);
}
row__ += infom;
jj++;
}
}
}
if (1)
{
// add cfm to the diagonal of m_A
for (int i = 0; i < m_A.rows(); ++i)
{
m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
}
}
///fill the upper triangle of the matrix, to make it symmetric
{
BT_PROFILE("fill the upper triangle ");
m_A.copyLowerToUpperTriangle();
}
{
BT_PROFILE("resize/init x");
m_x.resize(numConstraintRows);
m_xSplit.resize(numConstraintRows);
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
{
const btSolverConstraint& c = *m_allConstraintPtrArray[i];
m_x[i] = c.m_appliedImpulse;
m_xSplit[i] = c.m_appliedPushImpulse;
}
}
else
{
m_x.setZero();
m_xSplit.setZero();
}
}
}
void btMultiBodyMLCPConstraintSolver::createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal)
{
const int multiBodyNumConstraints = m_multiBodyAllConstraintPtrArray.size();
if (multiBodyNumConstraints == 0)
return;
// 1. Compute b
{
BT_PROFILE("init b (rhs)");
m_multiBodyB.resize(multiBodyNumConstraints);
m_multiBodyB.setZero();
for (int i = 0; i < multiBodyNumConstraints; ++i)
{
const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
const btScalar jacDiag = constraint.m_jacDiagABInv;
if (!btFuzzyZero(jacDiag))
{
// Note that rhsPenetration is currently always zero because the split impulse hasn't been implemented for multibody yet.
const btScalar rhs = constraint.m_rhs;
m_multiBodyB[i] = rhs / jacDiag;
}
}
}
// 2. Compute lo and hi
{
BT_PROFILE("init lo/ho");
m_multiBodyLo.resize(multiBodyNumConstraints);
m_multiBodyHi.resize(multiBodyNumConstraints);
for (int i = 0; i < multiBodyNumConstraints; ++i)
{
const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
m_multiBodyLo[i] = constraint.m_lowerLimit;
m_multiBodyHi[i] = constraint.m_upperLimit;
}
}
// 3. Construct A matrix by using the impulse testing
{
BT_PROFILE("Compute A");
{
BT_PROFILE("m_A.resize");
m_multiBodyA.resize(multiBodyNumConstraints, multiBodyNumConstraints);
}
for (int i = 0; i < multiBodyNumConstraints; ++i)
{
// Compute the diagonal of A, which is A(i, i)
const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
const btScalar diagA = computeConstraintMatrixDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint);
m_multiBodyA.setElem(i, i, diagA);
// Computes the off-diagonals of A:
// a. The rest of i-th row of A, from A(i, i+1) to A(i, n)
// b. The rest of i-th column of A, from A(i+1, i) to A(n, i)
for (int j = i + 1; j < multiBodyNumConstraints; ++j)
{
const btMultiBodySolverConstraint& offDiagConstraint = *m_multiBodyAllConstraintPtrArray[j];
const btScalar offDiagA = computeConstraintMatrixOffDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint, offDiagConstraint);
// Set the off-diagonal values of A. Note that A is symmetric.
m_multiBodyA.setElem(i, j, offDiagA);
m_multiBodyA.setElem(j, i, offDiagA);
}
}
}
// Add CFM to the diagonal of m_A
for (int i = 0; i < m_multiBodyA.rows(); ++i)
{
m_multiBodyA.setElem(i, i, m_multiBodyA(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
}
// 4. Initialize x
{
BT_PROFILE("resize/init x");
m_multiBodyX.resize(multiBodyNumConstraints);
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
for (int i = 0; i < multiBodyNumConstraints; ++i)
{
const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
m_multiBodyX[i] = constraint.m_appliedImpulse;
}
}
else
{
m_multiBodyX.setZero();
}
}
}
bool btMultiBodyMLCPConstraintSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
{
bool result = true;
if (m_A.rows() != 0)
{
// If using split impulse, we solve 2 separate (M)LCPs
if (infoGlobal.m_splitImpulse)
{
const btMatrixXu Acopy = m_A;
const btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
// TODO(JS): Do we really need these copies when solveMLCP takes them as const?
result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
if (result)
result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations);
}
else
{
result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
}
}
if (!result)
return false;
if (m_multiBodyA.rows() != 0)
{
result = m_solver->solveMLCP(m_multiBodyA, m_multiBodyB, m_multiBodyX, m_multiBodyLo, m_multiBodyHi, m_multiBodyLimitDependencies, infoGlobal.m_numIterations);
}
return result;
}
btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup(
btCollisionObject** bodies,
int numBodies,
btPersistentManifold** manifoldPtr,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& infoGlobal,
btIDebugDraw* debugDrawer)
{
// 1. Setup for rigid-bodies
btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(
bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
// 2. Setup for multi-bodies
// a. Collect all different kinds of constraint as pointers into one array, m_allConstraintPtrArray
// b. Set the index array for frictional contact constraints, m_limitDependencies
{
BT_PROFILE("gather constraint data");
int dindex = 0;
const int numRigidBodyConstraints = m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size();
const int numMultiBodyConstraints = m_multiBodyNonContactConstraints.size() + m_multiBodyNormalContactConstraints.size() + m_multiBodyFrictionContactConstraints.size();
m_allConstraintPtrArray.resize(0);
m_multiBodyAllConstraintPtrArray.resize(0);
// i. Setup for rigid bodies
m_limitDependencies.resize(numRigidBodyConstraints);
for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); ++i)
{
m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
m_limitDependencies[dindex++] = -1;
}
int firstContactConstraintOffset = dindex;
// The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
if (interleaveContactAndFriction)
{
for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
{
const int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2;
m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
m_limitDependencies[dindex++] = -1;
m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]);
int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact));
m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
if (numFrictionPerContact == 2)
{
m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]);
m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
}
}
}
else
{
for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
{
m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
m_limitDependencies[dindex++] = -1;
}
for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++)
{
m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset;
}
}
if (!m_allConstraintPtrArray.size())
{
m_A.resize(0, 0);
m_b.resize(0);
m_x.resize(0);
m_lo.resize(0);
m_hi.resize(0);
}
// ii. Setup for multibodies
dindex = 0;
m_multiBodyLimitDependencies.resize(numMultiBodyConstraints);
for (int i = 0; i < m_multiBodyNonContactConstraints.size(); ++i)
{
m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNonContactConstraints[i]);
m_multiBodyLimitDependencies[dindex++] = -1;
}
firstContactConstraintOffset = dindex;
// The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
if (interleaveContactAndFriction)
{
for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
{
const int numtiBodyNumFrictionPerContact = m_multiBodyNormalContactConstraints.size() == m_multiBodyFrictionContactConstraints.size() ? 1 : 2;
m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
m_multiBodyLimitDependencies[dindex++] = -1;
btMultiBodySolverConstraint& frictionContactConstraint1 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact];
m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint1);
const int findex = (frictionContactConstraint1.m_frictionIndex * (1 + numtiBodyNumFrictionPerContact)) + firstContactConstraintOffset;
m_multiBodyLimitDependencies[dindex++] = findex;
if (numtiBodyNumFrictionPerContact == 2)
{
btMultiBodySolverConstraint& frictionContactConstraint2 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact + 1];
m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint2);
m_multiBodyLimitDependencies[dindex++] = findex;
}
}
}
else
{
for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
{
m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
m_multiBodyLimitDependencies[dindex++] = -1;
}
for (int i = 0; i < m_multiBodyFrictionContactConstraints.size(); ++i)
{
m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyFrictionContactConstraints[i]);
m_multiBodyLimitDependencies[dindex++] = m_multiBodyFrictionContactConstraints[i].m_frictionIndex + firstContactConstraintOffset;
}
}
if (!m_multiBodyAllConstraintPtrArray.size())
{
m_multiBodyA.resize(0, 0);
m_multiBodyB.resize(0);
m_multiBodyX.resize(0);
m_multiBodyLo.resize(0);
m_multiBodyHi.resize(0);
}
}
// Construct MLCP terms
{
BT_PROFILE("createMLCPFast");
createMLCPFast(infoGlobal);
}
return btScalar(0);
}
btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
{
bool result = true;
{
BT_PROFILE("solveMLCP");
result = solveMLCP(infoGlobal);
}
// Fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations if the solution isn't valid.
if (!result)
{
m_fallback++;
return btMultiBodyConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
}
{
BT_PROFILE("process MLCP results");
for (int i = 0; i < m_allConstraintPtrArray.size(); ++i)
{
const btSolverConstraint& c = *m_allConstraintPtrArray[i];
const btScalar deltaImpulse = m_x[i] - c.m_appliedImpulse;
c.m_appliedImpulse = m_x[i];
int sbA = c.m_solverBodyIdA;
int sbB = c.m_solverBodyIdB;
btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
if (infoGlobal.m_splitImpulse)
{
const btScalar deltaPushImpulse = m_xSplit[i] - c.m_appliedPushImpulse;
solverBodyA.internalApplyPushImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaPushImpulse);
solverBodyB.internalApplyPushImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaPushImpulse);
c.m_appliedPushImpulse = m_xSplit[i];
}
}
for (int i = 0; i < m_multiBodyAllConstraintPtrArray.size(); ++i)
{
btMultiBodySolverConstraint& c = *m_multiBodyAllConstraintPtrArray[i];
const btScalar deltaImpulse = m_multiBodyX[i] - c.m_appliedImpulse;
c.m_appliedImpulse = m_multiBodyX[i];
btMultiBody* multiBodyA = c.m_multiBodyA;
if (multiBodyA)
{
const int ndofA = multiBodyA->getNumDofs() + 6;
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse);
#endif // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
}
else
{
const int sbA = c.m_solverBodyIdA;
btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
}
btMultiBody* multiBodyB = c.m_multiBodyB;
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumDofs() + 6;
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB);
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse);
#endif // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
}
else
{
const int sbB = c.m_solverBodyIdB;
btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
}
}
}
return btScalar(0);
}
btMultiBodyMLCPConstraintSolver::btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver)
: m_solver(solver), m_fallback(0)
{
// Do nothing
}
btMultiBodyMLCPConstraintSolver::~btMultiBodyMLCPConstraintSolver()
{
// Do nothing
}
void btMultiBodyMLCPConstraintSolver::setMLCPSolver(btMLCPSolverInterface* solver)
{
m_solver = solver;
}
int btMultiBodyMLCPConstraintSolver::getNumFallbacks() const
{
return m_fallback;
}
void btMultiBodyMLCPConstraintSolver::setNumFallbacks(int num)
{
m_fallback = num;
}
btConstraintSolverType btMultiBodyMLCPConstraintSolver::getSolverType() const
{
return BT_MLCP_SOLVER;
}

187
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h vendored Normal file
View File

@ -0,0 +1,187 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2018 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H
#define BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H
#include "LinearMath/btMatrixX.h"
#include "LinearMath/btThreads.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
class btMLCPSolverInterface;
class btMultiBody;
class btMultiBodyMLCPConstraintSolver : public btMultiBodyConstraintSolver
{
protected:
/// \name MLCP Formulation for Rigid Bodies
/// \{
/// A matrix in the MLCP formulation
btMatrixXu m_A;
/// b vector in the MLCP formulation.
btVectorXu m_b;
/// Constraint impulse, which is an output of MLCP solving.
btVectorXu m_x;
/// Lower bound of constraint impulse, \c m_x.
btVectorXu m_lo;
/// Upper bound of constraint impulse, \c m_x.
btVectorXu m_hi;
/// \}
/// \name Cache Variables for Split Impulse for Rigid Bodies
/// When using 'split impulse' we solve two separate (M)LCPs
/// \{
/// Split impulse Cache vector corresponding to \c m_b.
btVectorXu m_bSplit;
/// Split impulse cache vector corresponding to \c m_x.
btVectorXu m_xSplit;
/// \}
/// \name MLCP Formulation for Multibodies
/// \{
/// A matrix in the MLCP formulation
btMatrixXu m_multiBodyA;
/// b vector in the MLCP formulation.
btVectorXu m_multiBodyB;
/// Constraint impulse, which is an output of MLCP solving.
btVectorXu m_multiBodyX;
/// Lower bound of constraint impulse, \c m_x.
btVectorXu m_multiBodyLo;
/// Upper bound of constraint impulse, \c m_x.
btVectorXu m_multiBodyHi;
/// \}
/// Indices of normal contact constraint associated with frictional contact constraint for rigid bodies.
///
/// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate
/// normal contact impulse. For example, i-th element represents the index of a normal constraint that is
/// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint.
/// Otherwise, -1.
btAlignedObjectArray<int> m_limitDependencies;
/// Indices of normal contact constraint associated with frictional contact constraint for multibodies.
///
/// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate
/// normal contact impulse. For example, i-th element represents the index of a normal constraint that is
/// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint.
/// Otherwise, -1.
btAlignedObjectArray<int> m_multiBodyLimitDependencies;
/// Array of all the rigid body constraints
btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray;
/// Array of all the multibody constraints
btAlignedObjectArray<btMultiBodySolverConstraint*> m_multiBodyAllConstraintPtrArray;
/// MLCP solver
btMLCPSolverInterface* m_solver;
/// Count of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP solver fails.
int m_fallback;
/// \name MLCP Scratch Variables
/// The following scratch variables are not stateful -- contents are cleared prior to each use.
/// They are only cached here to avoid extra memory allocations and deallocations and to ensure
/// that multiple instances of the solver can be run in parallel.
///
/// \{
/// Cache variable for constraint Jacobian matrix.
btMatrixXu m_scratchJ3;
/// Cache variable for constraint Jacobian times inverse mass matrix.
btMatrixXu m_scratchJInvM3;
/// Cache variable for offsets.
btAlignedObjectArray<int> m_scratchOfs;
/// \}
/// Constructs MLCP terms, which are \c m_A, \c m_b, \c m_lo, and \c m_hi.
virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
/// Constructs MLCP terms for constraints of two rigid bodies
void createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal);
/// Constructs MLCP terms for constraints of two multi-bodies or one rigid body and one multibody
void createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal);
/// Solves MLCP and returns the success
virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
// Documentation inherited
btScalar solveGroupCacheFriendlySetup(
btCollisionObject** bodies,
int numBodies,
btPersistentManifold** manifoldPtr,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& infoGlobal,
btIDebugDraw* debugDrawer) BT_OVERRIDE;
// Documentation inherited
btScalar solveGroupCacheFriendlyIterations(
btCollisionObject** bodies,
int numBodies,
btPersistentManifold** manifoldPtr,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& infoGlobal,
btIDebugDraw* debugDrawer) BT_OVERRIDE;
public:
BT_DECLARE_ALIGNED_ALLOCATOR()
/// Constructor
///
/// \param[in] solver MLCP solver. Assumed it's not null.
/// \param[in] maxLCPSize Maximum size of LCP to solve using MLCP solver. If the MLCP size exceeds this number, sequaltial impulse method will be used.
explicit btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver);
/// Destructor
virtual ~btMultiBodyMLCPConstraintSolver();
/// Sets MLCP solver. Assumed it's not null.
void setMLCPSolver(btMLCPSolverInterface* solver);
/// Returns the number of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP
/// solver fails.
int getNumFallbacks() const;
/// Sets the number of fallbacks. This function may be used to reset the number to zero.
void setNumFallbacks(int num);
/// Returns the constraint solver type.
virtual btConstraintSolverType getSolverType() const;
};
#endif // BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H

28
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp vendored
View File

@ -64,13 +64,16 @@ int btMultiBodyPoint2Point::getIslandIdA() const
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
if (m_linkA < 0)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyA->getLink(m_linkA).m_collider)
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
}
}
return -1;
@ -82,16 +85,17 @@ int btMultiBodyPoint2Point::getIslandIdB() const
return m_rigidBodyB->getIslandTag();
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
if (m_linkB < 0)
{
col = m_bodyB->getLink(i).m_collider;
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyB->getLink(m_linkB).m_collider)
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
}
}
return -1;
}

29
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp vendored
View File

@ -68,13 +68,16 @@ int btMultiBodySliderConstraint::getIslandIdA() const
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
if (m_linkA < 0)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyA->getLink(m_linkA).m_collider)
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
}
}
return -1;
@ -86,20 +89,20 @@ int btMultiBodySliderConstraint::getIslandIdB() const
return m_rigidBodyB->getIslandTag();
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
if (m_linkB < 0)
{
col = m_bodyB->getLink(i).m_collider;
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyB->getLink(m_linkB).m_collider)
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
}
}
return -1;
}
void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
{
// Convert local points back to world

28
thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp vendored
View File

@ -121,11 +121,18 @@ void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedT
btQuaternion rotatingOrn(right,-wheel.m_rotation);
btMatrix3x3 rotatingMat(rotatingOrn);
btMatrix3x3 basis2(
right[0],fwd[0],up[0],
right[1],fwd[1],up[1],
right[2],fwd[2],up[2]
);
btMatrix3x3 basis2;
basis2[0][m_indexRightAxis] = -right[0];
basis2[1][m_indexRightAxis] = -right[1];
basis2[2][m_indexRightAxis] = -right[2];
basis2[0][m_indexUpAxis] = up[0];
basis2[1][m_indexUpAxis] = up[1];
basis2[2][m_indexUpAxis] = up[2];
basis2[0][m_indexForwardAxis] = fwd[0];
basis2[1][m_indexForwardAxis] = fwd[1];
basis2[2][m_indexForwardAxis] = fwd[2];
wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
wheel.m_worldTransform.setOrigin(
@ -493,8 +500,8 @@ struct btWheelContactPoint
};
btScalar calcRollingFriction(btWheelContactPoint& contactPoint);
btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround);
btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround)
{
btScalar j1=0.f;
@ -513,7 +520,7 @@ btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);
// calculate j that moves us to zero relative velocity
j1 = -vrel * contactPoint.m_jacDiagABInv;
j1 = -vrel * contactPoint.m_jacDiagABInv/btScalar(numWheelsOnGround);
btSetMin(j1, maxImpulse);
btSetMax(j1, -maxImpulse);
@ -567,7 +574,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep)
const btTransform& wheelTrans = getWheelTransformWS( i );
btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
m_axle[i] = btVector3(
m_axle[i] = -btVector3(
wheelBasis0[0][m_indexRightAxis],
wheelBasis0[1][m_indexRightAxis],
wheelBasis0[2][m_indexRightAxis]);
@ -615,7 +622,8 @@ void btRaycastVehicle::updateFriction(btScalar timeStep)
btScalar defaultRollingFrictionImpulse = 0.f;
btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
rollingFriction = calcRollingFriction(contactPt);
btAssert(numWheelsOnGround > 0);
rollingFriction = calcRollingFriction(contactPt, numWheelsOnGround);
}
}

8
thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp vendored
View File

@ -7,19 +7,19 @@
#if !defined(BT_ID_WO_BULLET) && !defined(BT_USE_INVERSE_DYNAMICS_WITH_BULLET2)
#include "Bullet3Common/b3Logging.h"
#define error_message(...) b3Error(__VA_ARGS__)
#define warning_message(...) b3Warning(__VA_ARGS__)
#define bt_id_error_message(...) b3Error(__VA_ARGS__)
#define bt_id_warning_message(...) b3Warning(__VA_ARGS__)
#define id_printf(...) b3Printf(__VA_ARGS__)
#else // BT_ID_WO_BULLET
#include <cstdio>
/// print error message with file/line information
#define error_message(...) \
#define bt_id_error_message(...) \
do { \
fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
fprintf(stderr, __VA_ARGS__); \
} while (0)
/// print warning message with file/line information
#define warning_message(...) \
#define bt_id_warning_message(...) \
do { \
fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
fprintf(stderr, __VA_ARGS__); \

42
thirdparty/bullet/BulletInverseDynamics/IDMath.cpp vendored
View File

@ -81,7 +81,7 @@ idScalar maxAbsMat3x(const mat3x &m) {
void mul(const mat33 &a, const mat3x &b, mat3x *result) {
if (b.cols() != result->cols()) {
error_message("size missmatch. b.cols()= %d, result->cols()= %d\n",
bt_id_error_message("size missmatch. b.cols()= %d, result->cols()= %d\n",
static_cast<int>(b.cols()), static_cast<int>(result->cols()));
abort();
}
@ -97,7 +97,7 @@ void mul(const mat33 &a, const mat3x &b, mat3x *result) {
}
void add(const mat3x &a, const mat3x &b, mat3x *result) {
if (a.cols() != b.cols()) {
error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
static_cast<int>(a.cols()), static_cast<int>(b.cols()));
abort();
}
@ -109,7 +109,7 @@ void add(const mat3x &a, const mat3x &b, mat3x *result) {
}
void sub(const mat3x &a, const mat3x &b, mat3x *result) {
if (a.cols() != b.cols()) {
error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
static_cast<int>(a.cols()), static_cast<int>(b.cols()));
abort();
}
@ -305,10 +305,10 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
// the determinant of the inertia tensor about the joint axis is almost
// zero and can have a very small negative value.
if (!isPositiveSemiDefiniteFuzzy(I)) {
error_message("invalid inertia matrix for body %d, not positive definite "
bt_id_error_message("invalid inertia matrix for body %d, not positive definite "
"(fixed joint)\n",
index);
error_message("matrix is:\n"
bt_id_error_message("matrix is:\n"
"[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e]\n",
@ -321,8 +321,8 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
// check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k)
if (!has_fixed_joint) {
if (I(0, 0) + I(1, 1) < I(2, 2)) {
error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
error_message("matrix is:\n"
bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
bt_id_error_message("matrix is:\n"
"[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e]\n",
@ -331,8 +331,8 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
return false;
}
if (I(0, 0) + I(1, 1) < I(2, 2)) {
error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
error_message("matrix is:\n"
bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
bt_id_error_message("matrix is:\n"
"[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e]\n",
@ -341,8 +341,8 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
return false;
}
if (I(1, 1) + I(2, 2) < I(0, 0)) {
error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index);
error_message("matrix is:\n"
bt_id_error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index);
bt_id_error_message("matrix is:\n"
"[%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e;\n"
"%.20e %.20e %.20e]\n",
@ -354,25 +354,25 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
// check positive/zero diagonal elements
for (int i = 0; i < 3; i++) {
if (I(i, i) < 0) { // accept zero
error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
bt_id_error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
return false;
}
}
// check symmetry
if (BT_ID_FABS(I(1, 0) - I(0, 1)) > kIsZero) {
error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= "
bt_id_error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= "
"%e\n",
index, I(1, 0) - I(0, 1));
return false;
}
if (BT_ID_FABS(I(2, 0) - I(0, 2)) > kIsZero) {
error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= "
bt_id_error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= "
"%e\n",
index, I(2, 0) - I(0, 2));
return false;
}
if (BT_ID_FABS(I(1, 2) - I(2, 1)) > kIsZero) {
error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index,
bt_id_error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index,
I(1, 2) - I(2, 1));
return false;
}
@ -381,7 +381,7 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
bool isValidTransformMatrix(const mat33 &m) {
#define print_mat(x) \
error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \
bt_id_error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \
x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2))
// check for unit length column vectors
@ -389,7 +389,7 @@ bool isValidTransformMatrix(const mat33 &m) {
const idScalar length_minus_1 =
BT_ID_FABS(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0);
if (length_minus_1 > kAxisLengthEpsilon) {
error_message("Not a valid rotation matrix (column %d not unit length)\n"
bt_id_error_message("Not a valid rotation matrix (column %d not unit length)\n"
"column = [%.18e %.18e %.18e]\n"
"length-1.0= %.18e\n",
i, m(0, i), m(1, i), m(2, i), length_minus_1);
@ -399,23 +399,23 @@ bool isValidTransformMatrix(const mat33 &m) {
}
// check for orthogonal column vectors
if (BT_ID_FABS(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) {
error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
bt_id_error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
print_mat(m);
return false;
}
if (BT_ID_FABS(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) {
error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
print_mat(m);
return false;
}
if (BT_ID_FABS(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) {
error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
print_mat(m);
return false;
}
// check determinant (rotation not reflection)
if (determinant(m) <= 0) {
error_message("Not a valid rotation matrix (determinant <=0)\n");
bt_id_error_message("Not a valid rotation matrix (determinant <=0)\n");
print_mat(m);
return false;
}

60
thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp vendored
View File

@ -83,11 +83,11 @@ int MultiBodyTree::numDoFs() const { return m_impl->m_num_dofs; }
int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const vecx &dot_u,
vecx *joint_forces) {
if (false == m_is_finalized) {
error_message("system has not been initialized\n");
bt_id_error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) {
error_message("error in inverse dynamics calculation\n");
bt_id_error_message("error in inverse dynamics calculation\n");
return -1;
}
return 0;
@ -97,13 +97,13 @@ int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinemati
const bool initialize_matrix,
const bool set_lower_triangular_matrix, matxx *mass_matrix) {
if (false == m_is_finalized) {
error_message("system has not been initialized\n");
bt_id_error_message("system has not been initialized\n");
return -1;
}
if (-1 ==
m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix,
set_lower_triangular_matrix, mass_matrix)) {
error_message("error in mass matrix calculation\n");
bt_id_error_message("error in mass matrix calculation\n");
return -1;
}
return 0;
@ -121,12 +121,12 @@ int MultiBodyTree::calculateKinematics(const vecx& q, const vecx& u, const vecx&
setZero(m_impl->m_world_gravity);
if (false == m_is_finalized) {
error_message("system has not been initialized\n");
bt_id_error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateKinematics(q, u, dot_u,
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION)) {
error_message("error in kinematics calculation\n");
bt_id_error_message("error in kinematics calculation\n");
return -1;
}
@ -137,12 +137,12 @@ int MultiBodyTree::calculateKinematics(const vecx& q, const vecx& u, const vecx&
int MultiBodyTree::calculatePositionKinematics(const vecx& q) {
if (false == m_is_finalized) {
error_message("system has not been initialized\n");
bt_id_error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateKinematics(q, q, q,
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
error_message("error in kinematics calculation\n");
bt_id_error_message("error in kinematics calculation\n");
return -1;
}
return 0;
@ -150,12 +150,12 @@ int MultiBodyTree::calculatePositionKinematics(const vecx& q) {
int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u) {
if (false == m_is_finalized) {
error_message("system has not been initialized\n");
bt_id_error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateKinematics(q, u, u,
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
error_message("error in kinematics calculation\n");
bt_id_error_message("error in kinematics calculation\n");
return -1;
}
return 0;
@ -165,12 +165,12 @@ int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx& q, const v
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
int MultiBodyTree::calculateJacobians(const vecx& q, const vecx& u) {
if (false == m_is_finalized) {
error_message("system has not been initialized\n");
bt_id_error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateJacobians(q, u,
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
error_message("error in jacobian calculation\n");
bt_id_error_message("error in jacobian calculation\n");
return -1;
}
return 0;
@ -178,12 +178,12 @@ int MultiBodyTree::calculateJacobians(const vecx& q, const vecx& u) {
int MultiBodyTree::calculateJacobians(const vecx& q){
if (false == m_is_finalized) {
error_message("system has not been initialized\n");
bt_id_error_message("system has not been initialized\n");
return -1;
}
if (-1 == m_impl->calculateJacobians(q, q,
MultiBodyTree::MultiBodyImpl::POSITION_ONLY)) {
error_message("error in jacobian calculation\n");
bt_id_error_message("error in jacobian calculation\n");
return -1;
}
return 0;
@ -214,7 +214,7 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
const vec3 &body_r_body_com, const mat33 &body_I_body,
const int user_int, void *user_ptr) {
if (body_index < 0) {
error_message("body index must be positive (got %d)\n", body_index);
bt_id_error_message("body index must be positive (got %d)\n", body_index);
return -1;
}
vec3 body_axis_of_motion(body_axis_of_motion_);
@ -223,14 +223,14 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
case PRISMATIC:
// check if axis is unit vector
if (!isUnitVector(body_axis_of_motion)) {
warning_message(
bt_id_warning_message(
"axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n",
body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2));
idScalar length = BT_ID_SQRT(BT_ID_POW(body_axis_of_motion(0), 2) +
BT_ID_POW(body_axis_of_motion(1), 2) +
BT_ID_POW(body_axis_of_motion(2), 2));
if (length < BT_ID_SQRT(std::numeric_limits<idScalar>::min())) {
error_message("axis of motion vector too short (%e)\n", length);
bt_id_error_message("axis of motion vector too short (%e)\n", length);
return -1;
}
body_axis_of_motion = (1.0 / length) * body_axis_of_motion;
@ -241,14 +241,14 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
case FLOATING:
break;
default:
error_message("unknown joint type %d\n", joint_type);
bt_id_error_message("unknown joint type %d\n", joint_type);
return -1;
}
// sanity check for mass properties. Zero mass is OK.
if (mass < 0) {
m_mass_parameters_are_valid = false;
error_message("Body %d has invalid mass %e\n", body_index, mass);
bt_id_error_message("Body %d has invalid mass %e\n", body_index, mass);
if (!m_accept_invalid_mass_parameters) {
return -1;
}
@ -296,7 +296,7 @@ int MultiBodyTree::finalize() {
const int &num_dofs = m_init_cache->numDoFs();
if(num_dofs<=0) {
error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
bt_id_error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
//return -1;
}
@ -331,6 +331,22 @@ int MultiBodyTree::finalize() {
rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref;
rigid_body.m_joint_type = joint.m_type;
int user_int;
if (-1 == m_init_cache->getUserInt(index, &user_int)) {
return -1;
}
if (-1 == m_impl->setUserInt(index, user_int)) {
return -1;
}
void* user_ptr;
if (-1 == m_init_cache->getUserPtr(index, &user_ptr)) {
return -1;
}
if (-1 == m_impl->setUserPtr(index, user_ptr)) {
return -1;
}
// Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized
// matrices.
switch (rigid_body.m_joint_type) {
@ -370,14 +386,14 @@ int MultiBodyTree::finalize() {
rigid_body.m_Jac_JT(2) = 0.0;
break;
default:
error_message("unsupported joint type %d\n", rigid_body.m_joint_type);
bt_id_error_message("unsupported joint type %d\n", rigid_body.m_joint_type);
return -1;
}
}
// 4 assign degree of freedom indices & build per-joint-type index arrays
if (-1 == m_impl->generateIndexSets()) {
error_message("generating index sets\n");
bt_id_error_message("generating index sets\n");
return -1;
}

12
thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp vendored
View File

@ -49,9 +49,9 @@ inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
class vecx : public btVectorX<idScalar> {
public:
vecx(int size) : btVectorX(size) {}
vecx(int size) : btVectorX<idScalar>(size) {}
const vecx& operator=(const btVectorX<idScalar>& rhs) {
*static_cast<btVectorX*>(this) = rhs;
*static_cast<btVectorX<idScalar>*>(this) = rhs;
return *this;
}
@ -78,7 +78,7 @@ inline vecx operator+(const vecx& a, const vecx& b) {
vecx result(a.size());
// TODO: error handling for a.size() != b.size()??
if (a.size() != b.size()) {
error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
abort();
}
for (int i = 0; i < a.size(); i++) {
@ -92,7 +92,7 @@ inline vecx operator-(const vecx& a, const vecx& b) {
vecx result(a.size());
// TODO: error handling for a.size() != b.size()??
if (a.size() != b.size()) {
error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
abort();
}
for (int i = 0; i < a.size(); i++) {
@ -121,7 +121,7 @@ public:
}
void operator=(const mat3x& rhs) {
if (m_cols != rhs.m_cols) {
error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
abort();
}
for(int i=0;i<rows();i++) {
@ -139,7 +139,7 @@ public:
inline vec3 operator*(const mat3x& a, const vecx& b) {
vec3 result;
if (a.cols() != b.size()) {
error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
abort();
}
result(0)=0.0;

10
thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp vendored
View File

@ -123,7 +123,7 @@ public:
};
void operator=(const mat3x& rhs) {
if (m_cols != rhs.m_cols) {
error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
abort();
}
for(int i=0;i<3*m_cols;i++) {
@ -336,7 +336,7 @@ inline vec3 operator/(const vec3& a, const idScalar& s) {
inline const vecx& vecx::operator=(const vecx& rhs) {
if (size() != rhs.size()) {
error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size());
bt_id_error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size());
abort();
}
if (&rhs != this) {
@ -356,7 +356,7 @@ inline vecx operator+(const vecx& a, const vecx& b) {
vecx result(a.size());
// TODO: error handling for a.size() != b.size()??
if (a.size() != b.size()) {
error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
abort();
}
for (int i = 0; i < a.size(); i++) {
@ -369,7 +369,7 @@ inline vecx operator-(const vecx& a, const vecx& b) {
vecx result(a.size());
// TODO: error handling for a.size() != b.size()??
if (a.size() != b.size()) {
error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
abort();
}
for (int i = 0; i < a.size(); i++) {
@ -389,7 +389,7 @@ inline vecx operator/(const vecx& a, const idScalar& s) {
inline vec3 operator*(const mat3x& a, const vecx& b) {
vec3 result;
if (a.cols() != b.size()) {
error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
abort();
}
result(0)=0.0;

30
thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp vendored
View File

@ -80,7 +80,7 @@ int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const {
case FLOATING:
return 6;
}
error_message("unknown joint type %d\n", type);
bt_id_error_message("unknown joint type %d\n", type);
return 0;
}
@ -136,13 +136,13 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() {
q_index += 6;
break;
default:
error_message("unsupported joint type %d\n", body.m_joint_type);
bt_id_error_message("unsupported joint type %d\n", body.m_joint_type);
return -1;
}
}
// sanity check
if (q_index != m_num_dofs) {
error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs);
bt_id_error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs);
return -1;
}
@ -155,10 +155,10 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() {
} else {
if (-1 == parent) {
// multiple bodies are directly linked to the environment, ie, not a single root
error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child);
bt_id_error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child);
} else {
// should never happen
error_message(
bt_id_error_message(
"building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n",
child, parent, static_cast<int>(m_parent_index.size()));
}
@ -234,7 +234,7 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
const vecx &dot_u, vecx *joint_forces) {
if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ||
joint_forces->size() != m_num_dofs) {
error_message("wrong vector dimension. system has %d DOFs,\n"
bt_id_error_message("wrong vector dimension. system has %d DOFs,\n"
"but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n",
m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size()));
@ -242,7 +242,7 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
}
// 1. relative kinematics
if(-1 == calculateKinematics(q,u,dot_u, POSITION_VELOCITY_ACCELERATION)) {
error_message("error in calculateKinematics\n");
bt_id_error_message("error in calculateKinematics\n");
return -1;
}
// 2. update contributions to equations of motion for every body.
@ -322,14 +322,14 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx& dot_u,
const KinUpdateType type) {
if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ) {
error_message("wrong vector dimension. system has %d DOFs,\n"
bt_id_error_message("wrong vector dimension. system has %d DOFs,\n"
"but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n",
m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
static_cast<int>(dot_u.size()));
return -1;
}
if(type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION) {
error_message("invalid type %d\n", type);
bt_id_error_message("invalid type %d\n", type);
return -1;
}
@ -516,13 +516,13 @@ void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody&body)
int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type) {
if (q.size() != m_num_dofs || u.size() != m_num_dofs) {
error_message("wrong vector dimension. system has %d DOFs,\n"
bt_id_error_message("wrong vector dimension. system has %d DOFs,\n"
"but dim(q)= %d, dim(u)= %d\n",
m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()));
return -1;
}
if(type != POSITION_ONLY && type != POSITION_VELOCITY) {
error_message("invalid type %d\n", type);
bt_id_error_message("invalid type %d\n", type);
return -1;
}
@ -606,7 +606,7 @@ static inline int jointNumDoFs(const JointType &type) {
return 6;
}
// this should never happen
error_message("invalid joint type\n");
bt_id_error_message("invalid joint type\n");
// TODO add configurable abort/crash function
abort();
return 0;
@ -626,7 +626,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs ||
mass_matrix->cols() != m_num_dofs) {
error_message("Dimension error. System has %d DOFs,\n"
bt_id_error_message("Dimension error. System has %d DOFs,\n"
"but dim(q)= %d, dim(mass_matrix)= %d x %d\n",
m_num_dofs, static_cast<int>(q.size()), static_cast<int>(mass_matrix->rows()),
static_cast<int>(mass_matrix->cols()));
@ -734,7 +734,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
// 1. for multi-dof joints, rest of the dofs of this body
for (int row = col - 1; row >= q_index_min; row--) {
if (FLOATING != body.m_joint_type) {
error_message("??\n");
bt_id_error_message("??\n");
return -1;
}
setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
@ -788,7 +788,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
#define CHECK_IF_BODY_INDEX_IS_VALID(index) \
do { \
if (index < 0 || index >= m_num_bodies) { \
error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \
bt_id_error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \
return -1; \
} \
} while (0)

12
thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp vendored
View File

@ -29,13 +29,13 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind
m_num_dofs += 6;
break;
default:
error_message("unknown joint type %d\n", joint_type);
bt_id_error_message("unknown joint type %d\n", joint_type);
return -1;
}
if(-1 == parent_index) {
if(m_root_index>=0) {
error_message("trying to add body %d as root, but already added %d as root body\n",
bt_id_error_message("trying to add body %d as root, but already added %d as root body\n",
body_index, m_root_index);
return -1;
}
@ -63,7 +63,7 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind
}
int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const {
if (index < 0 || index > static_cast<int>(m_inertias.size())) {
error_message("index out of range\n");
bt_id_error_message("index out of range\n");
return -1;
}
@ -73,7 +73,7 @@ int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inert
int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const {
if (index < 0 || index > static_cast<int>(m_user_int.size())) {
error_message("index out of range\n");
bt_id_error_message("index out of range\n");
return -1;
}
*user_int = m_user_int[index];
@ -82,7 +82,7 @@ int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const {
int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const {
if (index < 0 || index > static_cast<int>(m_user_ptr.size())) {
error_message("index out of range\n");
bt_id_error_message("index out of range\n");
return -1;
}
*user_ptr = m_user_ptr[index];
@ -91,7 +91,7 @@ int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const
int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const {
if (index < 0 || index > static_cast<int>(m_joints.size())) {
error_message("index out of range\n");
bt_id_error_message("index out of range\n");
return -1;
}
*joint = m_joints[index];

6
thirdparty/bullet/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp vendored
View File

@ -157,7 +157,7 @@ void btSoftMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* coll
void btSoftMultiBodyDynamicsWorld::debugDrawWorld()
{
btDiscreteDynamicsWorld::debugDrawWorld();
btMultiBodyDynamicsWorld::debugDrawWorld();
if (getDebugDrawer())
{
@ -357,10 +357,14 @@ void btSoftMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
serializeSoftBodies(serializer);
serializeMultiBodies(serializer);
serializeRigidBodies(serializer);
serializeCollisionObjects(serializer);
serializeContactManifolds(serializer);
serializer->finishSerialization();
}

Some files were not shown because too many files have changed in this diff Show More