Update bullet to Master 12409f1118a7c7a266f9071350c70789dfe73bb9
This commit is contained in:
Andrea Catania
parent
5307043751
commit
6142448417
|
|
@ -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
2
thirdparty/README.md
vendored
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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__
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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,13 +129,12 @@ void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroad
|
|||
|
||||
btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
|
||||
{
|
||||
gFindPairs++;
|
||||
if(proxy0->m_uniqueId>proxy1->m_uniqueId)
|
||||
if(proxy0->m_uniqueId>proxy1->m_uniqueId)
|
||||
btSwap(proxy0,proxy1);
|
||||
int proxyId1 = proxy0->getUid();
|
||||
int proxyId2 = proxy1->getUid();
|
||||
|
||||
/*if (proxyId1 > proxyId2)
|
||||
/*if (proxyId1 > proxyId2)
|
||||
btSwap(proxyId1, proxyId2);*/
|
||||
|
||||
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
|
||||
|
|
@ -271,13 +265,12 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx
|
|||
|
||||
void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
|
||||
{
|
||||
gRemovePairs++;
|
||||
if(proxy0->m_uniqueId>proxy1->m_uniqueId)
|
||||
if(proxy0->m_uniqueId>proxy1->m_uniqueId)
|
||||
btSwap(proxy0,proxy1);
|
||||
int proxyId1 = proxy0->getUid();
|
||||
int proxyId2 = proxy1->getUid();
|
||||
|
||||
/*if (proxyId1 > proxyId2)
|
||||
/*if (proxyId1 > proxyId2)
|
||||
btSwap(proxyId1, proxyId2);*/
|
||||
|
||||
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
|
||||
|
|
@ -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);
|
||||
|
|
@ -468,11 +522,8 @@ 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--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
|
|
|
|||
|
|
@ -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,8 +313,7 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
|
|||
pair.m_pProxy0 = 0;
|
||||
pair.m_pProxy1 = 0;
|
||||
m_invalidPair++;
|
||||
gOverlappingPairs--;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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,12 +1654,36 @@ 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)
|
||||
{
|
||||
|
||||
serializer->startSerialization();
|
||||
|
||||
serializeCollisionObjects(serializer);
|
||||
|
||||
serializeContactManifolds(serializer);
|
||||
|
||||
serializer->finishSerialization();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -107,6 +107,9 @@ protected:
|
|||
|
||||
void serializeCollisionObjects(btSerializer* serializer);
|
||||
|
||||
void serializeContactManifolds(btSerializer* serializer);
|
||||
|
||||
|
||||
public:
|
||||
|
||||
//this constructor doesn't own the dispatcher and paircache/broadphase
|
||||
|
|
|
|||
|
|
@ -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,9 +259,9 @@ 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
|
||||
btCompoundLeafCallback callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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),
|
||||
|
|
@ -152,7 +153,7 @@ partId, int triangleIndex)
|
|||
{
|
||||
m_resultOut->setBody1Wrap(tmpWrap);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
colAlgo->~btCollisionAlgorithm();
|
||||
|
|
@ -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,14 +178,14 @@ 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;
|
||||
|
||||
btVector3 extra(extraMargin,extraMargin,extraMargin);
|
||||
convexShape->getAabb(convexInTriangleSpace, m_aabbMin, m_aabbMax);
|
||||
btScalar extraMargin = collisionMarginTriangle + resultOut->m_closestPointDistanceThreshold;
|
||||
|
||||
btVector3 extra(extraMargin, extraMargin, extraMargin);
|
||||
|
||||
m_aabbMax += extra;
|
||||
m_aabbMin -= extra;
|
||||
|
||||
|
||||
}
|
||||
|
||||
void btConvexConcaveCollisionAlgorithm::clearCache()
|
||||
|
|
@ -193,35 +194,99 @@ 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");
|
||||
|
||||
|
||||
const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
|
||||
const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
|
||||
|
||||
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())
|
||||
{
|
||||
|
||||
btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape();
|
||||
btAlignedObjectArray<btVector3> queryVertices;
|
||||
|
||||
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;
|
||||
|
||||
if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
|
||||
{
|
||||
queryVertices.push_back(btVector3(0, 0, 0));
|
||||
btSphereShape* sphere = (btSphereShape*)convex;
|
||||
maxDist = sphere->getRadius() + SIMD_EPSILON;
|
||||
|
||||
}
|
||||
if (queryVertices.size())
|
||||
{
|
||||
resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
|
||||
//m_btConvexTriangleCallback.m_manifoldPtr->clearManifold();
|
||||
|
||||
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);
|
||||
resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
|
||||
m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
|
||||
|
||||
m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
|
||||
m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
|
||||
|
||||
concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
|
||||
concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
|
||||
|
||||
resultOut->refreshContactPoints();
|
||||
resultOut->refreshContactPoints();
|
||||
|
||||
m_btConvexTriangleCallback.clearWrapperData();
|
||||
m_btConvexTriangleCallback.clearWrapperData();
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -625,6 +746,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
|
|||
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
||||
btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
|
||||
{
|
||||
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();
|
||||
if (!triangleInfoMapPtr)
|
||||
return;
|
||||
|
||||
|
|
|
|||
|
|
@ -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,7 +332,12 @@ 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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
532
thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.cpp
vendored
Normal 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
134
thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.h
vendored
Normal 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
|
||||
|
|
@ -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();
|
||||
|
|
@ -311,7 +386,9 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
|
|||
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif //BT_RECONSTRUCT_FACES
|
||||
|
||||
m_polyhedron->initialize();
|
||||
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -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
99
thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp
vendored
Normal 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
30
thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.h
vendored
Normal 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
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
|
|
|
|||
|
|
@ -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
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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.));
|
||||
|
|
@ -137,8 +132,7 @@ 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 = lambda + dLambda;
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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 guessVector(transformB.getOrigin()-transformA.getOrigin());
|
||||
btGjkEpaSolver2::sResults results;
|
||||
|
||||
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),
|
||||
};
|
||||
|
||||
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
|
||||
int numVectors = sizeof(guessVectors) / sizeof(btVector3);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load diff
|
|
@ -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];
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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
1128
thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp
vendored
Normal file
File diff suppressed because it is too large
Load diff
66
thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.h
vendored
Normal file
66
thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.h
vendored
Normal 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
|
||||
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load diff
|
|
@ -4,8 +4,8 @@ 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,
|
||||
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.
|
||||
|
|
@ -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
|
||||
|
|
@ -64,44 +64,48 @@ protected:
|
|||
|
||||
void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
|
||||
btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
|
||||
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
|
||||
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
|
||||
const btContactSolverInfo& infoGlobal,
|
||||
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
|
||||
|
||||
void setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
|
||||
btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,
|
||||
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
|
||||
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
|
||||
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
|
||||
|
||||
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
|
||||
btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar torsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
|
||||
|
||||
|
||||
void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
|
||||
|
||||
void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
|
||||
const btContactSolverInfo& infoGlobal,btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
|
||||
|
||||
static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
|
||||
|
||||
void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB,
|
||||
void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB,
|
||||
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
|
||||
unsigned long m_btSeed2;
|
||||
|
||||
|
||||
|
||||
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold);
|
||||
|
||||
virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
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);
|
||||
|
|
@ -133,15 +139,15 @@ protected:
|
|||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
|
||||
btSequentialImpulseConstraintSolver();
|
||||
virtual ~btSequentialImpulseConstraintSolver();
|
||||
|
||||
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
|
||||
|
||||
|
||||
///clear internal cached data and reset random seed
|
||||
virtual void reset();
|
||||
|
||||
|
||||
unsigned long btRand2();
|
||||
|
||||
int btRandInt2 (int n);
|
||||
|
|
@ -155,7 +161,7 @@ public:
|
|||
return m_btSeed2;
|
||||
}
|
||||
|
||||
|
||||
|
||||
virtual btConstraintSolverType getSolverType() const
|
||||
{
|
||||
return BT_SEQUENTIAL_IMPULSE_SOLVER;
|
||||
|
|
|
|||
1621
thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp
vendored
Normal file
1621
thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp
vendored
Normal file
File diff suppressed because it is too large
Load diff
154
thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h
vendored
Normal file
154
thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h
vendored
Normal 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
0
thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
vendored
Normal file → Executable file
0
thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h
vendored
Normal file → Executable file
0
thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h
vendored
Normal file → Executable 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();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
@ -777,7 +781,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
|
|||
// hhat is NOT stored for the base (but ahat is)
|
||||
btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
|
||||
btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
|
||||
v_ptr += num_links * 2 + 2;
|
||||
v_ptr += num_links * 2 + 2;
|
||||
//
|
||||
// Y_i, invD_i
|
||||
btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
|
||||
|
|
@ -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)
|
||||
{
|
||||
btScalar *D_row = &D[dof * m_links[i].m_dofCount];
|
||||
}
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -702,15 +702,18 @@ 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;
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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,36 +60,101 @@ 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);
|
||||
if(constraint.m_multiBodyB)
|
||||
constraint.m_multiBodyB->setPosUpdated(false);
|
||||
}
|
||||
|
||||
|
||||
//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,82 +195,267 @@ 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;
|
||||
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)
|
||||
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)
|
||||
{
|
||||
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;
|
||||
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)
|
||||
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)
|
||||
{
|
||||
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)
|
||||
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
|
||||
{
|
||||
c.m_appliedImpulse = sum;
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -277,7 +277,11 @@ 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)
|
||||
{
|
||||
if (islandId<0)
|
||||
|
|
@ -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();
|
||||
|
||||
|
|
@ -824,21 +846,24 @@ 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);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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
966
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp
vendored
Normal 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
187
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h
vendored
Normal 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
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -121,12 +121,19 @@ 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(
|
||||
wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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__); \
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
|
|
|
|||
|
|
@ -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
Loading…
Reference in a new issue