From 21b7a4712971d83d69627135f72714ae685cd123 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 17 Oct 2015 18:52:48 -0700 Subject: [PATCH] although still not implemented, IMU enum had wrong value, thanks JF --- examples/Collision/CollisionSdkC_Api.cpp | 76 ++++--------------- examples/Collision/CollisionSdkC_Api.h | 6 +- .../Collision/CollisionTutorialBullet2.cpp | 35 +++++++-- .../Internal/Bullet2CollisionSdk.cpp | 73 +++++++++++++++++- .../Collision/Internal/Bullet2CollisionSdk.h | 10 ++- .../Internal/CollisionSdkInterface.h | 9 ++- examples/SharedMemory/SharedMemoryCommands.h | 2 +- 7 files changed, 136 insertions(+), 75 deletions(-) diff --git a/examples/Collision/CollisionSdkC_Api.cpp b/examples/Collision/CollisionSdkC_Api.cpp index b061da845..0fd2fded1 100644 --- a/examples/Collision/CollisionSdkC_Api.cpp +++ b/examples/Collision/CollisionSdkC_Api.cpp @@ -40,10 +40,10 @@ void plDeleteShape(plCollisionSdkHandle collisionSdkHandle, plCollisionShapeHand sdk->deleteShape(shapeHandle); } -plCollisionObjectHandle plCreateCollisionObject( plCollisionSdkHandle collisionSdkHandle, void* user_data, plCollisionShapeHandle cshape ) +plCollisionObjectHandle plCreateCollisionObject( plCollisionSdkHandle collisionSdkHandle, void* user_data, plCollisionShapeHandle cshape ,plVector3 childPos,plQuaternion childOrn) { CollisionSdkInterface* sdk = (CollisionSdkInterface*) collisionSdkHandle; - return sdk->createCollisionObject(user_data, cshape); + return sdk->createCollisionObject(user_data, cshape, childPos, childOrn); } @@ -64,63 +64,17 @@ void plRemoveCollisionObject(plCollisionSdkHandle collisionSdkHandle, plCollisio sdk->removeCollisionObject(world,object); } +/* Collision Queries */ +int plCollide(plCollisionSdkHandle collisionSdkHandle, plCollisionWorldHandle worldHandle, plCollisionObjectHandle colA, plCollisionObjectHandle colB, + lwContactPoint* pointsOut, int pointCapacity) +{ + CollisionSdkInterface* sdk = (CollisionSdkInterface*) collisionSdkHandle; + return sdk->collide(worldHandle, colA,colB,pointsOut,pointCapacity); +} -//plCollisionSdkHandle plCreateRealTimeBullet3CollisionSdk(); -//plCollisionSdkHandle plCreateCustomCollisionSdk(); - - -#if 0 - extern void plDeleteCollisionWorld(plCollisionWorldHandle world); - - - extern void plAddCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object); - extern void plRemoveCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object); - - - /* Collision Object */ - - extern plCollisionObjectHandle plCreateCollisionObject( plCollisionSdkHandle sdk, void* user_data, plCollisionShapeHandle cshape ); - extern void plDeleteCollisionObject(plCollisionSdkHandle sdk, plCollisionObjectHandle body); - - - /* Collision Shape definition */ - - extern plCollisionShapeHandle plNewSphereShape(plCollisionSdkHandle sdk, plReal radius); - extern plCollisionShapeHandle plNewCapsuleShape(plCollisionSdkHandle sdk, plReal radius, plReal height); - extern plCollisionShapeHandle plNewPlaneShape(plCollisionSdkHandle sdk, plReal planeNormalX, - plReal planeNormalY, - plReal planeNormalZ, - plReal planeConstant); - extern plCollisionShapeHandle plNewCompoundShape(plCollisionSdkHandle sdk); - extern void plAddChildShape(plCollisionShapeHandle compoundShape,plCollisionShapeHandle childShape, plVector3 childPos,plQuaternion childOrn); - - extern void plDeleteShape(plCollisionShapeHandle shape); - - - - /* Contact Results */ - - struct lwContactPoint - { - plVector3 m_ptOnAWorld; - plVector3 m_ptOnBWorld; - plVector3 m_normalOnB; - plReal m_distance; - }; - - /* Collision Filtering */ - typedef void(*plNearCallback)(plCollisionSdkHandle sdk, void* userData, plCollisionObjectHandle objA, plCollisionObjectHandle objB); - - - /* Collision Queries */ - extern int plCollide(plCollisionSdkHandle sdk, plCollisionObjectHandle colA, plCollisionObjectHandle colB, - lwContactPoint* pointsOut, int pointCapacity); - - extern void plWorldCollide(plCollisionWorldHandle world, - plNearCallback filter, void* userData); - - - - -#endif - +void plWorldCollide(plCollisionSdkHandle collisionSdkHandle, plCollisionWorldHandle world, + plNearCallback filter, void* userData) +{ + CollisionSdkInterface* sdk = (CollisionSdkInterface*) collisionSdkHandle; + sdk->collideWorld(world,filter,userData); +} diff --git a/examples/Collision/CollisionSdkC_Api.h b/examples/Collision/CollisionSdkC_Api.h index 86bfb518c..25916c019 100644 --- a/examples/Collision/CollisionSdkC_Api.h +++ b/examples/Collision/CollisionSdkC_Api.h @@ -51,7 +51,7 @@ extern "C" { /* Collision Object */ - extern plCollisionObjectHandle plCreateCollisionObject( plCollisionSdkHandle sdkHandle, void* user_data, plCollisionShapeHandle cshape ); + extern plCollisionObjectHandle plCreateCollisionObject( plCollisionSdkHandle sdkHandle, void* user_data, plCollisionShapeHandle cshape , plVector3 startPosition,plQuaternion startOrientation); extern void plDeleteCollisionObject(plCollisionSdkHandle sdkHandle, plCollisionObjectHandle body); @@ -85,10 +85,10 @@ extern "C" { /* Collision Queries */ - extern int plCollide(plCollisionSdkHandle sdk, plCollisionObjectHandle colA, plCollisionObjectHandle colB, + extern int plCollide(plCollisionSdkHandle sdkHandle, plCollisionWorldHandle worldHandle, plCollisionObjectHandle colA, plCollisionObjectHandle colB, lwContactPoint* pointsOut, int pointCapacity); - extern void plWorldCollide(plCollisionWorldHandle world, + extern void plWorldCollide(plCollisionSdkHandle sdkHandle, plCollisionWorldHandle world, plNearCallback filter, void* userData); diff --git a/examples/Collision/CollisionTutorialBullet2.cpp b/examples/Collision/CollisionTutorialBullet2.cpp index 3d913b4e3..69fc62846 100644 --- a/examples/Collision/CollisionTutorialBullet2.cpp +++ b/examples/Collision/CollisionTutorialBullet2.cpp @@ -17,6 +17,13 @@ #include "CollisionSdkC_Api.h" +static int myCounter=0; + +void myNearCallback(plCollisionSdkHandle sdk, void* userData, plCollisionObjectHandle objA, plCollisionObjectHandle objB) +{ + myCounter++; +} + class CollisionTutorialBullet2 : public CommonExampleInterface { CommonGraphicsApp* m_app; @@ -59,11 +66,29 @@ public: float radius = 1.f; plCollisionShapeHandle colShape = plCreateSphereShape(m_collisionSdkHandle, radius); void* userData = 0; - plCollisionObjectHandle colObj = plCreateCollisionObject(m_collisionSdkHandle,userData,colShape); - plAddCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle,colObj); - plRemoveCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,colObj); - plDeleteCollisionObject(m_collisionSdkHandle,colObj); - plDeleteShape(m_collisionSdkHandle,colShape); + btAlignedObjectArray colliders; + + for (int i=0;i<3;i++) + { + btVector3 pos(0,i*1,0); + btQuaternion orn(0,0,0,1); + plCollisionObjectHandle colObj = plCreateCollisionObject(m_collisionSdkHandle,userData,colShape,pos,orn); + colliders.push_back(colObj); + plAddCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle,colObj); + } + lwContactPoint pointsOut[10]; + int pointCapacity=10; + + int numContacts = plCollide(m_collisionSdkHandle,m_collisionWorldHandle,colliders[0],colliders[1],pointsOut,pointCapacity); + printf("numContacts = %d\n", numContacts); + void* myUserPtr = 0; + myCounter = 0; + plWorldCollide(m_collisionSdkHandle,m_collisionWorldHandle,myNearCallback, myUserPtr); + printf("myCounter=%d\n",myCounter); + + //plRemoveCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,colObj); + //plDeleteCollisionObject(m_collisionSdkHandle,colObj); + //plDeleteShape(m_collisionSdkHandle,colShape); } /* diff --git a/examples/Collision/Internal/Bullet2CollisionSdk.cpp b/examples/Collision/Internal/Bullet2CollisionSdk.cpp index 06b3a9b2b..f93f08593 100644 --- a/examples/Collision/Internal/Bullet2CollisionSdk.cpp +++ b/examples/Collision/Internal/Bullet2CollisionSdk.cpp @@ -91,7 +91,9 @@ void Bullet2CollisionSdk::removeCollisionObject(plCollisionWorldHandle worldHand } } -plCollisionObjectHandle Bullet2CollisionSdk::createCollisionObject( void* user_data, plCollisionShapeHandle shapeHandle ) +plCollisionObjectHandle Bullet2CollisionSdk::createCollisionObject( void* user_data, plCollisionShapeHandle shapeHandle , + plVector3 startPosition,plQuaternion startOrientation ) + { btCollisionShape* colShape = (btCollisionShape*) shapeHandle; btAssert(colShape); @@ -99,7 +101,10 @@ plCollisionObjectHandle Bullet2CollisionSdk::createCollisionObject( void* user_ { btCollisionObject* colObj= new btCollisionObject; colObj->setCollisionShape(colShape); - colObj->setWorldTransform(btTransform::getIdentity()); + btTransform tr; + tr.setOrigin(btVector3(startPosition[0],startPosition[1],startPosition[2])); + tr.setRotation(btQuaternion(startOrientation[0],startOrientation[1],startOrientation[2],startOrientation[3])); + colObj->setWorldTransform(tr); return (plCollisionObjectHandle) colObj; } return 0; @@ -111,6 +116,70 @@ void Bullet2CollisionSdk::deleteCollisionObject(plCollisionObjectHandle bodyHand delete colObj; } +struct Bullet2ContactResultCallback : public btCollisionWorld::ContactResultCallback +{ + int m_numContacts; + lwContactPoint* m_pointsOut; + int m_pointCapacity; + + Bullet2ContactResultCallback() :m_numContacts(0) + { + } + virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1) + { + if (m_numContactsm_collisionWorld && colObjA && colObjB) + { + Bullet2ContactResultCallback cb; + world->contactPairTest(colObjA,colObjB,cb); + return cb.m_numContacts; + } + return 0; +} + +static plNearCallback gTmpFilter; +static int gContactCount = 0; + +void Bullet2NearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo) +{ + if (gTmpFilter) + { + gContactCount++; + } +} + +void Bullet2CollisionSdk::collideWorld( plCollisionWorldHandle worldHandle, + plNearCallback filter, void* userData) +{ + btCollisionWorld* world = (btCollisionWorld*) worldHandle; + //chain the near-callback + gTmpFilter = filter; + gContactCount = 0; + + m_internalData->m_dispatcher->setNearCallback(Bullet2NearCallback); + world->performDiscreteCollisionDetection(); + gTmpFilter = 0; +} plCollisionSdkHandle Bullet2CollisionSdk::createBullet2SdkHandle() { diff --git a/examples/Collision/Internal/Bullet2CollisionSdk.h b/examples/Collision/Internal/Bullet2CollisionSdk.h index 422c3622f..a2e9df7d2 100644 --- a/examples/Collision/Internal/Bullet2CollisionSdk.h +++ b/examples/Collision/Internal/Bullet2CollisionSdk.h @@ -24,10 +24,16 @@ public: virtual void addCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object); virtual void removeCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object); - virtual plCollisionObjectHandle createCollisionObject( void* user_data, plCollisionShapeHandle cshape ); - virtual void deleteCollisionObject(plCollisionObjectHandle body); + virtual plCollisionObjectHandle createCollisionObject( void* user_data, plCollisionShapeHandle cshape , + plVector3 startPosition,plQuaternion startOrientation ); + virtual void deleteCollisionObject(plCollisionObjectHandle body); + virtual int collide(plCollisionWorldHandle world,plCollisionObjectHandle colA, plCollisionObjectHandle colB, + lwContactPoint* pointsOut, int pointCapacity); + virtual void collideWorld( plCollisionWorldHandle world, + plNearCallback filter, void* userData); + static plCollisionSdkHandle createBullet2SdkHandle(); }; diff --git a/examples/Collision/Internal/CollisionSdkInterface.h b/examples/Collision/Internal/CollisionSdkInterface.h index b4cf425bf..a995203ae 100644 --- a/examples/Collision/Internal/CollisionSdkInterface.h +++ b/examples/Collision/Internal/CollisionSdkInterface.h @@ -22,8 +22,15 @@ public: virtual void addCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object)=0; virtual void removeCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object)=0; - virtual plCollisionObjectHandle createCollisionObject( void* user_data, plCollisionShapeHandle cshape )=0; + virtual plCollisionObjectHandle createCollisionObject( void* user_data, plCollisionShapeHandle cshape , + plVector3 startPosition,plQuaternion startOrientation )=0; virtual void deleteCollisionObject(plCollisionObjectHandle body)=0; + + virtual int collide(plCollisionWorldHandle world,plCollisionObjectHandle colA, plCollisionObjectHandle colB, + lwContactPoint* pointsOut, int pointCapacity)=0; + + virtual void collideWorld( plCollisionWorldHandle world, + plNearCallback filter, void* userData)=0; }; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 28894199c..b009dc760 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -178,7 +178,7 @@ struct SendActualStateArgs enum EnumSensorTypes { SENSOR_FORCE_TORQUE=1, - SENSOR_IMU=1, + SENSOR_IMU=2, }; struct CreateSensorArgs