diff --git a/data/cube.urdf b/data/cube.urdf index 679e106b5..4a6d484ec 100644 --- a/data/cube.urdf +++ b/data/cube.urdf @@ -1,5 +1,5 @@ - + diff --git a/data/plane.urdf b/data/plane.urdf index 1b3d09682..b04f67e96 100644 --- a/data/plane.urdf +++ b/data/plane.urdf @@ -1,5 +1,5 @@ - + diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index aeb3f2aaa..89189c420 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -871,6 +871,16 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) int width = 1024; int height=768; + + if (args.CheckCmdLineFlag("width")) + { + args.GetCmdLineArgument("width",width ); + } + if (args.CheckCmdLineFlag("height")) + { + args.GetCmdLineArgument("height",height); + } + #ifndef NO_OPENGL3 SimpleOpenGL3App* simpleApp=0; sUseOpenGL2 =args.CheckCmdLineFlag("opengl2"); diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index 123ceea68..c181a379d 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -1026,6 +1026,11 @@ struct BulletMJCFImporterInternalData { handled = true; } + if (n=="site") + { + handled = true; + } + if (!handled) { logger->reportWarning( (sourceFileLocation(xml) + ": unknown field '" + n + "'").c_str() ); @@ -1323,6 +1328,11 @@ int BulletMJCFImporter::getRootLinkIndex() const return ""; } +std::string BulletMJCFImporter::getBodyName() const +{ + return m_data->m_fileModelName; +} + bool BulletMJCFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const { // UrdfLink* link = m_data->getLink(linkIndex); diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h index 66e9a2424..306d311ca 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h @@ -41,6 +41,8 @@ public: ///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range) virtual std::string getLinkName(int linkIndex) const; + + virtual std::string getBodyName() const; /// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const; diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 8407c6191..019eb2868 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -269,6 +269,11 @@ std::string BulletURDFImporter::getLinkName(int linkIndex) const } return ""; } + +std::string BulletURDFImporter::getBodyName() const +{ + return m_data->m_urdfParser.getModel().m_name; +} std::string BulletURDFImporter::getJointName(int linkIndex) const { @@ -956,9 +961,15 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha } break; } - + case URDF_GEOM_PLANE: + { + b3Warning("No default visual for URDF_GEOM_PLANE"); + break; + } default: + { b3Warning("Error: unknown visual geometry type %i\n", visual->m_geometry.m_type); + } } //if we have a convex, tesselate into localVertices/localIndices diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index c61bf77a2..76d34c7c7 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -34,6 +34,8 @@ public: virtual int getRootLinkIndex() const; virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray& childLinkIndices) const; + + virtual std::string getBodyName() const; virtual std::string getLinkName(int linkIndex) const; diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index c8f996bf5..220c4e588 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -15,7 +15,6 @@ public: virtual ~URDFImporterInterface() {} - virtual bool loadURDF(const char* fileName, bool forceFixedBase = false)=0; virtual bool loadSDF(const char* fileName, bool forceFixedBase = false) { return false;} @@ -27,6 +26,13 @@ public: ///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range) virtual std::string getLinkName(int linkIndex) const =0; + + //various derived class in internal source code break with new pure virtual methods, so provide some default implementation + virtual std::string getBodyName() const + { + return ""; + } + /// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false;} diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 7615782ba..b0695cd20 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -552,7 +552,6 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl return 0; } - b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index a0e18235e..fba7ab366 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -20,6 +20,7 @@ struct BodyJointInfoCache { std::string m_baseName; btAlignedObjectArray m_jointInfo; + std::string m_bodyName; }; struct PhysicsClientSharedMemoryInternalData { @@ -106,6 +107,7 @@ bool PhysicsClientSharedMemory::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& { BodyJointInfoCache* bodyJoints = *bodyJointsPtr; info.m_baseName = bodyJoints->m_baseName.c_str(); + info.m_bodyName = bodyJoints->m_bodyName.c_str(); return true; } @@ -306,6 +308,7 @@ void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const Sha BodyJointInfoCache* bodyJoints = new BodyJointInfoCache; m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints); + bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName; for (int i = 0; i < bf.m_multiBodies.size(); i++) { @@ -418,6 +421,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { BodyJointInfoCache* bodyJoints = new BodyJointInfoCache; m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints); + bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName; for (int i = 0; i < bf.m_multiBodies.size(); i++) { diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 5ca0f35a2..cb2c62e68 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -17,6 +17,7 @@ struct BodyJointInfoCache2 { std::string m_baseName; btAlignedObjectArray m_jointInfo; + std::string m_bodyName; }; @@ -605,6 +606,7 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints); + bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName; for (int i = 0; i < bf.m_multiBodies.size(); i++) { @@ -931,6 +933,7 @@ bool PhysicsDirect::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const { BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; info.m_baseName = bodyJoints->m_baseName.c_str(); + info.m_bodyName = bodyJoints->m_bodyName.c_str(); return true; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 4cd2c5464..4ed8ea901 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -120,6 +120,7 @@ struct InteralBodyData btMultiBody* m_multiBody; btRigidBody* m_rigidBody; int m_testData; + std::string m_bodyName; btTransform m_rootLocalInertialFrame; btAlignedObjectArray m_linkLocalInertialFrames; @@ -1579,6 +1580,7 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, { btScalar mass = 0; bodyHandle->m_rootLocalInertialFrame.setIdentity(); + bodyHandle->m_bodyName = u2b.getBodyName(); btVector3 localInertiaDiagonal(0,0,0); int urdfLinkIndex = u2b.getRootLinkIndex(); u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame); @@ -1796,6 +1798,8 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto btMultiBody* mb = creation.getBulletMultiBody(); btRigidBody* rb = creation.getRigidBody(); + + bodyHandle->m_bodyName = u2b.getBodyName(); if (useMultiBody) { @@ -2525,7 +2529,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED; serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId; - serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; + serverStatusOut.m_dataStreamArguments.m_bodyName[0] = 0; + + InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateJacobianArguments.m_bodyUniqueId); + if (bodyHandle) + { + strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName,bodyHandle->m_bodyName.c_str()); + } + + serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; hasStatus = true; break; @@ -2852,6 +2864,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); } serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; + InteralBodyData* body = m_data->getHandle(bodyUniqueId); + strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); hasStatus = true; } else @@ -5217,8 +5231,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } - - default: { BT_PROFILE("CMD_UNKNOWN"); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 096349762..f5038c6a4 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -103,6 +103,7 @@ struct BulletDataStreamArgs { char m_bulletFileName[MAX_FILENAME_LENGTH]; int m_bodyUniqueId; + char m_bodyName[MAX_FILENAME_LENGTH]; }; struct SetJointFeedbackArgs @@ -364,9 +365,6 @@ struct RequestActualStateArgs int m_bodyUniqueId; }; - - - struct SendActualStateArgs { int m_bodyUniqueId; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 21c8b685e..7a201aee9 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -201,6 +201,7 @@ struct b3UserConstraint struct b3BodyInfo { const char* m_baseName; + const char* m_bodyName; // for btRigidBody, it does not have a base, but can still have a body name from urdf }; diff --git a/examples/pybullet/examples/kuka_with_cube.py b/examples/pybullet/examples/kuka_with_cube.py index 309449138..aad32f489 100644 --- a/examples/pybullet/examples/kuka_with_cube.py +++ b/examples/pybullet/examples/kuka_with_cube.py @@ -48,6 +48,9 @@ p.setRealTimeSimulation(useRealTimeSimulation) trailDuration = 15 logId = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt",[0,1,2]) + +for i in xrange(5): + print "Body %d's name is %s." % (i, p.getBodyInfo(i)[1]) while 1: if (useRealTimeSimulation): diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 551163b71..cc9bda8d0 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -221,6 +221,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P int freeIndex = -1; int method = eCONNECT_GUI; int i; + char* options=""; b3PhysicsClientHandle sm = 0; if (sNumPhysicsClients >= MAX_PHYSICS_CLIENTS) @@ -237,15 +238,24 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P const char* hostName = "localhost"; - int size = PySequence_Size(args); - if (size == 1) + + static char* kwlist1[] = {"method","key", "options", NULL}; + static char* kwlist2[] = {"method","hostName", "port", "options", NULL}; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|is", kwlist1, &method,&key,&options)) { - if (!PyArg_ParseTuple(args, "i", &method)) + int port = -1; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|sis", kwlist2, &method,&hostName, &port,&options)) { - PyErr_SetString(SpamError, - "connectPhysicsServer expected argument GUI, " - "DIRECT, SHARED_MEMORY, UDP or TCP"); return NULL; + } else + { + PyErr_Clear(); + if (port>=0) + { + udpPort = port; + tcpPort = port; + } } } @@ -264,45 +274,12 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P } } - if (size == 2) - { - if (!PyArg_ParseTuple(args, "ii", &method, &key)) - { - if (!PyArg_ParseTuple(args, "is", &method, &hostName)) - { - PyErr_SetString(SpamError, - "connectPhysicsServer cannot parse second argument (either integer or string)"); - return NULL; - } - else - { - PyErr_Clear(); - } - } - } - - if (size == 3) - { - int port = -1; - if (!PyArg_ParseTuple(args, "isi", &method, &hostName, &port)) - { - PyErr_SetString(SpamError, - "connectPhysicsServer 3 arguments: method, hostname, port"); - return NULL; - } - if (port >= 0) - { - udpPort = port; - tcpPort = port; - } - } - switch (method) { case eCONNECT_GUI: { - int argc = 0; - char* argv[1] = {0}; + int argc = 2; + char* argv[2] = {"unused",options}; #ifdef __APPLE__ sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); @@ -1714,8 +1691,9 @@ static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args, PyObject* struct b3BodyInfo info; if (b3GetBodyInfo(sm, bodyUniqueId, &info)) { - PyObject* pyListJointInfo = PyTuple_New(1); + PyObject* pyListJointInfo = PyTuple_New(2); PyTuple_SetItem(pyListJointInfo, 0, PyString_FromString(info.m_baseName)); + PyTuple_SetItem(pyListJointInfo, 1, PyString_FromString(info.m_bodyName)); return pyListJointInfo; } } @@ -5161,7 +5139,7 @@ static PyMethodDef SpamMethods[] = { {"getJointState", (PyCFunction)pybullet_getJointState, METH_VARARGS | METH_KEYWORDS, "Get the state (position, velocity etc) for a joint on a body."}, - + {"getLinkState", (PyCFunction)pybullet_getLinkState, METH_VARARGS | METH_KEYWORDS, "Provides extra information such as the Cartesian world coordinates" " center of mass (COM) of the link, relative to the world reference" diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp index 11b4b28f6..179dfc4f2 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp @@ -562,7 +562,7 @@ void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyData* bodies,b3InertiaD b3AlignedObjectArray deltaAngularVelocities; deltaLinearVelocities.resize(totalNumSplitBodies); deltaAngularVelocities.resize(totalNumSplitBodies); - for (int i=0;i -class btAxisSweep3Internal : public btBroadphaseInterface -{ -protected: - - BP_FP_INT_TYPE m_bpHandleMask; - BP_FP_INT_TYPE m_handleSentinel; - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - class Edge - { - public: - BP_FP_INT_TYPE m_pos; // low bit is min/max - BP_FP_INT_TYPE m_handle; - - BP_FP_INT_TYPE IsMax() const {return static_cast(m_pos & 1);} - }; - -public: - class Handle : public btBroadphaseProxy - { - public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - // indexes into the edge arrays - BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12 -// BP_FP_INT_TYPE m_uniqueId; - btBroadphaseProxy* m_dbvtProxy;//for faster raycast - //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject - - SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;} - SIMD_FORCE_INLINE BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];} - }; // 24 bytes + 24 for Edge structures = 44 bytes total per entry - - -protected: - btVector3 m_worldAabbMin; // overall system bounds - btVector3 m_worldAabbMax; // overall system bounds - - btVector3 m_quantize; // scaling factor for quantization - - BP_FP_INT_TYPE m_numHandles; // number of active handles - BP_FP_INT_TYPE m_maxHandles; // max number of handles - Handle* m_pHandles; // handles pool - - BP_FP_INT_TYPE m_firstFreeHandle; // free handles list - - Edge* m_pEdges[3]; // edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries) - void* m_pEdgesRawPtr[3]; - - btOverlappingPairCache* m_pairCache; - - ///btOverlappingPairCallback is an additional optional user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache. - btOverlappingPairCallback* m_userPairCallback; - - bool m_ownsPairCache; - - int m_invalidPair; - - ///additional dynamic aabb structure, used to accelerate ray cast queries. - ///can be disabled using a optional argument in the constructor - btDbvtBroadphase* m_raycastAccelerator; - btOverlappingPairCache* m_nullPairCache; - - - // allocation/deallocation - BP_FP_INT_TYPE allocHandle(); - void freeHandle(BP_FP_INT_TYPE handle); - - - bool testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1); - -#ifdef DEBUG_BROADPHASE - void debugPrintAxis(int axis,bool checkCardinality=true); -#endif //DEBUG_BROADPHASE - - //Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); - //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); - - - - void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); - void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); - void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); - void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); - -public: - - btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0,bool disableRaycastAccelerator = false); - - virtual ~btAxisSweep3Internal(); - - BP_FP_INT_TYPE getNumHandles() const - { - return m_numHandles; - } - - virtual void calculateOverlappingPairs(btDispatcher* dispatcher); - - BP_FP_INT_TYPE addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher); - void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher); - void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); - SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;} - - virtual void resetPool(btDispatcher* dispatcher); - - void processAllOverlappingPairs(btOverlapCallback* callback); - - //Broadphase Interface - virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr , int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher); - virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); - virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; - - virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)); - virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); - - - void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const; - ///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result - void unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; - - bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - - btOverlappingPairCache* getOverlappingPairCache() - { - return m_pairCache; - } - const btOverlappingPairCache* getOverlappingPairCache() const - { - return m_pairCache; - } - - void setOverlappingPairUserCallback(btOverlappingPairCallback* pairCallback) - { - m_userPairCallback = pairCallback; - } - const btOverlappingPairCallback* getOverlappingPairUserCallback() const - { - return m_userPairCallback; - } - - ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame - ///will add some transform later - virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const - { - aabbMin = m_worldAabbMin; - aabbMax = m_worldAabbMax; - } - - virtual void printStats() - { -/* printf("btAxisSweep3.h\n"); - printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles); - printf("aabbMin=%f,%f,%f,aabbMax=%f,%f,%f\n",m_worldAabbMin.getX(),m_worldAabbMin.getY(),m_worldAabbMin.getZ(), - m_worldAabbMax.getX(),m_worldAabbMax.getY(),m_worldAabbMax.getZ()); - */ - - } - -}; - -//////////////////////////////////////////////////////////////////// - - - - -#ifdef DEBUG_BROADPHASE -#include - -template -void btAxisSweep3::debugPrintAxis(int axis, bool checkCardinality) -{ - int numEdges = m_pHandles[0].m_maxEdges[axis]; - printf("SAP Axis %d, numEdges=%d\n",axis,numEdges); - - int i; - for (i=0;im_handle); - int handleIndex = pEdge->IsMax()? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis]; - char beginOrEnd; - beginOrEnd=pEdge->IsMax()?'E':'B'; - printf(" [%c,h=%d,p=%x,i=%d]\n",beginOrEnd,pEdge->m_handle,pEdge->m_pos,handleIndex); - } - - if (checkCardinality) - btAssert(numEdges == m_numHandles*2+1); -} -#endif //DEBUG_BROADPHASE - -template -btBroadphaseProxy* btAxisSweep3Internal::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher) -{ - (void)shapeType; - BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher); - - Handle* handle = getHandle(handleId); - - if (m_raycastAccelerator) - { - btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher); - handle->m_dbvtProxy = rayProxy; - } - return handle; -} - - - -template -void btAxisSweep3Internal::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) -{ - Handle* handle = static_cast(proxy); - if (m_raycastAccelerator) - m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy,dispatcher); - removeHandle(static_cast(handle->m_uniqueId), dispatcher); -} - -template -void btAxisSweep3Internal::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher) -{ - Handle* handle = static_cast(proxy); - handle->m_aabbMin = aabbMin; - handle->m_aabbMax = aabbMax; - updateHandle(static_cast(handle->m_uniqueId), aabbMin, aabbMax,dispatcher); - if (m_raycastAccelerator) - m_raycastAccelerator->setAabb(handle->m_dbvtProxy,aabbMin,aabbMax,dispatcher); - -} - -template -void btAxisSweep3Internal::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax) -{ - if (m_raycastAccelerator) - { - m_raycastAccelerator->rayTest(rayFrom,rayTo,rayCallback,aabbMin,aabbMax); - } else - { - //choose axis? - BP_FP_INT_TYPE axis = 0; - //for each proxy - for (BP_FP_INT_TYPE i=1;i -void btAxisSweep3Internal::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) -{ - if (m_raycastAccelerator) - { - m_raycastAccelerator->aabbTest(aabbMin,aabbMax,callback); - } else - { - //choose axis? - BP_FP_INT_TYPE axis = 0; - //for each proxy - for (BP_FP_INT_TYPE i=1;im_aabbMin,handle->m_aabbMax)) - { - callback.process(handle); - } - } - } - } -} - - - -template -void btAxisSweep3Internal::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const -{ - Handle* pHandle = static_cast(proxy); - aabbMin = pHandle->m_aabbMin; - aabbMax = pHandle->m_aabbMax; -} - - -template -void btAxisSweep3Internal::unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const -{ - Handle* pHandle = static_cast(proxy); - - unsigned short vecInMin[3]; - unsigned short vecInMax[3]; - - vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos ; - vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos +1 ; - vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos ; - vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos +1 ; - vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos ; - vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos +1 ; - - aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()),(btScalar)(vecInMin[1]) / (m_quantize.getY()),(btScalar)(vecInMin[2]) / (m_quantize.getZ())); - aabbMin += m_worldAabbMin; - - aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()),(btScalar)(vecInMax[1]) / (m_quantize.getY()),(btScalar)(vecInMax[2]) / (m_quantize.getZ())); - aabbMax += m_worldAabbMin; -} - - - - -template -btAxisSweep3Internal::btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache , bool disableRaycastAccelerator) -:m_bpHandleMask(handleMask), -m_handleSentinel(handleSentinel), -m_pairCache(pairCache), -m_userPairCallback(0), -m_ownsPairCache(false), -m_invalidPair(0), -m_raycastAccelerator(0) -{ - BP_FP_INT_TYPE maxHandles = static_cast(userMaxHandles+1);//need to add one sentinel handle - - if (!m_pairCache) - { - void* ptr = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16); - m_pairCache = new(ptr) btHashedOverlappingPairCache(); - m_ownsPairCache = true; - } - - if (!disableRaycastAccelerator) - { - m_nullPairCache = new (btAlignedAlloc(sizeof(btNullPairCache),16)) btNullPairCache(); - m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase),16)) btDbvtBroadphase(m_nullPairCache);//m_pairCache); - m_raycastAccelerator->m_deferedcollide = true;//don't add/remove pairs - } - - //btAssert(bounds.HasVolume()); - - // init bounds - m_worldAabbMin = worldAabbMin; - m_worldAabbMax = worldAabbMax; - - btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin; - - BP_FP_INT_TYPE maxInt = m_handleSentinel; - - m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize; - - // allocate handles buffer, using btAlignedAlloc, and put all handles on free list - m_pHandles = new Handle[maxHandles]; - - m_maxHandles = maxHandles; - m_numHandles = 0; - - // handle 0 is reserved as the null index, and is also used as the sentinel - m_firstFreeHandle = 1; - { - for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++) - m_pHandles[i].SetNextFree(static_cast(i + 1)); - m_pHandles[maxHandles - 1].SetNextFree(0); - } - - { - // allocate edge buffers - for (int i = 0; i < 3; i++) - { - m_pEdgesRawPtr[i] = btAlignedAlloc(sizeof(Edge)*maxHandles*2,16); - m_pEdges[i] = new(m_pEdgesRawPtr[i]) Edge[maxHandles * 2]; - } - } - //removed overlap management - - // make boundary sentinels - - m_pHandles[0].m_clientObject = 0; - - for (int axis = 0; axis < 3; axis++) - { - m_pHandles[0].m_minEdges[axis] = 0; - m_pHandles[0].m_maxEdges[axis] = 1; - - m_pEdges[axis][0].m_pos = 0; - m_pEdges[axis][0].m_handle = 0; - m_pEdges[axis][1].m_pos = m_handleSentinel; - m_pEdges[axis][1].m_handle = 0; -#ifdef DEBUG_BROADPHASE - debugPrintAxis(axis); -#endif //DEBUG_BROADPHASE - - } - -} - -template -btAxisSweep3Internal::~btAxisSweep3Internal() -{ - if (m_raycastAccelerator) - { - m_nullPairCache->~btOverlappingPairCache(); - btAlignedFree(m_nullPairCache); - m_raycastAccelerator->~btDbvtBroadphase(); - btAlignedFree (m_raycastAccelerator); - } - - for (int i = 2; i >= 0; i--) - { - btAlignedFree(m_pEdgesRawPtr[i]); - } - delete [] m_pHandles; - - if (m_ownsPairCache) - { - m_pairCache->~btOverlappingPairCache(); - btAlignedFree(m_pairCache); - } -} - -template -void btAxisSweep3Internal::quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const -{ -#ifdef OLD_CLAMPING_METHOD - ///problem with this clamping method is that the floating point during quantization might still go outside the range [(0|isMax) .. (m_handleSentinel&m_bpHandleMask]|isMax] - ///see http://code.google.com/p/bullet/issues/detail?id=87 - btVector3 clampedPoint(point); - clampedPoint.setMax(m_worldAabbMin); - clampedPoint.setMin(m_worldAabbMax); - btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize; - out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & m_bpHandleMask) | isMax); - out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & m_bpHandleMask) | isMax); - out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & m_bpHandleMask) | isMax); -#else - btVector3 v = (point - m_worldAabbMin) * m_quantize; - out[0]=(v[0]<=0)?(BP_FP_INT_TYPE)isMax:(v[0]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[0]&m_bpHandleMask)|isMax); - out[1]=(v[1]<=0)?(BP_FP_INT_TYPE)isMax:(v[1]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[1]&m_bpHandleMask)|isMax); - out[2]=(v[2]<=0)?(BP_FP_INT_TYPE)isMax:(v[2]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[2]&m_bpHandleMask)|isMax); -#endif //OLD_CLAMPING_METHOD -} - - -template -BP_FP_INT_TYPE btAxisSweep3Internal::allocHandle() -{ - btAssert(m_firstFreeHandle); - - BP_FP_INT_TYPE handle = m_firstFreeHandle; - m_firstFreeHandle = getHandle(handle)->GetNextFree(); - m_numHandles++; - - return handle; -} - -template -void btAxisSweep3Internal::freeHandle(BP_FP_INT_TYPE handle) -{ - btAssert(handle > 0 && handle < m_maxHandles); - - getHandle(handle)->SetNextFree(m_firstFreeHandle); - m_firstFreeHandle = handle; - - m_numHandles--; -} - - -template -BP_FP_INT_TYPE btAxisSweep3Internal::addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher) -{ - // quantize the bounds - BP_FP_INT_TYPE min[3], max[3]; - quantize(min, aabbMin, 0); - quantize(max, aabbMax, 1); - - // allocate a handle - BP_FP_INT_TYPE handle = allocHandle(); - - - Handle* pHandle = getHandle(handle); - - pHandle->m_uniqueId = static_cast(handle); - //pHandle->m_pOverlaps = 0; - pHandle->m_clientObject = pOwner; - pHandle->m_collisionFilterGroup = collisionFilterGroup; - pHandle->m_collisionFilterMask = collisionFilterMask; - - // compute current limit of edge arrays - BP_FP_INT_TYPE limit = static_cast(m_numHandles * 2); - - - // insert new edges just inside the max boundary edge - for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++) - { - - m_pHandles[0].m_maxEdges[axis] += 2; - - m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1]; - - m_pEdges[axis][limit - 1].m_pos = min[axis]; - m_pEdges[axis][limit - 1].m_handle = handle; - - m_pEdges[axis][limit].m_pos = max[axis]; - m_pEdges[axis][limit].m_handle = handle; - - pHandle->m_minEdges[axis] = static_cast(limit - 1); - pHandle->m_maxEdges[axis] = limit; - } - - // now sort the new edges to their correct position - sortMinDown(0, pHandle->m_minEdges[0], dispatcher,false); - sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher,false); - sortMinDown(1, pHandle->m_minEdges[1], dispatcher,false); - sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher,false); - sortMinDown(2, pHandle->m_minEdges[2], dispatcher,true); - sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher,true); - - - return handle; -} - - -template -void btAxisSweep3Internal::removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher) -{ - - Handle* pHandle = getHandle(handle); - - //explicitly remove the pairs containing the proxy - //we could do it also in the sortMinUp (passing true) - ///@todo: compare performance - if (!m_pairCache->hasDeferredRemoval()) - { - m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher); - } - - // compute current limit of edge arrays - int limit = static_cast(m_numHandles * 2); - - int axis; - - for (axis = 0;axis<3;axis++) - { - m_pHandles[0].m_maxEdges[axis] -= 2; - } - - // remove the edges by sorting them up to the end of the list - for ( axis = 0; axis < 3; axis++) - { - Edge* pEdges = m_pEdges[axis]; - BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis]; - pEdges[max].m_pos = m_handleSentinel; - - sortMaxUp(axis,max,dispatcher,false); - - - BP_FP_INT_TYPE i = pHandle->m_minEdges[axis]; - pEdges[i].m_pos = m_handleSentinel; - - - sortMinUp(axis,i,dispatcher,false); - - pEdges[limit-1].m_handle = 0; - pEdges[limit-1].m_pos = m_handleSentinel; - -#ifdef DEBUG_BROADPHASE - debugPrintAxis(axis,false); -#endif //DEBUG_BROADPHASE - - - } - - - // free the handle - freeHandle(handle); - - -} - -template -void btAxisSweep3Internal::resetPool(btDispatcher* /*dispatcher*/) -{ - if (m_numHandles == 0) - { - m_firstFreeHandle = 1; - { - for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++) - m_pHandles[i].SetNextFree(static_cast(i + 1)); - m_pHandles[m_maxHandles - 1].SetNextFree(0); - } - } -} - - -extern int gOverlappingPairs; -//#include - -template -void btAxisSweep3Internal::calculateOverlappingPairs(btDispatcher* dispatcher) -{ - - if (m_pairCache->hasDeferredRemoval()) - { - - btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray(); - - //perform a sort, to find duplicates and to sort 'invalid' pairs to the end - overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); - - overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); - m_invalidPair = 0; - - - int i; - - btBroadphasePair previousPair; - previousPair.m_pProxy0 = 0; - previousPair.m_pProxy1 = 0; - previousPair.m_algorithm = 0; - - - for (i=0;iprocessOverlap(pair); - } else - { - needsRemoval = true; - } - } else - { - //remove duplicate - needsRemoval = true; - //should have no algorithm - btAssert(!pair.m_algorithm); - } - - if (needsRemoval) - { - m_pairCache->cleanOverlappingPair(pair,dispatcher); - - // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); - // m_overlappingPairArray.pop_back(); - 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 - #ifdef CLEAN_INVALID_PAIRS - - //perform a sort, to sort 'invalid' pairs to the end - overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); - - overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); - m_invalidPair = 0; - #endif//CLEAN_INVALID_PAIRS - - //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size()); - } - -} - - -template -bool btAxisSweep3Internal::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) -{ - const Handle* pHandleA = static_cast(proxy0); - const Handle* pHandleB = static_cast(proxy1); - - //optimization 1: check the array index (memory address), instead of the m_pos - - for (int axis = 0; axis < 3; axis++) - { - if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || - pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) - { - return false; - } - } - return true; -} - -template -bool btAxisSweep3Internal::testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1) -{ - //optimization 1: check the array index (memory address), instead of the m_pos - - if (pHandleA->m_maxEdges[axis0] < pHandleB->m_minEdges[axis0] || - pHandleB->m_maxEdges[axis0] < pHandleA->m_minEdges[axis0] || - pHandleA->m_maxEdges[axis1] < pHandleB->m_minEdges[axis1] || - pHandleB->m_maxEdges[axis1] < pHandleA->m_minEdges[axis1]) - { - return false; - } - return true; -} - -template -void btAxisSweep3Internal::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher) -{ -// btAssert(bounds.IsFinite()); - //btAssert(bounds.HasVolume()); - - Handle* pHandle = getHandle(handle); - - // quantize the new bounds - BP_FP_INT_TYPE min[3], max[3]; - quantize(min, aabbMin, 0); - quantize(max, aabbMax, 1); - - // update changed edges - for (int axis = 0; axis < 3; axis++) - { - BP_FP_INT_TYPE emin = pHandle->m_minEdges[axis]; - BP_FP_INT_TYPE emax = pHandle->m_maxEdges[axis]; - - int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos; - int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos; - - m_pEdges[axis][emin].m_pos = min[axis]; - m_pEdges[axis][emax].m_pos = max[axis]; - - // expand (only adds overlaps) - if (dmin < 0) - sortMinDown(axis, emin,dispatcher,true); - - if (dmax > 0) - sortMaxUp(axis, emax,dispatcher,true); - - // shrink (only removes overlaps) - if (dmin > 0) - sortMinUp(axis, emin,dispatcher,true); - - if (dmax < 0) - sortMaxDown(axis, emax,dispatcher,true); - -#ifdef DEBUG_BROADPHASE - debugPrintAxis(axis); -#endif //DEBUG_BROADPHASE - } - - -} - - - - -// sorting a min edge downwards can only ever *add* overlaps -template -void btAxisSweep3Internal::sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps) -{ - - Edge* pEdge = m_pEdges[axis] + edge; - Edge* pPrev = pEdge - 1; - Handle* pHandleEdge = getHandle(pEdge->m_handle); - - while (pEdge->m_pos < pPrev->m_pos) - { - Handle* pHandlePrev = getHandle(pPrev->m_handle); - - if (pPrev->IsMax()) - { - // if previous edge is a maximum check the bounds and add an overlap if necessary - const int axis1 = (1 << axis) & 3; - const int axis2 = (1 << axis1) & 3; - if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev,axis1,axis2)) - { - m_pairCache->addOverlappingPair(pHandleEdge,pHandlePrev); - if (m_userPairCallback) - m_userPairCallback->addOverlappingPair(pHandleEdge,pHandlePrev); - - //AddOverlap(pEdge->m_handle, pPrev->m_handle); - - } - - // update edge reference in other handle - pHandlePrev->m_maxEdges[axis]++; - } - else - pHandlePrev->m_minEdges[axis]++; - - pHandleEdge->m_minEdges[axis]--; - - // swap the edges - Edge swap = *pEdge; - *pEdge = *pPrev; - *pPrev = swap; - - // decrement - pEdge--; - pPrev--; - } - -#ifdef DEBUG_BROADPHASE - debugPrintAxis(axis); -#endif //DEBUG_BROADPHASE - -} - -// sorting a min edge upwards can only ever *remove* overlaps -template -void btAxisSweep3Internal::sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps) -{ - Edge* pEdge = m_pEdges[axis] + edge; - Edge* pNext = pEdge + 1; - Handle* pHandleEdge = getHandle(pEdge->m_handle); - - while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos)) - { - Handle* pHandleNext = getHandle(pNext->m_handle); - - if (pNext->IsMax()) - { - Handle* handle0 = getHandle(pEdge->m_handle); - Handle* handle1 = getHandle(pNext->m_handle); - const int axis1 = (1 << axis) & 3; - const int axis2 = (1 << axis1) & 3; - - // if next edge is maximum remove any overlap between the two handles - if (updateOverlaps -#ifdef USE_OVERLAP_TEST_ON_REMOVES - && testOverlap2D(handle0,handle1,axis1,axis2) -#endif //USE_OVERLAP_TEST_ON_REMOVES - ) - { - - - m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher); - if (m_userPairCallback) - m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher); - - } - - - // update edge reference in other handle - pHandleNext->m_maxEdges[axis]--; - } - else - pHandleNext->m_minEdges[axis]--; - - pHandleEdge->m_minEdges[axis]++; - - // swap the edges - Edge swap = *pEdge; - *pEdge = *pNext; - *pNext = swap; - - // increment - pEdge++; - pNext++; - } - - -} - -// sorting a max edge downwards can only ever *remove* overlaps -template -void btAxisSweep3Internal::sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps) -{ - - Edge* pEdge = m_pEdges[axis] + edge; - Edge* pPrev = pEdge - 1; - Handle* pHandleEdge = getHandle(pEdge->m_handle); - - while (pEdge->m_pos < pPrev->m_pos) - { - Handle* pHandlePrev = getHandle(pPrev->m_handle); - - if (!pPrev->IsMax()) - { - // if previous edge was a minimum remove any overlap between the two handles - Handle* handle0 = getHandle(pEdge->m_handle); - Handle* handle1 = getHandle(pPrev->m_handle); - const int axis1 = (1 << axis) & 3; - const int axis2 = (1 << axis1) & 3; - - if (updateOverlaps -#ifdef USE_OVERLAP_TEST_ON_REMOVES - && testOverlap2D(handle0,handle1,axis1,axis2) -#endif //USE_OVERLAP_TEST_ON_REMOVES - ) - { - //this is done during the overlappingpairarray iteration/narrowphase collision - - - m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher); - if (m_userPairCallback) - m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher); - - - - } - - // update edge reference in other handle - pHandlePrev->m_minEdges[axis]++;; - } - else - pHandlePrev->m_maxEdges[axis]++; - - pHandleEdge->m_maxEdges[axis]--; - - // swap the edges - Edge swap = *pEdge; - *pEdge = *pPrev; - *pPrev = swap; - - // decrement - pEdge--; - pPrev--; - } - - -#ifdef DEBUG_BROADPHASE - debugPrintAxis(axis); -#endif //DEBUG_BROADPHASE - -} - -// sorting a max edge upwards can only ever *add* overlaps -template -void btAxisSweep3Internal::sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps) -{ - Edge* pEdge = m_pEdges[axis] + edge; - Edge* pNext = pEdge + 1; - Handle* pHandleEdge = getHandle(pEdge->m_handle); - - while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos)) - { - Handle* pHandleNext = getHandle(pNext->m_handle); - - const int axis1 = (1 << axis) & 3; - const int axis2 = (1 << axis1) & 3; - - if (!pNext->IsMax()) - { - // if next edge is a minimum check the bounds and add an overlap if necessary - if (updateOverlaps && testOverlap2D(pHandleEdge, pHandleNext,axis1,axis2)) - { - Handle* handle0 = getHandle(pEdge->m_handle); - Handle* handle1 = getHandle(pNext->m_handle); - m_pairCache->addOverlappingPair(handle0,handle1); - if (m_userPairCallback) - m_userPairCallback->addOverlappingPair(handle0,handle1); - } - - // update edge reference in other handle - pHandleNext->m_minEdges[axis]--; - } - else - pHandleNext->m_maxEdges[axis]--; - - pHandleEdge->m_maxEdges[axis]++; - - // swap the edges - Edge swap = *pEdge; - *pEdge = *pNext; - *pNext = swap; - - // increment - pEdge++; - pNext++; - } - -} - - - -//////////////////////////////////////////////////////////////////// - +#include "btAxisSweep3Internal.h" /// The btAxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase. /// It uses arrays rather then lists for storage of the 3 axis. Also it operates using 16 bit integer coordinates instead of floats. @@ -1047,4 +50,3 @@ public: }; #endif - diff --git a/src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h b/src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h new file mode 100644 index 000000000..2c4d41bc0 --- /dev/null +++ b/src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h @@ -0,0 +1,1022 @@ +//Bullet Continuous Collision Detection and Physics Library +//Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +// +// btAxisSweep3.h +// +// Copyright (c) 2006 Simon Hobbs +// +// 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_AXIS_SWEEP_3_INTERNAL_H +#define BT_AXIS_SWEEP_3_INTERNAL_H + +#include "LinearMath/btVector3.h" +#include "btOverlappingPairCache.h" +#include "btBroadphaseInterface.h" +#include "btBroadphaseProxy.h" +#include "btOverlappingPairCallback.h" +#include "btDbvtBroadphase.h" + +//#define DEBUG_BROADPHASE 1 +#define USE_OVERLAP_TEST_ON_REMOVES 1 + +/// The internal templace class btAxisSweep3Internal implements the sweep and prune broadphase. +/// It uses quantized integers to represent the begin and end points for each of the 3 axis. +/// Dont use this class directly, use btAxisSweep3 or bt32BitAxisSweep3 instead. +template +class btAxisSweep3Internal : public btBroadphaseInterface +{ +protected: + + BP_FP_INT_TYPE m_bpHandleMask; + BP_FP_INT_TYPE m_handleSentinel; + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + class Edge + { + public: + BP_FP_INT_TYPE m_pos; // low bit is min/max + BP_FP_INT_TYPE m_handle; + + BP_FP_INT_TYPE IsMax() const {return static_cast(m_pos & 1);} + }; + +public: + class Handle : public btBroadphaseProxy + { + public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + // indexes into the edge arrays + BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12 +// BP_FP_INT_TYPE m_uniqueId; + btBroadphaseProxy* m_dbvtProxy;//for faster raycast + //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject + + SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;} + SIMD_FORCE_INLINE BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];} + }; // 24 bytes + 24 for Edge structures = 44 bytes total per entry + + +protected: + btVector3 m_worldAabbMin; // overall system bounds + btVector3 m_worldAabbMax; // overall system bounds + + btVector3 m_quantize; // scaling factor for quantization + + BP_FP_INT_TYPE m_numHandles; // number of active handles + BP_FP_INT_TYPE m_maxHandles; // max number of handles + Handle* m_pHandles; // handles pool + + BP_FP_INT_TYPE m_firstFreeHandle; // free handles list + + Edge* m_pEdges[3]; // edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries) + void* m_pEdgesRawPtr[3]; + + btOverlappingPairCache* m_pairCache; + + ///btOverlappingPairCallback is an additional optional user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache. + btOverlappingPairCallback* m_userPairCallback; + + bool m_ownsPairCache; + + int m_invalidPair; + + ///additional dynamic aabb structure, used to accelerate ray cast queries. + ///can be disabled using a optional argument in the constructor + btDbvtBroadphase* m_raycastAccelerator; + btOverlappingPairCache* m_nullPairCache; + + + // allocation/deallocation + BP_FP_INT_TYPE allocHandle(); + void freeHandle(BP_FP_INT_TYPE handle); + + + bool testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1); + +#ifdef DEBUG_BROADPHASE + void debugPrintAxis(int axis,bool checkCardinality=true); +#endif //DEBUG_BROADPHASE + + //Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); + //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); + + + + void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); + void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); + void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); + void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); + +public: + + btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0,bool disableRaycastAccelerator = false); + + virtual ~btAxisSweep3Internal(); + + BP_FP_INT_TYPE getNumHandles() const + { + return m_numHandles; + } + + virtual void calculateOverlappingPairs(btDispatcher* dispatcher); + + BP_FP_INT_TYPE addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher); + void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher); + void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); + SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;} + + virtual void resetPool(btDispatcher* dispatcher); + + void processAllOverlappingPairs(btOverlapCallback* callback); + + //Broadphase Interface + virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr , int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher); + virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); + virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; + + virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)); + virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); + + + void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const; + ///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result + void unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; + + bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + + btOverlappingPairCache* getOverlappingPairCache() + { + return m_pairCache; + } + const btOverlappingPairCache* getOverlappingPairCache() const + { + return m_pairCache; + } + + void setOverlappingPairUserCallback(btOverlappingPairCallback* pairCallback) + { + m_userPairCallback = pairCallback; + } + const btOverlappingPairCallback* getOverlappingPairUserCallback() const + { + return m_userPairCallback; + } + + ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame + ///will add some transform later + virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const + { + aabbMin = m_worldAabbMin; + aabbMax = m_worldAabbMax; + } + + virtual void printStats() + { +/* printf("btAxisSweep3.h\n"); + printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles); + printf("aabbMin=%f,%f,%f,aabbMax=%f,%f,%f\n",m_worldAabbMin.getX(),m_worldAabbMin.getY(),m_worldAabbMin.getZ(), + m_worldAabbMax.getX(),m_worldAabbMax.getY(),m_worldAabbMax.getZ()); + */ + + } + +}; + +//////////////////////////////////////////////////////////////////// + + + + +#ifdef DEBUG_BROADPHASE +#include + +template +void btAxisSweep3::debugPrintAxis(int axis, bool checkCardinality) +{ + int numEdges = m_pHandles[0].m_maxEdges[axis]; + printf("SAP Axis %d, numEdges=%d\n",axis,numEdges); + + int i; + for (i=0;im_handle); + int handleIndex = pEdge->IsMax()? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis]; + char beginOrEnd; + beginOrEnd=pEdge->IsMax()?'E':'B'; + printf(" [%c,h=%d,p=%x,i=%d]\n",beginOrEnd,pEdge->m_handle,pEdge->m_pos,handleIndex); + } + + if (checkCardinality) + btAssert(numEdges == m_numHandles*2+1); +} +#endif //DEBUG_BROADPHASE + +template +btBroadphaseProxy* btAxisSweep3Internal::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher) +{ + (void)shapeType; + BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher); + + Handle* handle = getHandle(handleId); + + if (m_raycastAccelerator) + { + btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher); + handle->m_dbvtProxy = rayProxy; + } + return handle; +} + + + +template +void btAxisSweep3Internal::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) +{ + Handle* handle = static_cast(proxy); + if (m_raycastAccelerator) + m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy,dispatcher); + removeHandle(static_cast(handle->m_uniqueId), dispatcher); +} + +template +void btAxisSweep3Internal::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher) +{ + Handle* handle = static_cast(proxy); + handle->m_aabbMin = aabbMin; + handle->m_aabbMax = aabbMax; + updateHandle(static_cast(handle->m_uniqueId), aabbMin, aabbMax,dispatcher); + if (m_raycastAccelerator) + m_raycastAccelerator->setAabb(handle->m_dbvtProxy,aabbMin,aabbMax,dispatcher); + +} + +template +void btAxisSweep3Internal::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax) +{ + if (m_raycastAccelerator) + { + m_raycastAccelerator->rayTest(rayFrom,rayTo,rayCallback,aabbMin,aabbMax); + } else + { + //choose axis? + BP_FP_INT_TYPE axis = 0; + //for each proxy + for (BP_FP_INT_TYPE i=1;i +void btAxisSweep3Internal::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) +{ + if (m_raycastAccelerator) + { + m_raycastAccelerator->aabbTest(aabbMin,aabbMax,callback); + } else + { + //choose axis? + BP_FP_INT_TYPE axis = 0; + //for each proxy + for (BP_FP_INT_TYPE i=1;im_aabbMin,handle->m_aabbMax)) + { + callback.process(handle); + } + } + } + } +} + + + +template +void btAxisSweep3Internal::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const +{ + Handle* pHandle = static_cast(proxy); + aabbMin = pHandle->m_aabbMin; + aabbMax = pHandle->m_aabbMax; +} + + +template +void btAxisSweep3Internal::unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const +{ + Handle* pHandle = static_cast(proxy); + + unsigned short vecInMin[3]; + unsigned short vecInMax[3]; + + vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos ; + vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos +1 ; + vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos ; + vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos +1 ; + vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos ; + vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos +1 ; + + aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()),(btScalar)(vecInMin[1]) / (m_quantize.getY()),(btScalar)(vecInMin[2]) / (m_quantize.getZ())); + aabbMin += m_worldAabbMin; + + aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()),(btScalar)(vecInMax[1]) / (m_quantize.getY()),(btScalar)(vecInMax[2]) / (m_quantize.getZ())); + aabbMax += m_worldAabbMin; +} + + + + +template +btAxisSweep3Internal::btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache , bool disableRaycastAccelerator) +:m_bpHandleMask(handleMask), +m_handleSentinel(handleSentinel), +m_pairCache(pairCache), +m_userPairCallback(0), +m_ownsPairCache(false), +m_invalidPair(0), +m_raycastAccelerator(0) +{ + BP_FP_INT_TYPE maxHandles = static_cast(userMaxHandles+1);//need to add one sentinel handle + + if (!m_pairCache) + { + void* ptr = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16); + m_pairCache = new(ptr) btHashedOverlappingPairCache(); + m_ownsPairCache = true; + } + + if (!disableRaycastAccelerator) + { + m_nullPairCache = new (btAlignedAlloc(sizeof(btNullPairCache),16)) btNullPairCache(); + m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase),16)) btDbvtBroadphase(m_nullPairCache);//m_pairCache); + m_raycastAccelerator->m_deferedcollide = true;//don't add/remove pairs + } + + //btAssert(bounds.HasVolume()); + + // init bounds + m_worldAabbMin = worldAabbMin; + m_worldAabbMax = worldAabbMax; + + btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin; + + BP_FP_INT_TYPE maxInt = m_handleSentinel; + + m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize; + + // allocate handles buffer, using btAlignedAlloc, and put all handles on free list + m_pHandles = new Handle[maxHandles]; + + m_maxHandles = maxHandles; + m_numHandles = 0; + + // handle 0 is reserved as the null index, and is also used as the sentinel + m_firstFreeHandle = 1; + { + for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++) + m_pHandles[i].SetNextFree(static_cast(i + 1)); + m_pHandles[maxHandles - 1].SetNextFree(0); + } + + { + // allocate edge buffers + for (int i = 0; i < 3; i++) + { + m_pEdgesRawPtr[i] = btAlignedAlloc(sizeof(Edge)*maxHandles*2,16); + m_pEdges[i] = new(m_pEdgesRawPtr[i]) Edge[maxHandles * 2]; + } + } + //removed overlap management + + // make boundary sentinels + + m_pHandles[0].m_clientObject = 0; + + for (int axis = 0; axis < 3; axis++) + { + m_pHandles[0].m_minEdges[axis] = 0; + m_pHandles[0].m_maxEdges[axis] = 1; + + m_pEdges[axis][0].m_pos = 0; + m_pEdges[axis][0].m_handle = 0; + m_pEdges[axis][1].m_pos = m_handleSentinel; + m_pEdges[axis][1].m_handle = 0; +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis); +#endif //DEBUG_BROADPHASE + + } + +} + +template +btAxisSweep3Internal::~btAxisSweep3Internal() +{ + if (m_raycastAccelerator) + { + m_nullPairCache->~btOverlappingPairCache(); + btAlignedFree(m_nullPairCache); + m_raycastAccelerator->~btDbvtBroadphase(); + btAlignedFree (m_raycastAccelerator); + } + + for (int i = 2; i >= 0; i--) + { + btAlignedFree(m_pEdgesRawPtr[i]); + } + delete [] m_pHandles; + + if (m_ownsPairCache) + { + m_pairCache->~btOverlappingPairCache(); + btAlignedFree(m_pairCache); + } +} + +template +void btAxisSweep3Internal::quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const +{ +#ifdef OLD_CLAMPING_METHOD + ///problem with this clamping method is that the floating point during quantization might still go outside the range [(0|isMax) .. (m_handleSentinel&m_bpHandleMask]|isMax] + ///see http://code.google.com/p/bullet/issues/detail?id=87 + btVector3 clampedPoint(point); + clampedPoint.setMax(m_worldAabbMin); + clampedPoint.setMin(m_worldAabbMax); + btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize; + out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & m_bpHandleMask) | isMax); + out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & m_bpHandleMask) | isMax); + out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & m_bpHandleMask) | isMax); +#else + btVector3 v = (point - m_worldAabbMin) * m_quantize; + out[0]=(v[0]<=0)?(BP_FP_INT_TYPE)isMax:(v[0]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[0]&m_bpHandleMask)|isMax); + out[1]=(v[1]<=0)?(BP_FP_INT_TYPE)isMax:(v[1]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[1]&m_bpHandleMask)|isMax); + out[2]=(v[2]<=0)?(BP_FP_INT_TYPE)isMax:(v[2]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[2]&m_bpHandleMask)|isMax); +#endif //OLD_CLAMPING_METHOD +} + + +template +BP_FP_INT_TYPE btAxisSweep3Internal::allocHandle() +{ + btAssert(m_firstFreeHandle); + + BP_FP_INT_TYPE handle = m_firstFreeHandle; + m_firstFreeHandle = getHandle(handle)->GetNextFree(); + m_numHandles++; + + return handle; +} + +template +void btAxisSweep3Internal::freeHandle(BP_FP_INT_TYPE handle) +{ + btAssert(handle > 0 && handle < m_maxHandles); + + getHandle(handle)->SetNextFree(m_firstFreeHandle); + m_firstFreeHandle = handle; + + m_numHandles--; +} + + +template +BP_FP_INT_TYPE btAxisSweep3Internal::addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher) +{ + // quantize the bounds + BP_FP_INT_TYPE min[3], max[3]; + quantize(min, aabbMin, 0); + quantize(max, aabbMax, 1); + + // allocate a handle + BP_FP_INT_TYPE handle = allocHandle(); + + + Handle* pHandle = getHandle(handle); + + pHandle->m_uniqueId = static_cast(handle); + //pHandle->m_pOverlaps = 0; + pHandle->m_clientObject = pOwner; + pHandle->m_collisionFilterGroup = collisionFilterGroup; + pHandle->m_collisionFilterMask = collisionFilterMask; + + // compute current limit of edge arrays + BP_FP_INT_TYPE limit = static_cast(m_numHandles * 2); + + + // insert new edges just inside the max boundary edge + for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++) + { + + m_pHandles[0].m_maxEdges[axis] += 2; + + m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1]; + + m_pEdges[axis][limit - 1].m_pos = min[axis]; + m_pEdges[axis][limit - 1].m_handle = handle; + + m_pEdges[axis][limit].m_pos = max[axis]; + m_pEdges[axis][limit].m_handle = handle; + + pHandle->m_minEdges[axis] = static_cast(limit - 1); + pHandle->m_maxEdges[axis] = limit; + } + + // now sort the new edges to their correct position + sortMinDown(0, pHandle->m_minEdges[0], dispatcher,false); + sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher,false); + sortMinDown(1, pHandle->m_minEdges[1], dispatcher,false); + sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher,false); + sortMinDown(2, pHandle->m_minEdges[2], dispatcher,true); + sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher,true); + + + return handle; +} + + +template +void btAxisSweep3Internal::removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher) +{ + + Handle* pHandle = getHandle(handle); + + //explicitly remove the pairs containing the proxy + //we could do it also in the sortMinUp (passing true) + ///@todo: compare performance + if (!m_pairCache->hasDeferredRemoval()) + { + m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher); + } + + // compute current limit of edge arrays + int limit = static_cast(m_numHandles * 2); + + int axis; + + for (axis = 0;axis<3;axis++) + { + m_pHandles[0].m_maxEdges[axis] -= 2; + } + + // remove the edges by sorting them up to the end of the list + for ( axis = 0; axis < 3; axis++) + { + Edge* pEdges = m_pEdges[axis]; + BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis]; + pEdges[max].m_pos = m_handleSentinel; + + sortMaxUp(axis,max,dispatcher,false); + + + BP_FP_INT_TYPE i = pHandle->m_minEdges[axis]; + pEdges[i].m_pos = m_handleSentinel; + + + sortMinUp(axis,i,dispatcher,false); + + pEdges[limit-1].m_handle = 0; + pEdges[limit-1].m_pos = m_handleSentinel; + +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis,false); +#endif //DEBUG_BROADPHASE + + + } + + + // free the handle + freeHandle(handle); + + +} + +template +void btAxisSweep3Internal::resetPool(btDispatcher* /*dispatcher*/) +{ + if (m_numHandles == 0) + { + m_firstFreeHandle = 1; + { + for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++) + m_pHandles[i].SetNextFree(static_cast(i + 1)); + m_pHandles[m_maxHandles - 1].SetNextFree(0); + } + } +} + + +extern int gOverlappingPairs; +//#include + +template +void btAxisSweep3Internal::calculateOverlappingPairs(btDispatcher* dispatcher) +{ + + if (m_pairCache->hasDeferredRemoval()) + { + + btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray(); + + //perform a sort, to find duplicates and to sort 'invalid' pairs to the end + overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); + + overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); + m_invalidPair = 0; + + + int i; + + btBroadphasePair previousPair; + previousPair.m_pProxy0 = 0; + previousPair.m_pProxy1 = 0; + previousPair.m_algorithm = 0; + + + for (i=0;iprocessOverlap(pair); + } else + { + needsRemoval = true; + } + } else + { + //remove duplicate + needsRemoval = true; + //should have no algorithm + btAssert(!pair.m_algorithm); + } + + if (needsRemoval) + { + m_pairCache->cleanOverlappingPair(pair,dispatcher); + + // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); + // m_overlappingPairArray.pop_back(); + 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 + #ifdef CLEAN_INVALID_PAIRS + + //perform a sort, to sort 'invalid' pairs to the end + overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); + + overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); + m_invalidPair = 0; + #endif//CLEAN_INVALID_PAIRS + + //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size()); + } + +} + + +template +bool btAxisSweep3Internal::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) +{ + const Handle* pHandleA = static_cast(proxy0); + const Handle* pHandleB = static_cast(proxy1); + + //optimization 1: check the array index (memory address), instead of the m_pos + + for (int axis = 0; axis < 3; axis++) + { + if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || + pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) + { + return false; + } + } + return true; +} + +template +bool btAxisSweep3Internal::testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1) +{ + //optimization 1: check the array index (memory address), instead of the m_pos + + if (pHandleA->m_maxEdges[axis0] < pHandleB->m_minEdges[axis0] || + pHandleB->m_maxEdges[axis0] < pHandleA->m_minEdges[axis0] || + pHandleA->m_maxEdges[axis1] < pHandleB->m_minEdges[axis1] || + pHandleB->m_maxEdges[axis1] < pHandleA->m_minEdges[axis1]) + { + return false; + } + return true; +} + +template +void btAxisSweep3Internal::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher) +{ +// btAssert(bounds.IsFinite()); + //btAssert(bounds.HasVolume()); + + Handle* pHandle = getHandle(handle); + + // quantize the new bounds + BP_FP_INT_TYPE min[3], max[3]; + quantize(min, aabbMin, 0); + quantize(max, aabbMax, 1); + + // update changed edges + for (int axis = 0; axis < 3; axis++) + { + BP_FP_INT_TYPE emin = pHandle->m_minEdges[axis]; + BP_FP_INT_TYPE emax = pHandle->m_maxEdges[axis]; + + int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos; + int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos; + + m_pEdges[axis][emin].m_pos = min[axis]; + m_pEdges[axis][emax].m_pos = max[axis]; + + // expand (only adds overlaps) + if (dmin < 0) + sortMinDown(axis, emin,dispatcher,true); + + if (dmax > 0) + sortMaxUp(axis, emax,dispatcher,true); + + // shrink (only removes overlaps) + if (dmin > 0) + sortMinUp(axis, emin,dispatcher,true); + + if (dmax < 0) + sortMaxDown(axis, emax,dispatcher,true); + +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis); +#endif //DEBUG_BROADPHASE + } + + +} + + + + +// sorting a min edge downwards can only ever *add* overlaps +template +void btAxisSweep3Internal::sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps) +{ + + Edge* pEdge = m_pEdges[axis] + edge; + Edge* pPrev = pEdge - 1; + Handle* pHandleEdge = getHandle(pEdge->m_handle); + + while (pEdge->m_pos < pPrev->m_pos) + { + Handle* pHandlePrev = getHandle(pPrev->m_handle); + + if (pPrev->IsMax()) + { + // if previous edge is a maximum check the bounds and add an overlap if necessary + const int axis1 = (1 << axis) & 3; + const int axis2 = (1 << axis1) & 3; + if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev,axis1,axis2)) + { + m_pairCache->addOverlappingPair(pHandleEdge,pHandlePrev); + if (m_userPairCallback) + m_userPairCallback->addOverlappingPair(pHandleEdge,pHandlePrev); + + //AddOverlap(pEdge->m_handle, pPrev->m_handle); + + } + + // update edge reference in other handle + pHandlePrev->m_maxEdges[axis]++; + } + else + pHandlePrev->m_minEdges[axis]++; + + pHandleEdge->m_minEdges[axis]--; + + // swap the edges + Edge swap = *pEdge; + *pEdge = *pPrev; + *pPrev = swap; + + // decrement + pEdge--; + pPrev--; + } + +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis); +#endif //DEBUG_BROADPHASE + +} + +// sorting a min edge upwards can only ever *remove* overlaps +template +void btAxisSweep3Internal::sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps) +{ + Edge* pEdge = m_pEdges[axis] + edge; + Edge* pNext = pEdge + 1; + Handle* pHandleEdge = getHandle(pEdge->m_handle); + + while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos)) + { + Handle* pHandleNext = getHandle(pNext->m_handle); + + if (pNext->IsMax()) + { + Handle* handle0 = getHandle(pEdge->m_handle); + Handle* handle1 = getHandle(pNext->m_handle); + const int axis1 = (1 << axis) & 3; + const int axis2 = (1 << axis1) & 3; + + // if next edge is maximum remove any overlap between the two handles + if (updateOverlaps +#ifdef USE_OVERLAP_TEST_ON_REMOVES + && testOverlap2D(handle0,handle1,axis1,axis2) +#endif //USE_OVERLAP_TEST_ON_REMOVES + ) + { + + + m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher); + if (m_userPairCallback) + m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher); + + } + + + // update edge reference in other handle + pHandleNext->m_maxEdges[axis]--; + } + else + pHandleNext->m_minEdges[axis]--; + + pHandleEdge->m_minEdges[axis]++; + + // swap the edges + Edge swap = *pEdge; + *pEdge = *pNext; + *pNext = swap; + + // increment + pEdge++; + pNext++; + } + + +} + +// sorting a max edge downwards can only ever *remove* overlaps +template +void btAxisSweep3Internal::sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps) +{ + + Edge* pEdge = m_pEdges[axis] + edge; + Edge* pPrev = pEdge - 1; + Handle* pHandleEdge = getHandle(pEdge->m_handle); + + while (pEdge->m_pos < pPrev->m_pos) + { + Handle* pHandlePrev = getHandle(pPrev->m_handle); + + if (!pPrev->IsMax()) + { + // if previous edge was a minimum remove any overlap between the two handles + Handle* handle0 = getHandle(pEdge->m_handle); + Handle* handle1 = getHandle(pPrev->m_handle); + const int axis1 = (1 << axis) & 3; + const int axis2 = (1 << axis1) & 3; + + if (updateOverlaps +#ifdef USE_OVERLAP_TEST_ON_REMOVES + && testOverlap2D(handle0,handle1,axis1,axis2) +#endif //USE_OVERLAP_TEST_ON_REMOVES + ) + { + //this is done during the overlappingpairarray iteration/narrowphase collision + + + m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher); + if (m_userPairCallback) + m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher); + + + + } + + // update edge reference in other handle + pHandlePrev->m_minEdges[axis]++;; + } + else + pHandlePrev->m_maxEdges[axis]++; + + pHandleEdge->m_maxEdges[axis]--; + + // swap the edges + Edge swap = *pEdge; + *pEdge = *pPrev; + *pPrev = swap; + + // decrement + pEdge--; + pPrev--; + } + + +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis); +#endif //DEBUG_BROADPHASE + +} + +// sorting a max edge upwards can only ever *add* overlaps +template +void btAxisSweep3Internal::sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps) +{ + Edge* pEdge = m_pEdges[axis] + edge; + Edge* pNext = pEdge + 1; + Handle* pHandleEdge = getHandle(pEdge->m_handle); + + while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos)) + { + Handle* pHandleNext = getHandle(pNext->m_handle); + + const int axis1 = (1 << axis) & 3; + const int axis2 = (1 << axis1) & 3; + + if (!pNext->IsMax()) + { + // if next edge is a minimum check the bounds and add an overlap if necessary + if (updateOverlaps && testOverlap2D(pHandleEdge, pHandleNext,axis1,axis2)) + { + Handle* handle0 = getHandle(pEdge->m_handle); + Handle* handle1 = getHandle(pNext->m_handle); + m_pairCache->addOverlappingPair(handle0,handle1); + if (m_userPairCallback) + m_userPairCallback->addOverlappingPair(handle0,handle1); + } + + // update edge reference in other handle + pHandleNext->m_minEdges[axis]--; + } + else + pHandleNext->m_maxEdges[axis]--; + + pHandleEdge->m_maxEdges[axis]++; + + // swap the edges + Edge swap = *pEdge; + *pEdge = *pNext; + *pNext = swap; + + // increment + pEdge++; + pNext++; + } + +} + +#endif diff --git a/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h index d31b77f46..fb68e0024 100644 --- a/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h +++ b/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h @@ -41,6 +41,10 @@ struct btBroadphaseRayCallback : public btBroadphaseAabbCallback btScalar m_lambda_max; virtual ~btBroadphaseRayCallback() {} + +protected: + + btBroadphaseRayCallback() {} }; #include "LinearMath/btVector3.h" diff --git a/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp b/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp index f4d7341f8..0fd4ef46b 100644 --- a/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp +++ b/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp @@ -15,3 +15,4 @@ subject to the following restrictions: #include "btBroadphaseProxy.h" +BT_NOT_EMPTY_FILE // fix warning LNK4221: This object file does not define any previously undefined public symbols, so it will not be used by any link operation that consumes this library diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h index 9c7b6f813..3e069fa5e 100644 --- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h +++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h @@ -23,6 +23,9 @@ struct btBroadphasePair; ///The btOverlappingPairCallback class is an additional optional broadphase user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache. class btOverlappingPairCallback { +protected: + btOverlappingPairCallback() {} + public: virtual ~btOverlappingPairCallback() { diff --git a/src/BulletCollision/CMakeLists.txt b/src/BulletCollision/CMakeLists.txt index 7c8d0e8fb..67afc7e0c 100644 --- a/src/BulletCollision/CMakeLists.txt +++ b/src/BulletCollision/CMakeLists.txt @@ -102,6 +102,7 @@ SET(Root_HDRS ../btBulletCollisionCommon.h ) SET(BroadphaseCollision_HDRS + BroadphaseCollision/btAxisSweep3Internal.h BroadphaseCollision/btAxisSweep3.h BroadphaseCollision/btBroadphaseInterface.h BroadphaseCollision/btBroadphaseProxy.h @@ -189,12 +190,15 @@ SET(CollisionShapes_HDRS SET(Gimpact_HDRS Gimpact/btBoxCollision.h Gimpact/btClipPolygon.h + Gimpact/btContactProcessingStructs.h Gimpact/btContactProcessing.h Gimpact/btGenericPoolAllocator.h Gimpact/btGeometryOperations.h + Gimpact/btGImpactBvhStructs.h Gimpact/btGImpactBvh.h Gimpact/btGImpactCollisionAlgorithm.h Gimpact/btGImpactMassUtil.h + Gimpact/btGImpactQuantizedBvhStructs.h Gimpact/btGImpactQuantizedBvh.h Gimpact/btGImpactShape.h Gimpact/btQuantization.h diff --git a/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h index 489812b96..0e19f1ea3 100644 --- a/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h @@ -24,12 +24,13 @@ class btActivatingCollisionAlgorithm : public btCollisionAlgorithm // btCollisionObject* m_colObj0; // btCollisionObject* m_colObj1; -public: +protected: btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci); btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); +public: virtual ~btActivatingCollisionAlgorithm(); }; diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h index 06a762f20..f29f7a709 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h @@ -33,8 +33,6 @@ class btDispatcher; class btCollisionObject; class btCollisionShape; -typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1); -extern btShapePairCallback gCompoundCompoundChildShapePairCallback; /// btCompoundCompoundCollisionAlgorithm supports collision between two btCompoundCollisionShape shapes class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index 621292e22..aa3d159f8 100644 --- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -111,6 +111,7 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b return; bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); + bool isNewCollision = m_manifoldPtr->getNumContacts() == 0; btVector3 pointA = pointInWorld + normalOnBInWorld * depth; @@ -193,5 +194,9 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1); } + if (gContactStartedCallback && isNewCollision) + { + gContactStartedCallback(m_manifoldPtr); + } } diff --git a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h index 961d001a9..7bf8e01c1 100644 --- a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h +++ b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h @@ -93,10 +93,12 @@ protected: aabbMax = m_localAabbMax; } -public: +protected: btPolyhedralConvexAabbCachingShape(); +public: + inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const { diff --git a/src/BulletCollision/Gimpact/btContactProcessing.h b/src/BulletCollision/Gimpact/btContactProcessing.h index 0c66f8e10..d1027dbe6 100644 --- a/src/BulletCollision/Gimpact/btContactProcessing.h +++ b/src/BulletCollision/Gimpact/btContactProcessing.h @@ -27,86 +27,7 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" #include "LinearMath/btAlignedObjectArray.h" #include "btTriangleShapeEx.h" - - - -/** -Configuration var for applying interpolation of contact normals -*/ -#define NORMAL_CONTACT_AVERAGE 1 - -#define CONTACT_DIFF_EPSILON 0.00001f - -///The GIM_CONTACT is an internal GIMPACT structure, similar to btManifoldPoint. -///@todo: remove and replace GIM_CONTACT by btManifoldPoint. -class GIM_CONTACT -{ -public: - btVector3 m_point; - btVector3 m_normal; - btScalar m_depth;//Positive value indicates interpenetration - btScalar m_distance;//Padding not for use - int m_feature1;//Face number - int m_feature2;//Face number -public: - GIM_CONTACT() - { - } - - GIM_CONTACT(const GIM_CONTACT & contact): - m_point(contact.m_point), - m_normal(contact.m_normal), - m_depth(contact.m_depth), - m_feature1(contact.m_feature1), - m_feature2(contact.m_feature2) - { - } - - GIM_CONTACT(const btVector3 &point,const btVector3 & normal, - btScalar depth, int feature1, int feature2): - m_point(point), - m_normal(normal), - m_depth(depth), - m_feature1(feature1), - m_feature2(feature2) - { - } - - //! Calcs key for coord classification - SIMD_FORCE_INLINE unsigned int calc_key_contact() const - { - int _coords[] = { - (int)(m_point[0]*1000.0f+1.0f), - (int)(m_point[1]*1333.0f), - (int)(m_point[2]*2133.0f+3.0f)}; - unsigned int _hash=0; - unsigned int *_uitmp = (unsigned int *)(&_coords[0]); - _hash = *_uitmp; - _uitmp++; - _hash += (*_uitmp)<<4; - _uitmp++; - _hash += (*_uitmp)<<8; - return _hash; - } - - SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,int normal_count) - { - btVector3 vec_sum(m_normal); - for(int i=0;i { @@ -141,5 +62,4 @@ public: void merge_contacts_unique(const btContactArray & contacts); }; - #endif // GIM_CONTACT_H_INCLUDED diff --git a/src/BulletCollision/Gimpact/btContactProcessingStructs.h b/src/BulletCollision/Gimpact/btContactProcessingStructs.h new file mode 100644 index 000000000..efbc4a567 --- /dev/null +++ b/src/BulletCollision/Gimpact/btContactProcessingStructs.h @@ -0,0 +1,109 @@ +#ifndef BT_CONTACT_H_STRUCTS_INCLUDED +#define BT_CONTACT_H_STRUCTS_INCLUDED + +/*! \file gim_contact.h +\author Francisco Leon Najera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 "LinearMath/btTransform.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "btTriangleShapeEx.h" + + +/** +Configuration var for applying interpolation of contact normals +*/ +#define NORMAL_CONTACT_AVERAGE 1 + +#define CONTACT_DIFF_EPSILON 0.00001f + +///The GIM_CONTACT is an internal GIMPACT structure, similar to btManifoldPoint. +///@todo: remove and replace GIM_CONTACT by btManifoldPoint. +class GIM_CONTACT +{ +public: + btVector3 m_point; + btVector3 m_normal; + btScalar m_depth;//Positive value indicates interpenetration + btScalar m_distance;//Padding not for use + int m_feature1;//Face number + int m_feature2;//Face number +public: + GIM_CONTACT() + { + } + + GIM_CONTACT(const GIM_CONTACT & contact): + m_point(contact.m_point), + m_normal(contact.m_normal), + m_depth(contact.m_depth), + m_feature1(contact.m_feature1), + m_feature2(contact.m_feature2) + { + } + + GIM_CONTACT(const btVector3 &point,const btVector3 & normal, + btScalar depth, int feature1, int feature2): + m_point(point), + m_normal(normal), + m_depth(depth), + m_feature1(feature1), + m_feature2(feature2) + { + } + + //! Calcs key for coord classification + SIMD_FORCE_INLINE unsigned int calc_key_contact() const + { + int _coords[] = { + (int)(m_point[0]*1000.0f+1.0f), + (int)(m_point[1]*1333.0f), + (int)(m_point[2]*2133.0f+3.0f)}; + unsigned int _hash=0; + unsigned int *_uitmp = (unsigned int *)(&_coords[0]); + _hash = *_uitmp; + _uitmp++; + _hash += (*_uitmp)<<4; + _uitmp++; + _hash += (*_uitmp)<<8; + return _hash; + } + + SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,int normal_count) + { + btVector3 vec_sum(m_normal); + for(int i=0;i @@ -74,59 +50,6 @@ public: } }; - -///GIM_BVH_DATA is an internal GIMPACT collision structure to contain axis aligned bounding box -struct GIM_BVH_DATA -{ - btAABB m_bound; - int m_data; -}; - -//! Node Structure for trees -class GIM_BVH_TREE_NODE -{ -public: - btAABB m_bound; -protected: - int m_escapeIndexOrDataIndex; -public: - GIM_BVH_TREE_NODE() - { - m_escapeIndexOrDataIndex = 0; - } - - SIMD_FORCE_INLINE bool isLeafNode() const - { - //skipindex is negative (internal node), triangleindex >=0 (leafnode) - return (m_escapeIndexOrDataIndex>=0); - } - - SIMD_FORCE_INLINE int getEscapeIndex() const - { - //btAssert(m_escapeIndexOrDataIndex < 0); - return -m_escapeIndexOrDataIndex; - } - - SIMD_FORCE_INLINE void setEscapeIndex(int index) - { - m_escapeIndexOrDataIndex = -index; - } - - SIMD_FORCE_INLINE int getDataIndex() const - { - //btAssert(m_escapeIndexOrDataIndex >= 0); - - return m_escapeIndexOrDataIndex; - } - - SIMD_FORCE_INLINE void setDataIndex(int index) - { - m_escapeIndexOrDataIndex = index; - } - -}; - - class GIM_BVH_DATA_ARRAY:public btAlignedObjectArray { }; @@ -392,5 +315,4 @@ public: btPairSet & collision_pairs); }; - #endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/src/BulletCollision/Gimpact/btGImpactBvhStructs.h b/src/BulletCollision/Gimpact/btGImpactBvhStructs.h new file mode 100644 index 000000000..9342a572d --- /dev/null +++ b/src/BulletCollision/Gimpact/btGImpactBvhStructs.h @@ -0,0 +1,105 @@ +#ifndef GIM_BOX_SET_STRUCT_H_INCLUDED +#define GIM_BOX_SET_STRUCT_H_INCLUDED + +/*! \file gim_box_set.h +\author Francisco Leon Najera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 "LinearMath/btAlignedObjectArray.h" + +#include "btBoxCollision.h" +#include "btTriangleShapeEx.h" + +//! Overlapping pair +struct GIM_PAIR +{ + int m_index1; + int m_index2; + GIM_PAIR() + {} + + GIM_PAIR(const GIM_PAIR & p) + { + m_index1 = p.m_index1; + m_index2 = p.m_index2; + } + + GIM_PAIR(int index1, int index2) + { + m_index1 = index1; + m_index2 = index2; + } +}; + +///GIM_BVH_DATA is an internal GIMPACT collision structure to contain axis aligned bounding box +struct GIM_BVH_DATA +{ + btAABB m_bound; + int m_data; +}; + +//! Node Structure for trees +class GIM_BVH_TREE_NODE +{ +public: + btAABB m_bound; +protected: + int m_escapeIndexOrDataIndex; +public: + GIM_BVH_TREE_NODE() + { + m_escapeIndexOrDataIndex = 0; + } + + SIMD_FORCE_INLINE bool isLeafNode() const + { + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (m_escapeIndexOrDataIndex>=0); + } + + SIMD_FORCE_INLINE int getEscapeIndex() const + { + //btAssert(m_escapeIndexOrDataIndex < 0); + return -m_escapeIndexOrDataIndex; + } + + SIMD_FORCE_INLINE void setEscapeIndex(int index) + { + m_escapeIndexOrDataIndex = -index; + } + + SIMD_FORCE_INLINE int getDataIndex() const + { + //btAssert(m_escapeIndexOrDataIndex >= 0); + + return m_escapeIndexOrDataIndex; + } + + SIMD_FORCE_INLINE void setDataIndex(int index) + { + m_escapeIndexOrDataIndex = index; + } + +}; + +#endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h b/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h index e6e52fff4..42e5520fc 100644 --- a/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h +++ b/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h @@ -26,73 +26,7 @@ subject to the following restrictions: #include "btGImpactBvh.h" #include "btQuantization.h" - - - - - -///btQuantizedBvhNode is a compressed aabb node, 16 bytes. -///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). -ATTRIBUTE_ALIGNED16 (struct) BT_QUANTIZED_BVH_NODE -{ - //12 bytes - unsigned short int m_quantizedAabbMin[3]; - unsigned short int m_quantizedAabbMax[3]; - //4 bytes - int m_escapeIndexOrDataIndex; - - BT_QUANTIZED_BVH_NODE() - { - m_escapeIndexOrDataIndex = 0; - } - - SIMD_FORCE_INLINE bool isLeafNode() const - { - //skipindex is negative (internal node), triangleindex >=0 (leafnode) - return (m_escapeIndexOrDataIndex>=0); - } - - SIMD_FORCE_INLINE int getEscapeIndex() const - { - //btAssert(m_escapeIndexOrDataIndex < 0); - return -m_escapeIndexOrDataIndex; - } - - SIMD_FORCE_INLINE void setEscapeIndex(int index) - { - m_escapeIndexOrDataIndex = -index; - } - - SIMD_FORCE_INLINE int getDataIndex() const - { - //btAssert(m_escapeIndexOrDataIndex >= 0); - - return m_escapeIndexOrDataIndex; - } - - SIMD_FORCE_INLINE void setDataIndex(int index) - { - m_escapeIndexOrDataIndex = index; - } - - SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp( - unsigned short * quantizedMin,unsigned short * quantizedMax) const - { - if(m_quantizedAabbMin[0] > quantizedMax[0] || - m_quantizedAabbMax[0] < quantizedMin[0] || - m_quantizedAabbMin[1] > quantizedMax[1] || - m_quantizedAabbMax[1] < quantizedMin[1] || - m_quantizedAabbMin[2] > quantizedMax[2] || - m_quantizedAabbMax[2] < quantizedMin[2]) - { - return false; - } - return true; - } - -}; - - +#include "btGImpactQuantizedBvhStructs.h" class GIM_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray { @@ -368,5 +302,4 @@ public: btPairSet & collision_pairs); }; - #endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h b/src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h new file mode 100644 index 000000000..7dd5a1b9d --- /dev/null +++ b/src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h @@ -0,0 +1,91 @@ +#ifndef GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED +#define GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED + +/*! \file btGImpactQuantizedBvh.h +\author Francisco Leon Najera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +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 "btGImpactBvh.h" +#include "btQuantization.h" + +///btQuantizedBvhNode is a compressed aabb node, 16 bytes. +///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). +ATTRIBUTE_ALIGNED16 (struct) BT_QUANTIZED_BVH_NODE +{ + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes + int m_escapeIndexOrDataIndex; + + BT_QUANTIZED_BVH_NODE() + { + m_escapeIndexOrDataIndex = 0; + } + + SIMD_FORCE_INLINE bool isLeafNode() const + { + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (m_escapeIndexOrDataIndex>=0); + } + + SIMD_FORCE_INLINE int getEscapeIndex() const + { + //btAssert(m_escapeIndexOrDataIndex < 0); + return -m_escapeIndexOrDataIndex; + } + + SIMD_FORCE_INLINE void setEscapeIndex(int index) + { + m_escapeIndexOrDataIndex = -index; + } + + SIMD_FORCE_INLINE int getDataIndex() const + { + //btAssert(m_escapeIndexOrDataIndex >= 0); + + return m_escapeIndexOrDataIndex; + } + + SIMD_FORCE_INLINE void setDataIndex(int index) + { + m_escapeIndexOrDataIndex = index; + } + + SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp( + unsigned short * quantizedMin,unsigned short * quantizedMax) const + { + if(m_quantizedAabbMin[0] > quantizedMax[0] || + m_quantizedAabbMax[0] < quantizedMin[0] || + m_quantizedAabbMin[1] > quantizedMax[1] || + m_quantizedAabbMax[1] < quantizedMin[1] || + m_quantizedAabbMin[2] > quantizedMax[2] || + m_quantizedAabbMax[2] < quantizedMin[2]) + { + return false; + } + return true; + } + +}; + +#endif // GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED diff --git a/src/BulletCollision/Gimpact/gim_array.h b/src/BulletCollision/Gimpact/gim_array.h index 27e6f32fc..cda51a5fc 100644 --- a/src/BulletCollision/Gimpact/gim_array.h +++ b/src/BulletCollision/Gimpact/gim_array.h @@ -228,7 +228,7 @@ public: inline void push_back_memcpy(const T & obj) { this->growingCheck(); - irr_simd_memcpy(&m_data[m_size],&obj,sizeof(T)); + gim_simd_memcpy(&m_data[m_size],&obj,sizeof(T)); m_size++; } diff --git a/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h b/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h index d98051da3..0c48cb60f 100644 --- a/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h +++ b/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h @@ -41,10 +41,13 @@ email: projectileman@yahoo.com - +#ifndef PLANEDIREPSILON #define PLANEDIREPSILON 0.0000001f -#define PARALELENORMALS 0.000001f +#endif +#ifndef PARALELENORMALS +#define PARALELENORMALS 0.000001f +#endif #define TRIANGLE_NORMAL(v1,v2,v3,n)\ {\ diff --git a/src/BulletCollision/Gimpact/gim_box_collision.h b/src/BulletCollision/Gimpact/gim_box_collision.h index 9c572638a..a051b4fdb 100644 --- a/src/BulletCollision/Gimpact/gim_box_collision.h +++ b/src/BulletCollision/Gimpact/gim_box_collision.h @@ -97,6 +97,8 @@ email: projectileman@yahoo.com // return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1); //} +#ifndef TEST_CROSS_EDGE_BOX_MCR + #define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\ {\ const btScalar dir0 = -edge[i_dir_0];\ @@ -113,6 +115,7 @@ email: projectileman@yahoo.com if(pmin>rad || -rad>pmax) return false;\ }\ +#endif #define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ {\ @@ -190,8 +193,9 @@ public: } }; - +#ifndef BOX_PLANE_EPSILON #define BOX_PLANE_EPSILON 0.000001f +#endif //! Axis aligned box class GIM_AABB @@ -571,7 +575,7 @@ public: } }; - +#ifndef BT_BOX_COLLISION_H_INCLUDED //! Compairison of transformation objects SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2) { @@ -582,6 +586,7 @@ SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btT if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false; return true; } +#endif diff --git a/src/BulletCollision/Gimpact/gim_contact.h b/src/BulletCollision/Gimpact/gim_contact.h index 5d9f8ef81..b41c714b5 100644 --- a/src/BulletCollision/Gimpact/gim_contact.h +++ b/src/BulletCollision/Gimpact/gim_contact.h @@ -40,8 +40,15 @@ email: projectileman@yahoo.com /** Configuration var for applying interpolation of contact normals */ +#ifndef NORMAL_CONTACT_AVERAGE #define NORMAL_CONTACT_AVERAGE 1 +#endif + +#ifndef CONTACT_DIFF_EPSILON #define CONTACT_DIFF_EPSILON 0.00001f +#endif + +#ifndef BT_CONTACT_H_STRUCTS_INCLUDED /// Structure for collision results ///Functions for managing and sorting contacts resulting from a collision query. @@ -121,6 +128,7 @@ public: }; +#endif class gim_contact_array:public gim_array { diff --git a/src/BulletCollision/Gimpact/gim_tri_collision.h b/src/BulletCollision/Gimpact/gim_tri_collision.h index 5b552a1ed..267f806e7 100644 --- a/src/BulletCollision/Gimpact/gim_tri_collision.h +++ b/src/BulletCollision/Gimpact/gim_tri_collision.h @@ -38,8 +38,9 @@ email: projectileman@yahoo.com - +#ifndef MAX_TRI_CLIPPING #define MAX_TRI_CLIPPING 16 +#endif //! Structure for collision struct GIM_TRIANGLE_CONTACT_DATA diff --git a/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h index 46ce1ab75..0ea7b483c 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h +++ b/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h @@ -67,10 +67,12 @@ struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result btVector3 m_closestPointInB; btScalar m_distance; //negative means penetration ! + protected: btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT)) { - } + + public: virtual ~btStorageResult() {}; virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index c5bd7c16f..23aaece22 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -21,6 +21,8 @@ subject to the following restrictions: btScalar gContactBreakingThreshold = btScalar(0.02); ContactDestroyedCallback gContactDestroyedCallback = 0; ContactProcessedCallback gContactProcessedCallback = 0; +ContactStartedCallback gContactStartedCallback = 0; +ContactEndedCallback gContactEndedCallback = 0; ///gContactCalcArea3Points will approximate the convex hull area using 3 points ///when setting it to false, it will use 4 points to compute the area: it is more accurate but slower bool gContactCalcArea3Points = true; diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index a351cb3c2..f872c8e1c 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -28,10 +28,18 @@ struct btCollisionResult; ///maximum contact breaking and merging threshold extern btScalar gContactBreakingThreshold; +#ifndef SWIG +class btPersistentManifold; + typedef bool (*ContactDestroyedCallback)(void* userPersistentData); typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp,void* body0,void* body1); +typedef void (*ContactStartedCallback)(btPersistentManifold* const &manifold); +typedef void (*ContactEndedCallback)(btPersistentManifold* const &manifold); extern ContactDestroyedCallback gContactDestroyedCallback; extern ContactProcessedCallback gContactProcessedCallback; +extern ContactStartedCallback gContactStartedCallback; +extern ContactEndedCallback gContactEndedCallback; +#endif //SWIG //the enum starts at 1024 to avoid type conflicts with btTypedConstraint enum btContactManifoldTypes @@ -171,6 +179,11 @@ public: btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0); m_cachedPoints--; + + if (gContactEndedCallback && m_cachedPoints == 0) + { + gContactEndedCallback(this); + } } void replaceContactPoint(const btManifoldPoint& newPoint, int insertIndex) { @@ -235,6 +248,11 @@ public: { clearUserCache(m_pointCache[i]); } + + if (gContactEndedCallback && m_cachedPoints) + { + gContactEndedCallback(this); + } m_cachedPoints = 0; } diff --git a/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/src/BulletDynamics/ConstraintSolver/btContactConstraint.h index 477c79d17..adb226835 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btContactConstraint.h @@ -28,11 +28,13 @@ protected: btPersistentManifold m_contactManifold; -public: +protected: btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB); +public: + void setContactManifold(btPersistentManifold* contactManifold); btPersistentManifold* getContactManifold() diff --git a/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/src/BulletDynamics/Vehicle/btRaycastVehicle.h index 82d44c73e..04656b912 100644 --- a/src/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -19,7 +19,7 @@ class btDynamicsWorld; #include "btWheelInfo.h" #include "BulletDynamics/Dynamics/btActionInterface.h" -class btVehicleTuning; +//class btVehicleTuning; ///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle. class btRaycastVehicle : public btActionInterface diff --git a/src/BulletDynamics/Vehicle/btWheelInfo.h b/src/BulletDynamics/Vehicle/btWheelInfo.h index f916053ec..f991a57b6 100644 --- a/src/BulletDynamics/Vehicle/btWheelInfo.h +++ b/src/BulletDynamics/Vehicle/btWheelInfo.h @@ -79,6 +79,8 @@ struct btWheelInfo void* m_clientInfo;//can be used to store pointer to sync transforms... + btWheelInfo() {} + btWheelInfo(btWheelInfoConstructionInfo& ci) { diff --git a/src/BulletInverseDynamics/IDConfig.hpp b/src/BulletInverseDynamics/IDConfig.hpp index 08bbdcb70..ebb10e7a1 100644 --- a/src/BulletInverseDynamics/IDConfig.hpp +++ b/src/BulletInverseDynamics/IDConfig.hpp @@ -68,6 +68,10 @@ typedef btScalar idScalar; #ifdef BT_USE_DOUBLE_PRECISION #define BT_ID_USE_DOUBLE_PRECISION #endif + +#ifndef BT_USE_INVERSE_DYNAMICS_WITH_BULLET2 + + // use bullet types for arrays and array indices #include "Bullet3Common/b3AlignedObjectArray.h" // this is to make it work with C++2003, otherwise we could do this: @@ -78,7 +82,20 @@ struct idArray { typedef b3AlignedObjectArray type; }; typedef int idArrayIdx; -#define ID_DECLARE_ALIGNED_ALLOCATOR B3_DECLARE_ALIGNED_ALLOCATOR +#define ID_DECLARE_ALIGNED_ALLOCATOR() B3_DECLARE_ALIGNED_ALLOCATOR() + +#else // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2 + +#include "LinearMath/btAlignedObjectArray.h" +template +struct idArray { + typedef btAlignedObjectArray type; +}; +typedef int idArrayIdx; +#define ID_DECLARE_ALIGNED_ALLOCATOR() BT_DECLARE_ALIGNED_ALLOCATOR() + +#endif // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2 + // use bullet's allocator functions #define idMalloc btAllocFunc diff --git a/src/BulletInverseDynamics/IDErrorMessages.hpp b/src/BulletInverseDynamics/IDErrorMessages.hpp index a3866edc5..1dc22f860 100644 --- a/src/BulletInverseDynamics/IDErrorMessages.hpp +++ b/src/BulletInverseDynamics/IDErrorMessages.hpp @@ -5,7 +5,7 @@ /// name of file being compiled, without leading path components #define __INVDYN_FILE_WO_DIR__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__) -#ifndef BT_ID_WO_BULLET +#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__) @@ -24,11 +24,6 @@ fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ fprintf(stderr, __VA_ARGS__); \ } while (0) -#define warning_message(...) \ - do { \ - fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ - fprintf(stderr, __VA_ARGS__); \ - } while (0) #define id_printf(...) printf(__VA_ARGS__) #endif // BT_ID_WO_BULLET #endif diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 11ef57bee..ada0dfd1a 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -617,7 +617,7 @@ public: RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt); void Process(const btDbvtNode* leaf); - static inline btScalar rayFromToTriangle(const btVector3& rayFrom, + static /*inline*/ btScalar rayFromToTriangle(const btVector3& rayFrom, const btVector3& rayTo, const btVector3& rayNormalizedDirection, const btVector3& a, diff --git a/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h index 7e3ab329f..6d46a21db 100644 --- a/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h @@ -20,7 +20,9 @@ subject to the following restrictions: #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "BulletSoftBody/btSoftBody.h" +#ifndef BT_SOFT_RIGID_DYNAMICS_WORLD_H typedef btAlignedObjectArray btSoftBodyArray; +#endif class btSoftBodySolver; diff --git a/src/LinearMath/btQuickprof.h b/src/LinearMath/btQuickprof.h index fd0030715..7b38d71b9 100644 --- a/src/LinearMath/btQuickprof.h +++ b/src/LinearMath/btQuickprof.h @@ -65,9 +65,11 @@ btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc(); void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc); void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc); - +#ifndef BT_NO_PROFILE // FIX redefinition //To disable built-in profiling, please comment out next line //#define BT_NO_PROFILE 1 +#endif //BT_NO_PROFILE + #ifndef BT_NO_PROFILE //btQuickprofGetCurrentThreadIndex will return -1 if thread index cannot be determined, //otherwise returns thread index in range [0..maxThreads] diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index de9fddd3f..174205c7f 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -32,6 +32,28 @@ inline int btGetVersion() return BT_BULLET_VERSION; } + +// The following macro "BT_NOT_EMPTY_FILE" can be put into a file +// in order suppress the MS Visual C++ Linker warning 4221 +// +// warning LNK4221: no public symbols found; archive member will be inaccessible +// +// This warning occurs on PC and XBOX when a file compiles out completely +// has no externally visible symbols which may be dependant on configuration +// #defines and options. +// +// see more https://stackoverflow.com/questions/1822887/what-is-the-best-way-to-eliminate-ms-visual-c-linker-warning-warning-lnk422 + +#if defined (_MSC_VER) + #define BT_NOT_EMPTY_FILE_CAT_II(p, res) res + #define BT_NOT_EMPTY_FILE_CAT_I(a, b) BT_NOT_EMPTY_FILE_CAT_II(~, a ## b) + #define BT_NOT_EMPTY_FILE_CAT(a, b) BT_NOT_EMPTY_FILE_CAT_I(a, b) + #define BT_NOT_EMPTY_FILE namespace { char BT_NOT_EMPTY_FILE_CAT(NoEmptyFileDummy, __COUNTER__); } +#else + #define BT_NOT_EMPTY_FILE +#endif + + // clang and most formatting tools don't support indentation of preprocessor guards, so turn it off // clang-format off #if defined(DEBUG) || defined (_DEBUG)