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

perform GrahamScanConvexHull2D around an arbitrary oriented 2D plane in 3D, to fix some convex hull face merging problems

Browse Source 
add compound shape support to BulletXmlWorldImporter and fix some compile issue under Debian (hopefully)
object picking change in the demos: create a ball-socket picking constraint when holding shift while mouse dragging, otherwise a fixed (6dof) constraint
add assert in constraint solver, when both objects have their inertia tensor rows set to zero
btPolyhedralContactClipping: add edge-edge contact point in  findSeparatingAxis (similar to the default GJK case)
This commit is contained in:
erwin.coumans
2012-09-28 07:14:48 +00:00
parent 837e5cb5e0
commit 60bf599246
24 changed files with 385 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
Demos/BasicDemo/BasicDemo.cpp
View File

@@ -34,6 +34,9 @@ subject to the following restrictions:
#include "btBulletDynamicsCommon.h"
#include <stdio.h> //printf debugging
#include "GLDebugDrawer.h"
static GLDebugDrawer gDebugDraw;
void BasicDemo::clientMoveAndDisplay()
@@ -100,11 +103,13 @@ void BasicDemo::initPhysics()
m_solver = sol;
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
m_dynamicsWorld->setDebugDrawer(&gDebugDraw);
m_dynamicsWorld->setGravity(btVector3(0,-10,0));
///create a few basic rigid bodies
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
btBoxShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
//groundShape->initializePolyhedralFeatures();
// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
m_collisionShapes.push_back(groundShape);
@@ -138,7 +143,7 @@ void BasicDemo::initPhysics()
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
btCollisionShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
btBoxShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
m_collisionShapes.push_back(colShape);

2
Demos/BasicDemo/main.cpp
View File

@@ -19,8 +19,6 @@ subject to the following restrictions:
#include "LinearMath/btHashMap.h"
int main(int argc,char** argv)
{

24
Demos/BulletXmlImportDemo/BulletXmlImportDemo.cpp
View File

@@ -28,15 +28,24 @@ subject to the following restrictions:
#include <stdio.h> //printf debugging
#include "GLDebugDrawer.h"
GLDebugDrawer gDebugDrawer;
void BulletXmlImportDemo::initPhysics()
{
setTexturing(true);
setTexturing(true);
setShadows(true);
setCameraDistance(btScalar(30.));
setupEmptyDynamicsWorld();
m_dynamicsWorld->setDebugDrawer(&gDebugDrawer);
btBulletXmlWorldImporter* importer = new btBulletXmlWorldImporter(m_dynamicsWorld);
importer->loadFile("bullet_basic.xml");
// importer->loadFile("bulletser.xml");
@@ -54,6 +63,8 @@ void BulletXmlImportDemo::clientMoveAndDisplay()
///step the simulation
if (m_dynamicsWorld)
{
m_dynamicsWorld->stepSimulation(ms / 1000000.f);
m_dynamicsWorld->debugDrawWorld();
}
@@ -90,6 +101,7 @@ void BulletXmlImportDemo::setupEmptyDynamicsWorld()
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
btGImpactCollisionAlgorithm::registerAlgorithm(m_dispatcher);
m_broadphase = new btDbvtBroadphase();
@@ -101,7 +113,7 @@ void BulletXmlImportDemo::setupEmptyDynamicsWorld()
//btGImpactCollisionAlgorithm::registerAlgorithm((btCollisionDispatcher*)m_dynamicsWorld->getDispatcher());
m_dynamicsWorld->setGravity(btVector3(0,-10,0));
}
@@ -115,7 +127,11 @@ BulletXmlImportDemo::~BulletXmlImportDemo()
exitPhysics();
}
void BulletXmlImportDemo::clientResetScene()
{
exitPhysics();
initPhysics();
}
void BulletXmlImportDemo::exitPhysics()

4
Demos/BulletXmlImportDemo/BulletXmlImportDemo.h
View File

@@ -55,9 +55,13 @@ class BulletXmlImportDemo : public PlatformDemoApplication
BulletXmlImportDemo()
{
//m_idle=true;
setCameraDistance(btScalar(30.));
}
virtual ~BulletXmlImportDemo();
virtual void clientResetScene();
void initPhysics();
void setupEmptyDynamicsWorld();

3
Demos/BulletXmlImportDemo/CMakeLists.txt
View File

@@ -11,9 +11,6 @@ SET(GLUT_ROOT ${BULLET_PHYSICS_SOURCE_DIR}/Glut)
# You shouldn't have to modify anything below this line
########################################################
IF(BUILD_AMD_OPENCL_DEMOS)
SUBDIRS(AMD)
ENDIF()
INCLUDE_DIRECTORIES(
${BULLET_PHYSICS_SOURCE_DIR}/src

8
Demos/BulletXmlImportDemo/main.cpp
View File

@@ -15,12 +15,11 @@ subject to the following restrictions:
#include "BulletXmlImportDemo.h"
#include "GlutStuff.h"
#include "GLDebugDrawer.h"
#include "btBulletDynamicsCommon.h"
#include "LinearMath/btHashMap.h"
#ifdef USE_AMD_OPENCL
@@ -84,7 +83,7 @@ bool initCL( void* glCtx, void* glDC )
int main(int argc,char** argv)
{
GLDebugDrawer gDebugDrawer;
#ifdef USE_AMD_OPENCL
bool initialized = initCL(0,0);
@@ -94,8 +93,7 @@ int main(int argc,char** argv)
BulletXmlImportDemo serializeDemo;
serializeDemo.initPhysics();
serializeDemo.getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
#ifdef CHECK_MEMORY_LEAKS
serializeDemo.exitPhysics();

1
Demos/FractureDemo/CMakeLists.txt
View File

@@ -62,6 +62,7 @@ ENDIF()
POST_BUILD
# COMMAND copy /Y ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR}
COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR}
COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR}/Debug
)
ENDIF(CMAKE_CL_64)
ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)

5
Demos/FractureDemo/btFractureBody.cpp
View File

@@ -22,12 +22,13 @@ void btFractureBody::recomputeConnectivity(btCollisionWorld* world)
struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback
{
bool m_connected;
MyContactResultCallback() :m_connected(false)
btScalar m_margin;
MyContactResultCallback() :m_connected(false),m_margin(0.05)
{
}
virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
{
if (cp.getDistance()<=0)
if (cp.getDistance()<=m_margin)
m_connected = true;
return 1.f;
}

9
Demos/OpenGL/DemoApplication.cpp
View File

@@ -33,7 +33,7 @@ subject to the following restrictions:
#include "LinearMath/btSerializer.h"
#include "GLDebugFont.h"
static bool use6Dof = false;
extern bool gDisableDeactivation;
int numObjects = 0;
const int maxNumObjects = 16384;
@@ -535,7 +535,7 @@ void DemoApplication::setShootBoxShape ()
if (!m_shootBoxShape)
{
btBoxShape* box = new btBoxShape(btVector3(0.5,0.5,0.5));
box->initializePolyhedralFeatures();
// box->initializePolyhedralFeatures();
m_shootBoxShape = box;
}
}
@@ -776,7 +776,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
if (use6Dof)
if ((m_modifierKeys& BT_ACTIVE_SHIFT)==0)
{
btTransform tr;
tr.setIdentity();
@@ -822,8 +822,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
}
use6Dof = !use6Dof;
//save mouse position for dragging
gOldPickingPos = rayTo;
gHitPos = pickPos;

9
Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp
View File

@@ -215,7 +215,10 @@ btCollisionShape* btWorldImporter::convertCollisionShape( btCollisionShapeData*
{
case BOX_SHAPE_PROXYTYPE:
{
shape = createBoxShape(implicitShapeDimensions/localScaling+margin);
btBoxShape* box= (btBoxShape*)createBoxShape(implicitShapeDimensions/localScaling+margin);
//box->initializePolyhedralFeatures();
shape = box;
break;
}
case SPHERE_SHAPE_PROXYTYPE:
@@ -333,6 +336,7 @@ btCollisionShape* btWorldImporter::convertCollisionShape( btCollisionShapeData*
{
hullShape->addPoint(tmpPoints[i]);
}
hullShape->setMargin(bsd->m_collisionMargin);
//hullShape->initializePolyhedralFeatures();
shape = hullShape;
break;
@@ -345,7 +349,8 @@ btCollisionShape* btWorldImporter::convertCollisionShape( btCollisionShapeData*
if (shape)
{
//shape->setMargin(bsd->m_collisionMargin);
shape->setMargin(bsd->m_collisionMargin);
btVector3 localScaling;
localScaling.deSerializeFloat(bsd->m_localScaling);
shape->setLocalScaling(localScaling);

37
Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.cpp
View File

@@ -238,7 +238,7 @@ void btBulletXmlWorldImporter::deSerializeConvexHullShapeData(TiXmlNode* pParent
SET_INT_VALUE(pParent,convexHullData,m_numUnscaledPoints);
m_collisionShapeData.push_back((btCollisionShapeData*)convexHullData);
m_pointerLookup.insert(ptr,convexHullData);
m_pointerLookup.insert((void*)ptr,convexHullData);
}
void btBulletXmlWorldImporter::deSerializeCompoundShapeChildData(TiXmlNode* pParent)
@@ -292,7 +292,7 @@ void btBulletXmlWorldImporter::deSerializeCompoundShapeChildData(TiXmlNode* pPar
{
m_compoundShapeChildDataArrays.push_back(compoundChildArrayPtr);
btCompoundShapeChildData* cd = &compoundChildArrayPtr->at(0);
m_pointerLookup.insert(ptr,cd);
m_pointerLookup.insert((void*)ptr,cd);
}
}
@@ -328,7 +328,7 @@ void btBulletXmlWorldImporter::deSerializeCompoundShapeData(TiXmlNode* pParent)
SET_FLOAT_VALUE(pParent, compoundData,m_collisionMargin);
m_collisionShapeData.push_back((btCollisionShapeData*)compoundData);
m_pointerLookup.insert(ptr,compoundData);
m_pointerLookup.insert((void*)ptr,compoundData);
}
@@ -348,12 +348,17 @@ void btBulletXmlWorldImporter::deSerializeStaticPlaneShapeData(TiXmlNode* pParen
SET_FLOAT_VALUE(pParent, planeData,m_planeConstant);
m_collisionShapeData.push_back((btCollisionShapeData*)planeData);
m_pointerLookup.insert(ptr,planeData);
m_pointerLookup.insert((void*)ptr,planeData);
}
void btBulletXmlWorldImporter::deSerializeDynamicsWorldData(TiXmlNode* pParent)
{
btContactSolverInfo solverInfo;
//btVector3 gravity(0,0,0);
//setDynamicsWorldInfo(gravity,solverInfo);
//gravity and world info
}
@@ -371,12 +376,13 @@ void btBulletXmlWorldImporter::deSerializeConvexInternalShapeData(TiXmlNode* pPa
deSerializeCollisionShapeData(xmlShapeData,&convexShape->m_collisionShapeData);
SET_FLOAT_VALUE(pParent,convexShape,m_collisionMargin)
SET_VECTOR4_VALUE(pParent,convexShape,m_localScaling)
SET_VECTOR4_VALUE(pParent,convexShape,m_implicitShapeDimensions)
m_collisionShapeData.push_back((btCollisionShapeData*)convexShape);
m_pointerLookup.insert(ptr,convexShape);
m_pointerLookup.insert((void*)ptr,convexShape);
}
@@ -433,7 +439,7 @@ void btBulletXmlWorldImporter::deSerializeGeneric6DofConstraintData(TiXmlNode* p
SET_INT_VALUE(pParent, dof6Data,m_useOffsetForConstraintFrame);
m_constraintData.push_back((btTypedConstraintData*)dof6Data);
m_pointerLookup.insert(ptr,dof6Data);
m_pointerLookup.insert((void*)ptr,dof6Data);
}
void btBulletXmlWorldImporter::deSerializeRigidBodyFloatData(TiXmlNode* pParent)
@@ -500,7 +506,7 @@ void btBulletXmlWorldImporter::deSerializeRigidBodyFloatData(TiXmlNode* pParent)
m_rigidBodyData.push_back(rbData);
m_pointerLookup.insert(ptr,rbData);
m_pointerLookup.insert((void*)ptr,rbData);
// rbData->m_collisionObjectData.m_collisionShape = (void*) (int)atof(txt);
}
@@ -558,13 +564,13 @@ void btBulletXmlWorldImporter::fixupConstraintData(btTypedConstraintData* tcd)
{
if (tcd->m_rbA)
{
btRigidBodyData** ptrptr = (btRigidBodyData**)m_pointerLookup.find((intptr_t)tcd->m_rbA);
btRigidBodyData** ptrptr = (btRigidBodyData**)m_pointerLookup.find(tcd->m_rbA);
btAssert(ptrptr);
tcd->m_rbA = ptrptr? *ptrptr : 0;
}
if (tcd->m_rbB)
{
btRigidBodyData** ptrptr = (btRigidBodyData**)m_pointerLookup.find((intptr_t)tcd->m_rbB);
btRigidBodyData** ptrptr = (btRigidBodyData**)m_pointerLookup.find(tcd->m_rbB);
btAssert(ptrptr);
tcd->m_rbB = ptrptr? *ptrptr : 0;
}
@@ -581,7 +587,7 @@ void btBulletXmlWorldImporter::fixupCollisionDataPointers(btCollisionShapeData*
{
btCompoundShapeData* compound = (btCompoundShapeData*) shapeData;
int ptr = (intptr_t) compound->m_childShapePtr;
void** cdptr = m_pointerLookup.find(ptr);
void** cdptr = m_pointerLookup.find((void*)ptr);
btCompoundShapeChildData** c = (btCompoundShapeChildData**)cdptr;
btAssert(c);
if (c)
@@ -598,7 +604,7 @@ void btBulletXmlWorldImporter::fixupCollisionDataPointers(btCollisionShapeData*
{
btConvexHullShapeData* convexData = (btConvexHullShapeData*)shapeData;
int ptr = (intptr_t)convexData->m_unscaledPointsFloatPtr;
btVector3FloatData** ptrptr = (btVector3FloatData**)m_pointerLookup.find(ptr);
btVector3FloatData** ptrptr = (btVector3FloatData**)m_pointerLookup.find((void*)ptr);
btAssert(ptrptr);
if (ptrptr)
{
@@ -646,7 +652,7 @@ void btBulletXmlWorldImporter::auto_serialize_root_level_children(TiXmlNode* pPa
for (int i=0;i<numVectors;i++)
vectors[i] = v[i];
m_floatVertexArrays.push_back(vectors);
m_pointerLookup.insert(ptr,vectors);
m_pointerLookup.insert((void*)ptr,vectors);
continue;
}
@@ -713,8 +719,7 @@ void btBulletXmlWorldImporter::auto_serialize_root_level_children(TiXmlNode* pPa
for (int c=0;c<childDataArray->size();c++)
{
btCompoundShapeChildData* childData = &childDataArray->at(c);
int hashKey = (intptr_t) childData->m_childShape;
btCollisionShapeData** ptrptr = (btCollisionShapeData**)m_pointerLookup[hashKey];
btCollisionShapeData** ptrptr = (btCollisionShapeData**)m_pointerLookup[childData->m_childShape];
btAssert(ptrptr);
if (ptrptr)
{
@@ -734,8 +739,8 @@ void btBulletXmlWorldImporter::auto_serialize_root_level_children(TiXmlNode* pPa
for (int i=0;i<m_rigidBodyData.size();i++)
{
btRigidBodyData* rbData = m_rigidBodyData[i];
int hashKey = (intptr_t)rbData->m_collisionObjectData.m_collisionShape;
void** ptrptr = m_pointerLookup.find(hashKey);
void** ptrptr = m_pointerLookup.find(rbData->m_collisionObjectData.m_collisionShape);
//btAssert(ptrptr);
rbData->m_collisionObjectData.m_broadphaseHandle = 0;
rbData->m_collisionObjectData.m_rootCollisionShape = 0;

2
Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.h
View File

@@ -43,7 +43,7 @@ protected:
btAlignedObjectArray<btAlignedObjectArray<btCompoundShapeChildData>* > m_compoundShapeChildDataArrays;
btAlignedObjectArray<btRigidBodyData*> m_rigidBodyData;
btAlignedObjectArray<btTypedConstraintData*> m_constraintData;
btHashMap<btHashInt,void*> m_pointerLookup;
btHashMap<btHashPtr,void*> m_pointerLookup;
int m_fileVersion;
bool m_fileOk;

2
src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
View File

@@ -84,7 +84,7 @@ btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObj
btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold());
void* mem = 0;
void* mem = 0;
if (m_persistentManifoldPoolAllocator->getFreeCount())
{

63
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
View File

@@ -407,9 +407,51 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
{
}
};
struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result
{
btDiscreteCollisionDetectorInterface::Result* m_originalResult;
btVector3 m_reportedNormalOnWorld;
btScalar m_marginOnA;
btScalar m_marginOnB;
btScalar m_reportedDistance;
bool m_foundResult;
btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB)
:m_originalResult(result),
m_marginOnA(marginOnA),
m_marginOnB(marginOnB),
m_foundResult(false)
{
}
virtual void setShapeIdentifiersA(int partId0,int index0){}
virtual void setShapeIdentifiersB(int partId1,int index1){}
virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorldOrg,btScalar depthOrg)
{
m_reportedDistance = depthOrg;
m_reportedNormalOnWorld = normalOnBInWorld;
btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB;
m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB);
if (m_reportedDistance<0.f)
{
m_foundResult = true;
}
m_originalResult->addContactPoint(normalOnBInWorld,adjustedPointB,m_reportedDistance);
}
};
btDummyResult dummy;
///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it
btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin();
btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin();
btWithoutMarginResult withoutMargin(resultOut, min0Margin,min1Margin);
btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0;
btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1;
@@ -431,32 +473,35 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
body0Wrap->getWorldTransform(),
body1Wrap->getWorldTransform(),
sepNormalWorldSpace);
sepNormalWorldSpace,*resultOut);
} else
{
#ifdef ZERO_MARGIN
gjkPairDetector.setIgnoreMargin(true);
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#else
//gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
gjkPairDetector.getClosestPoints(input,withoutMargin,dispatchInfo.m_debugDraw);
//gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
#endif //ZERO_MARGIN
btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
if (l2>SIMD_EPSILON)
//btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
//if (l2>SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin();
#ifdef ZERO_MARGIN
foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
#else
foundSepAxis = gjkPairDetector.getCachedSeparatingDistance()<(min0->getMargin()+min1->getMargin());
foundSepAxis = withoutMargin.m_foundResult && minDist<0;//-(min0->getMargin()+min1->getMargin());
#endif
}
}
if (foundSepAxis)
{
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
@@ -498,7 +543,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
body0Wrap->getWorldTransform(),
body1Wrap->getWorldTransform(),
sepNormalWorldSpace);
sepNormalWorldSpace,*resultOut);
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
} else

31
src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
View File

@@ -201,13 +201,11 @@ const char* btConvexHullShape::serialize(void* dataBuffer, btSerializer* seriali
return "btConvexHullShapeData";
}
void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const
void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
{
#if 1
min = FLT_MAX;
max = -FLT_MAX;
btVector3 witnesPtMin;
btVector3 witnesPtMax;
minProj = FLT_MAX;
maxProj = -FLT_MAX;
int numVerts = m_unscaledPoints.size();
for(int i=0;i<numVerts;i++)
@@ -215,31 +213,30 @@ void btConvexHullShape::project(const btTransform& trans, const btVector3& dir,
btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
btVector3 pt = trans * vtx;
btScalar dp = pt.dot(dir);
if(dp < min)
if(dp < minProj)
{
min = dp;
minProj = dp;
witnesPtMin = pt;
}
if(dp > max)
if(dp > maxProj)
{
max = dp;
maxProj = dp;
witnesPtMax=pt;
}
}
#else
btVector3 localAxis = dir*trans.getBasis();
btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis));
witnesPtMin = trans(localGetSupportingVertex(localAxis));
witnesPtMax = trans(localGetSupportingVertex(-localAxis));
min = vtx1.dot(dir);
max = vtx2.dot(dir);
minProj = witnesPtMin.dot(dir);
maxProj = witnesPtMax.dot(dir);
#endif
if(min>max)
if(minProj>maxProj)
{
btScalar tmp = min;
min = max;
max = tmp;
btSwap(minProj,maxProj);
btSwap(witnesPtMin,witnesPtMax);
}

2
src/BulletCollision/CollisionShapes/btConvexHullShape.h
View File

@@ -73,7 +73,7 @@ public:
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
virtual void project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const;
virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;
//debugging

26
src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
View File

@@ -274,23 +274,29 @@ void btConvexPolyhedron::initialize()
#endif
}
void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const
void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
{
min = FLT_MAX;
max = -FLT_MAX;
minProj = FLT_MAX;
maxProj = -FLT_MAX;
int numVerts = m_vertices.size();
for(int i=0;i<numVerts;i++)
{
btVector3 pt = trans * m_vertices[i];
btScalar dp = pt.dot(dir);
if(dp < min) min = dp;
if(dp > max) max = dp;
if(dp < minProj)
{
minProj = dp;
witnesPtMin = pt;
}
if(dp > maxProj)
{
maxProj = dp;
witnesPtMax = pt;
}
}
if(min>max)
if(minProj>maxProj)
{
btScalar tmp = min;
min = max;
max = tmp;
btSwap(minProj,maxProj);
btSwap(witnesPtMin,witnesPtMax);
}
}

2
src/BulletCollision/CollisionShapes/btConvexPolyhedron.h
View File

@@ -56,7 +56,7 @@ ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron
void initialize();
bool testContainment() const;
void project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const;
void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;
};

94
src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
View File

@@ -37,7 +37,7 @@ btPolyhedralConvexShape::~btPolyhedralConvexShape()
}
bool btPolyhedralConvexShape::initializePolyhedralFeatures()
bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin)
{
if (m_polyhedron)
@@ -56,31 +56,34 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures()
btVector3& newVertex = orgVertices.expand();
getVertex(i,newVertex);
}
#if 0
btAlignedObjectArray<btVector3> planeEquations;
btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations);
btAlignedObjectArray<btVector3> shiftedPlaneEquations;
for (int p=0;p<planeEquations.size();p++)
btConvexHullComputer conv;
if (shiftVerticesByMargin)
{
btVector3 plane = planeEquations[p];
plane[3] -= getMargin();
shiftedPlaneEquations.push_back(plane);
btAlignedObjectArray<btVector3> planeEquations;
btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations);
btAlignedObjectArray<btVector3> shiftedPlaneEquations;
for (int p=0;p<planeEquations.size();p++)
{
btVector3 plane = planeEquations[p];
btScalar margin = getMargin();
plane[3] -= getMargin();
shiftedPlaneEquations.push_back(plane);
}
btAlignedObjectArray<btVector3> tmpVertices;
btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices);
conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f);
} else
{
conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);
}
btAlignedObjectArray<btVector3> tmpVertices;
btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices);
btConvexHullComputer conv;
conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f);
#else
btConvexHullComputer conv;
conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);
#endif
btAlignedObjectArray<btVector3> faceNormals;
@@ -193,7 +196,8 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures()
{
//do the merge: use Graham Scan 2d convex hull
btAlignedObjectArray<GrahamVector2> orgpoints;
btAlignedObjectArray<GrahamVector3> orgpoints;
btVector3 averageFaceNormal(0,0,0);
for (int i=0;i<coplanarFaceGroup.size();i++)
{
@@ -201,16 +205,12 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures()
btFace& face = tmpFaces[coplanarFaceGroup[i]];
btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
btVector3 xyPlaneNormal(0,0,1);
btQuaternion rotationArc = shortestArcQuat(faceNormal,xyPlaneNormal);
averageFaceNormal+=faceNormal;
for (int f=0;f<face.m_indices.size();f++)
{
int orgIndex = face.m_indices[f];
btVector3 pt = m_polyhedron->m_vertices[orgIndex];
btVector3 rotatedPt = quatRotate(rotationArc,pt);
rotatedPt.setZ(0);
bool found = false;
for (int i=0;i<orgpoints.size();i++)
@@ -223,34 +223,45 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures()
}
}
if (!found)
orgpoints.push_back(GrahamVector2(rotatedPt,orgIndex));
orgpoints.push_back(GrahamVector3(pt,orgIndex));
}
}
btFace combinedFace;
for (int i=0;i<4;i++)
combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
btAlignedObjectArray<GrahamVector2> hull;
GrahamScanConvexHull2D(orgpoints,hull);
btAlignedObjectArray<GrahamVector3> hull;
averageFaceNormal.normalize();
GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal);
for (int i=0;i<hull.size();i++)
{
combinedFace.m_indices.push_back(hull[i].m_orgIndex);
for(int k = 0; k < orgpoints.size(); k++) {
if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex) {
for(int k = 0; k < orgpoints.size(); k++)
{
if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex)
{
orgpoints[k].m_orgIndex = -1; // invalidate...
break;
}
}
}
}
// are there rejected vertices?
bool reject_merge = false;
for(int i = 0; i < orgpoints.size(); i++) {
if(orgpoints[i].m_orgIndex == -1)
continue; // this is in the hull...
// this vertex is rejected -- is anybody else using this vertex?
for(int j = 0; j < tmpFaces.size(); j++) {
btFace& face = tmpFaces[j];
// is this a face of the current coplanar group?
bool is_in_current_group = false;
@@ -276,20 +287,23 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures()
if(reject_merge)
break;
}
if(!reject_merge) {
if (!reject_merge)
{
// do this merge!
did_merge = true;
m_polyhedron->m_faces.push_back(combinedFace);
m_polyhedron->m_faces.push_back(combinedFace);
}
}
if(!did_merge)
{
for (int i=0;i<coplanarFaceGroup.size();i++)
{
m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
btFace face = tmpFaces[coplanarFaceGroup[i]];
m_polyhedron->m_faces.push_back(face);
}
}
}

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

@@ -40,7 +40,8 @@ public:
virtual ~btPolyhedralConvexShape();
///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges)
virtual bool initializePolyhedralFeatures();
///experimental/work-in-progress
virtual bool initializePolyhedralFeatures(int shiftVerticesByMargin=0);
const btConvexPolyhedron* getConvexPolyhedron() const
{

165
src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
View File

@@ -77,12 +77,15 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex
}
static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth)
static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB)
{
btScalar Min0,Max0;
btScalar Min1,Max1;
hullA.project(transA,sep_axis, Min0, Max0);
hullB.project(transB, sep_axis, Min1, Max1);
btVector3 witnesPtMinA,witnesPtMaxA;
btVector3 witnesPtMinB,witnesPtMaxB;
hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA);
hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB);
if(Max0<Min1 || Max1<Min0)
return false;
@@ -91,7 +94,19 @@ static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedro
btAssert(d0>=0.0f);
btScalar d1 = Max1 - Min0;
btAssert(d1>=0.0f);
depth = d0<d1 ? d0:d1;
if (d0<d1)
{
depth = d0;
witnessPointA = witnesPtMaxA;
witnessPointB = witnesPtMinB;
} else
{
depth = d1;
witnessPointA = witnesPtMinA;
witnessPointB = witnesPtMaxB;
}
return true;
}
@@ -163,8 +178,66 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans
}
#endif //TEST_INTERNAL_OBJECTS
SIMD_FORCE_INLINE void btSegmentsClosestPoints(
btVector3& ptsVector,
btVector3& offsetA,
btVector3& offsetB,
btScalar& tA, btScalar& tB,
const btVector3& translation,
const btVector3& dirA, btScalar hlenA,
const btVector3& dirB, btScalar hlenB )
{
// compute the parameters of the closest points on each line segment
btScalar dirA_dot_dirB = btDot(dirA,dirB);
btScalar dirA_dot_trans = btDot(dirA,translation);
btScalar dirB_dot_trans = btDot(dirB,translation);
btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
if ( denom == 0.0f ) {
tA = 0.0f;
} else {
tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
if ( tA < -hlenA )
tA = -hlenA;
else if ( tA > hlenA )
tA = hlenA;
}
tB = tA * dirA_dot_dirB - dirB_dot_trans;
if ( tB < -hlenB ) {
tB = -hlenB;
tA = tB * dirA_dot_dirB + dirA_dot_trans;
if ( tA < -hlenA )
tA = -hlenA;
else if ( tA > hlenA )
tA = hlenA;
} else if ( tB > hlenB ) {
tB = hlenB;
tA = tB * dirA_dot_dirB + dirA_dot_trans;
if ( tA < -hlenA )
tA = -hlenA;
else if ( tA > hlenA )
tA = hlenA;
}
// compute the closest points relative to segment centers.
offsetA = dirA * tA;
offsetB = dirB * tB;
ptsVector = translation - offsetA + offsetB;
}
bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep)
bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
{
gActualSATPairTests++;
@@ -182,9 +255,9 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
for(int i=0;i<numFacesA;i++)
{
const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
const btVector3 faceANormalWS = transA.getBasis() * Normal;
btVector3 faceANormalWS = transA.getBasis() * Normal;
if (DeltaC2.dot(faceANormalWS)<0)
continue;
faceANormalWS*=-1.f;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
@@ -195,7 +268,8 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
#endif
btScalar d;
if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d))
btVector3 wA,wB;
if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d,wA,wB))
return false;
if(d<dmin)
@@ -210,9 +284,9 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
for(int i=0;i<numFacesB;i++)
{
const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
const btVector3 WorldNormal = transB.getBasis() * Normal;
btVector3 WorldNormal = transB.getBasis() * Normal;
if (DeltaC2.dot(WorldNormal)<0)
continue;
WorldNormal *=-1.f;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
@@ -223,7 +297,8 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
#endif
btScalar d;
if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d))
btVector3 wA,wB;
if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d,wA,wB))
return false;
if(d<dmin)
@@ -234,6 +309,12 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
}
btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
int edgeA=-1;
int edgeB=-1;
btVector3 worldEdgeA;
btVector3 worldEdgeB;
btVector3 witnessPointA,witnessPointB;
int curEdgeEdge = 0;
// Test edges
@@ -252,7 +333,7 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
{
Cross = Cross.normalize();
if (DeltaC2.dot(Cross)<0)
continue;
Cross *= -1.f;
#ifdef TEST_INTERNAL_OBJECTS
@@ -263,20 +344,68 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
#endif
btScalar dist;
if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist))
btVector3 wA,wB;
if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist,wA,wB))
return false;
if(dist<dmin)
{
dmin = dist;
sep = Cross;
edgeA=e0;
edgeB=e1;
worldEdgeA = WorldEdge0;
worldEdgeB = WorldEdge1;
witnessPointA=wA;
witnessPointB=wB;
}
}
}
}
if((DeltaC2.dot(sep))>0.0f)
if (edgeA>=0&&edgeB>=0)
{
// printf("edge-edge\n");
//add an edge-edge contact
btVector3 ptsVector;
btVector3 offsetA;
btVector3 offsetB;
btScalar tA;
btScalar tB;
btVector3 translation = witnessPointB-witnessPointA;
btVector3 dirA = worldEdgeA;
btVector3 dirB = worldEdgeB;
btScalar hlenB = 1e30f;
btScalar hlenA = 1e30f;
btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB,
translation,
dirA, hlenA,
dirB,hlenB);
btScalar nlSqrt = ptsVector.length2();
if (nlSqrt>SIMD_EPSILON)
{
btScalar nl = btSqrt(nlSqrt);
ptsVector *= 1.f/nl;
if (ptsVector.dot(DeltaC2)<0.f)
{
ptsVector*=-1.f;
}
btVector3 ptOnB = witnessPointB + offsetB;
btScalar distance = nl;
resultOut.addContactPoint(ptsVector, ptOnB,-distance);
}
}
if((DeltaC2.dot(sep))<0.0f)
sep = -sep;
return true;
@@ -359,8 +488,8 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
for (int i=0;i<pVtxIn->size();i++)
{
btScalar depth = planeNormalWS.dot(pVtxIn->at(i))+planeEqWS;
btVector3 vtx = pVtxIn->at(i);
btScalar depth = planeNormalWS.dot(vtx)+planeEqWS;
if (depth <=minDist)
{
// printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
@@ -395,6 +524,9 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
}
void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
{
@@ -404,6 +536,7 @@ void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin
//const btVector3 DeltaC2 = c0 - c1;
int closestFaceB=-1;
btScalar dmax = -FLT_MAX;
{

2
src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h
View File

@@ -35,7 +35,7 @@ struct btPolyhedralContactClipping
static void clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut);
static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut);
static bool findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep);
static bool findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut);
///the clipFace method is used internally
static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS);

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

@@ -1146,8 +1146,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
sum += iMJlB.dot(solverConstraint.m_contactNormal);
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
btScalar fsum = btFabs(sum);
btAssert(fsum > SIMD_EPSILON);
solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f;
}

30
src/LinearMath/btGrahamScan2dConvexHull.h
View File

@@ -21,9 +21,9 @@ subject to the following restrictions:
#include "btVector3.h"
#include "btAlignedObjectArray.h"
struct GrahamVector2 : public btVector3
struct GrahamVector3 : public btVector3
{
GrahamVector2(const btVector3& org, int orgIndex)
GrahamVector3(const btVector3& org, int orgIndex)
:btVector3(org),
m_orgIndex(orgIndex)
{
@@ -39,7 +39,7 @@ struct btAngleCompareFunc {
: m_anchor(anchor)
{
}
bool operator()(const GrahamVector2& a, const GrahamVector2& b) const {
bool operator()(const GrahamVector3& a, const GrahamVector3& b) const {
if (a.m_angle != b.m_angle)
return a.m_angle < b.m_angle;
else
@@ -56,32 +56,38 @@ struct btAngleCompareFunc {
}
};
inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector2>& originalPoints, btAlignedObjectArray<GrahamVector2>& hull)
inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector3>& originalPoints, btAlignedObjectArray<GrahamVector3>& hull, const btVector3& normalAxis)
{
btVector3 axis0,axis1;
btPlaneSpace1(normalAxis,axis0,axis1);
if (originalPoints.size()<=1)
{
for (int i=0;i<originalPoints.size();i++)
hull.push_back(originalPoints[0]);
return;
}
//step1 : find anchor point with smallest x/y and move it to first location
//also precompute angles
//step1 : find anchor point with smallest projection on axis0 and move it to first location
for (int i=0;i<originalPoints.size();i++)
{
const btVector3& left = originalPoints[i];
const btVector3& right = originalPoints[0];
if (left.x() < right.x() ||
(!(right.x() < left.x()) && left.y() < right.y()))
btScalar projL = originalPoints[i].dot(axis0);
btScalar projR = originalPoints[0].dot(axis0);
if (projL < projR)
{
originalPoints.swap(0,i);
}
}
for (int i=0;i<originalPoints.size();i++)
//also precompute angles
originalPoints[0].m_angle = -1e30f;
for (int i=1;i<originalPoints.size();i++)
{
btVector3 xvec(1,0,0);
btVector3 xvec = axis0;
btVector3 ar = originalPoints[i]-originalPoints[0];
originalPoints[i].m_angle = btCross(xvec, ar).dot(btVector3(0,0,1)) / ar.length();
originalPoints[i].m_angle = btCross(xvec, ar).dot(normalAxis) / ar.length();
}
//step 2: sort all points, based on 'angle' with this anchor
@@ -99,7 +105,7 @@ inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector2>& original
while (!isConvex&& hull.size()>1) {
btVector3& a = hull[hull.size()-2];
btVector3& b = hull[hull.size()-1];
isConvex = btCross(a-b,a-originalPoints[i]).dot(btVector3(0,0,1))> 0;
isConvex = btCross(a-b,a-originalPoints[i]).dot(normalAxis)> 0;
if (!isConvex)
hull.pop_back();
else