diff --git a/data/cube.mtl b/data/cube.mtl index 8ce4b8207..935e48aa1 100644 --- a/data/cube.mtl +++ b/data/cube.mtl @@ -1,11 +1,14 @@ -# Blender MTL File: 'None' -# Material Count: 1 +newmtl cube + Ns 10.0000 + Ni 1.5000 + d 1.0000 + Tr 0.0000 + Tf 1.0000 1.0000 1.0000 + illum 2 + Ka 0.0000 0.0000 0.0000 + Kd 0.5880 0.5880 0.5880 + Ks 0.0000 0.0000 0.0000 + Ke 0.0000 0.0000 0.0000 + map_Ka cube.png + map_Kd cube.png -newmtl Material -Ns 96.078431 -Ka 0.000000 0.000000 0.000000 -Kd 0.640000 0.640000 0.640000 -Ks 0.500000 0.500000 0.500000 -Ni 1.000000 -d 1.000000 -illum 2 diff --git a/data/cube.obj b/data/cube.obj index 5270c7374..74ee020a3 100644 --- a/data/cube.obj +++ b/data/cube.obj @@ -1,26 +1,48 @@ -# Blender v2.66 (sub 1) OBJ File: '' -# www.blender.org +# cube.obj +# + +o cube mtllib cube.mtl -o Cube -v 1.000000 -1.000000 -1.000000 -v 1.000000 -1.000000 1.000000 -v -1.000000 -1.000000 1.000000 -v -1.000000 -1.000000 -1.000000 -v 1.000000 1.000000 -0.999999 -v 0.999999 1.000000 1.000001 -v -1.000000 1.000000 1.000000 -v -1.000000 1.000000 -1.000000 -usemtl Material -s off -f 1 2 3 -f 5 8 7 -f 1 5 6 -f 2 6 3 -f 3 7 4 -f 5 1 4 -f 4 1 3 -f 6 5 7 -f 2 1 6 -f 6 7 3 -f 7 8 4 -f 8 5 4 + +v -0.500000 -0.500000 0.500000 +v 0.500000 -0.500000 0.500000 +v -0.500000 0.500000 0.500000 +v 0.500000 0.500000 0.500000 +v -0.500000 0.500000 -0.500000 +v 0.500000 0.500000 -0.500000 +v -0.500000 -0.500000 -0.500000 +v 0.500000 -0.500000 -0.500000 + +vt 0.000000 0.000000 +vt 1.000000 0.000000 +vt 0.000000 1.000000 +vt 1.000000 1.000000 + +vn 0.000000 0.000000 1.000000 +vn 0.000000 1.000000 0.000000 +vn 0.000000 0.000000 -1.000000 +vn 0.000000 -1.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn -1.000000 0.000000 0.000000 + +g cube +usemtl cube +s 1 +f 1/1/1 2/2/1 3/3/1 +f 3/3/1 2/2/1 4/4/1 +s 2 +f 3/1/2 4/2/2 5/3/2 +f 5/3/2 4/2/2 6/4/2 +s 3 +f 5/4/3 6/3/3 7/2/3 +f 7/2/3 6/3/3 8/1/3 +s 4 +f 7/1/4 8/2/4 1/3/4 +f 1/3/4 8/2/4 2/4/4 +s 5 +f 2/1/5 8/2/5 4/3/5 +f 4/3/5 8/2/5 6/4/5 +s 6 +f 7/1/6 1/2/6 5/3/6 +f 5/3/6 1/2/6 3/4/6 + diff --git a/examples/Collision/CollisionSdkC_Api.cpp b/examples/Collision/CollisionSdkC_Api.cpp index 13dc8378d..ee4f8b991 100644 --- a/examples/Collision/CollisionSdkC_Api.cpp +++ b/examples/Collision/CollisionSdkC_Api.cpp @@ -61,6 +61,22 @@ plCollisionShapeHandle plCreatePlaneShape(plCollisionSdkHandle collisionSdkHandl return sdk->createPlaneShape(worldHandle,planeNormalX,planeNormalY,planeNormalZ,planeConstant); } +plCollisionShapeHandle plCreateCapsuleShape(plCollisionSdkHandle collisionSdkHandle, plCollisionWorldHandle worldHandle, plReal radius, plReal height, int capsuleAxis) +{ + CollisionSdkInterface* sdk = (CollisionSdkInterface*) collisionSdkHandle; + return sdk->createCapsuleShape(worldHandle,radius,height,capsuleAxis); +} + +plCollisionShapeHandle plCreateCompoundShape(plCollisionSdkHandle collisionSdkHandle,plCollisionWorldHandle worldHandle) +{ + CollisionSdkInterface* sdk = (CollisionSdkInterface*) collisionSdkHandle; + return sdk->createCompoundShape(worldHandle); +} +void plAddChildShape(plCollisionSdkHandle collisionSdkHandle, plCollisionWorldHandle worldHandle, plCollisionShapeHandle compoundShape,plCollisionShapeHandle childShape, plVector3 childPos,plQuaternion childOrn) +{ + CollisionSdkInterface* sdk = (CollisionSdkInterface*) collisionSdkHandle; + sdk->addChildShape(worldHandle,compoundShape,childShape,childPos,childOrn); +} void plDeleteShape(plCollisionSdkHandle collisionSdkHandle, plCollisionWorldHandle worldHandle, plCollisionShapeHandle shapeHandle) { diff --git a/examples/Collision/CollisionSdkC_Api.h b/examples/Collision/CollisionSdkC_Api.h index b3e5cdb4b..fbbd767e3 100644 --- a/examples/Collision/CollisionSdkC_Api.h +++ b/examples/Collision/CollisionSdkC_Api.h @@ -35,8 +35,11 @@ extern "C" { /* Collision SDK */ extern plCollisionSdkHandle plCreateBullet2CollisionSdk(); - + +#ifndef DISABLE_REAL_TIME_BULLET3_COLLISION_SDK extern plCollisionSdkHandle plCreateRealTimeBullet3CollisionSdk(); +#endif //DISABLE_REAL_TIME_BULLET3_COLLISION_SDK + // extern plCollisionSdkHandle plCreateCustomCollisionSdk(); extern void plDeleteCollisionSdk(plCollisionSdkHandle collisionSdkHandle); @@ -63,14 +66,14 @@ extern "C" { /* Collision Shape definition */ extern plCollisionShapeHandle plCreateSphereShape(plCollisionSdkHandle sdk, plCollisionWorldHandle worldHandle, plReal radius); - extern plCollisionShapeHandle plNewCapsuleShape(plCollisionSdkHandle sdk, plCollisionWorldHandle worldHandle, plReal radius, plReal height); + extern plCollisionShapeHandle plCreateCapsuleShape(plCollisionSdkHandle sdk, plCollisionWorldHandle worldHandle, plReal radius, plReal height, int capsuleAxis); extern plCollisionShapeHandle plCreatePlaneShape(plCollisionSdkHandle sdk, plCollisionWorldHandle worldHandle, plReal planeNormalX, plReal planeNormalY, plReal planeNormalZ, plReal planeConstant); - extern plCollisionShapeHandle plNewCompoundShape(plCollisionSdkHandle sdk,plCollisionWorldHandle worldHandle); - extern void plAddChildShape(plCollisionSdkHandle sdk, plCollisionShapeHandle compoundShape,plCollisionShapeHandle childShape, plVector3 childPos,plQuaternion childOrn); + extern plCollisionShapeHandle plCreateCompoundShape(plCollisionSdkHandle sdk,plCollisionWorldHandle worldHandle); + extern void plAddChildShape(plCollisionSdkHandle collisionSdkHandle, plCollisionWorldHandle worldHandle, plCollisionShapeHandle compoundShape,plCollisionShapeHandle childShape, plVector3 childPos,plQuaternion childOrn); extern void plDeleteShape(plCollisionSdkHandle collisionSdkHandle, plCollisionWorldHandle worldHandle, plCollisionShapeHandle shape); diff --git a/examples/Collision/CollisionTutorialBullet2.cpp b/examples/Collision/CollisionTutorialBullet2.cpp index 2d3859ef3..9473c83f2 100644 --- a/examples/Collision/CollisionTutorialBullet2.cpp +++ b/examples/Collision/CollisionTutorialBullet2.cpp @@ -22,11 +22,18 @@ ///todo: use the 'userData' to prevent this use of global variables static int gTotalPoints = 0; const int sPointCapacity = 10000; -const int sNumSpheres = 128; +const int sNumCompounds = 10; +const int sNumSpheres = 10; lwContactPoint pointsOut[sPointCapacity]; int numNearCallbacks = 0; - +static btVector4 sColors[4] = +{ + btVector4(1,0.7,0.7,1), + btVector4(1,1,0.7,1), + btVector4(0.7,1,0.7,1), + btVector4(0.7,1,1,1), +}; void myNearCallback(plCollisionSdkHandle sdkHandle, plCollisionWorldHandle worldHandle, void* userData, plCollisionObjectHandle objA, plCollisionObjectHandle objB) { @@ -69,23 +76,25 @@ public: m_counter(0), m_timeSeriesCanvas0(0) { - int numBodies = 1; + gTotalPoints = 0; m_app->setUpAxis(1); m_app->m_renderer->enableBlend(true); switch (m_tutorialIndex) { - case TUT_SPHERE_SPHERE_RTB3: - case TUT_SPHERE_SPHERE_BULLET2: + case TUT_SPHERE_PLANE_RTB3: + case TUT_SPHERE_PLANE_BULLET2: { - numBodies=10; - if (m_tutorialIndex==TUT_SPHERE_SPHERE_BULLET2) + + if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2) { m_collisionSdkHandle = plCreateBullet2CollisionSdk(); } else { +#ifndef DISABLE_REAL_TIME_BULLET3_COLLISION_SDK m_collisionSdkHandle = plCreateRealTimeBullet3CollisionSdk(); +#endif //DISABLE_REAL_TIME_BULLET3_COLLISION_SDK } if (m_collisionSdkHandle) { @@ -97,29 +106,54 @@ public: //create objects, do query etc { float radius = 1.f; - plCollisionShapeHandle colShape = plCreateSphereShape(m_collisionSdkHandle, m_collisionWorldHandle,radius); - void* userPointer = 0; - - int sphereGfxShapeId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_HIGH);//, textureIndex); - for (int i=0;im_renderer->registerGraphicsInstance(sphereGfxShapeId,pos, orn,color,scaling); + plCollisionShapeHandle childShape = plCreateSphereShape(m_collisionSdkHandle, m_collisionWorldHandle,radius); + plAddChildShape(m_collisionSdkHandle,m_collisionWorldHandle,compoundShape, childShape,childPos,childOrn); - plCollisionObjectHandle colObj = plCreateCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,userPointer, gfxIndex,colShape,pos,orn); - colliders.push_back(colObj); - plAddCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle,colObj); + + //m_guiHelper->createCollisionObjectGraphicsObject(colObj,color); + + } + if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2) + { + btCollisionShape* colShape = (btCollisionShape*) compoundShape; + m_guiHelper->createCollisionShapeGraphicsObject(colShape); + } else + { + } + + { + btVector3 pos(j*sNumSpheres*1.5,-2.4,0); + btQuaternion orn(0,0,0,1); + plCollisionObjectHandle colObjHandle = plCreateCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,userPointer, -1,compoundShape,pos,orn); + if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2) + { + btCollisionObject* colObj = (btCollisionObject*) colObjHandle; + btVector4 color=sColors[j&3]; + m_guiHelper->createCollisionObjectGraphicsObject(colObj,color); + colliders.push_back(colObjHandle); + plAddCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle,colObjHandle); + } + } + } } } { - plCollisionShapeHandle colShape = plCreatePlaneShape(m_collisionSdkHandle, m_collisionWorldHandle,0,1,0,0); + plCollisionShapeHandle colShape = plCreatePlaneShape(m_collisionSdkHandle, m_collisionWorldHandle,0,1,0,-3.5); btVector3 pos(0,0,0); btQuaternion orn(0,0,0,1); void* userPointer = 0; @@ -140,6 +174,7 @@ public: //plDeleteShape(m_collisionSdkHandle,colShape); } + /* m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Constant Velocity"); @@ -151,11 +186,7 @@ public: break; } - case TUT_SPHERE_PLANE_RTB3: - case TUT_SPHERE_PLANE_BULLET2: - { - break; - } + default: { @@ -169,7 +200,7 @@ public: { - int boxId = m_app->registerCubeShape(100,1,100); + int boxId = m_app->registerCubeShape(100,0.01,100); b3Vector3 pos = b3MakeVector3(0,-3.5,0); b3Quaternion orn(0,0,0,1); b3Vector4 color = b3MakeVector4(1,1,1,1); @@ -245,7 +276,10 @@ public: numNearCallbacks = 0; { BT_PROFILE("plWorldCollide"); - plWorldCollide(m_collisionSdkHandle,m_collisionWorldHandle,myNearCallback, myUserPtr); + if (m_collisionSdkHandle && m_collisionWorldHandle) + { + plWorldCollide(m_collisionSdkHandle,m_collisionWorldHandle,myNearCallback, myUserPtr); + } } #if 0 diff --git a/examples/Collision/CollisionTutorialBullet2.h b/examples/Collision/CollisionTutorialBullet2.h index 76f0f9a3b..df34e7946 100644 --- a/examples/Collision/CollisionTutorialBullet2.h +++ b/examples/Collision/CollisionTutorialBullet2.h @@ -3,10 +3,8 @@ enum EnumCollisionTutorialTypes { - TUT_SPHERE_SPHERE_BULLET2=0, - TUT_SPHERE_PLANE_BULLET2, - TUT_SPHERE_SPHERE_RTB3, - TUT_SPHERE_PLANE_RTB3, + TUT_SPHERE_PLANE_BULLET2=0, + TUT_SPHERE_PLANE_RTB3, }; class CommonExampleInterface* CollisionTutorialBullet2CreateFunc(struct CommonExampleOptions& options); diff --git a/examples/Collision/Internal/Bullet2CollisionSdk.cpp b/examples/Collision/Internal/Bullet2CollisionSdk.cpp index 0deb271be..a11e6efad 100644 --- a/examples/Collision/Internal/Bullet2CollisionSdk.cpp +++ b/examples/Collision/Internal/Bullet2CollisionSdk.cpp @@ -75,6 +75,52 @@ plCollisionShapeHandle Bullet2CollisionSdk::createPlaneShape(plCollisionWorldHan return (plCollisionShapeHandle) planeShape; } +plCollisionShapeHandle Bullet2CollisionSdk::createCapsuleShape(plCollisionWorldHandle worldHandle, + plReal radius, + plReal height, + int capsuleAxis) +{ + btCapsuleShape* capsule = 0; + + switch (capsuleAxis) + { + case 0: + { + capsule = new btCapsuleShapeX(radius,height); + break; + } + case 1: + { + capsule = new btCapsuleShape(radius,height); + break; + } + case 2: + { + capsule = new btCapsuleShapeZ(radius,height); + break; + } + default: + { + btAssert(0); + } + } + return (plCollisionShapeHandle)capsule; +} + +plCollisionShapeHandle Bullet2CollisionSdk::createCompoundShape(plCollisionWorldHandle worldHandle) +{ + return (plCollisionShapeHandle) new btCompoundShape(); +} +void Bullet2CollisionSdk::addChildShape(plCollisionWorldHandle worldHandle,plCollisionShapeHandle compoundShapeHandle, plCollisionShapeHandle childShapeHandle,plVector3 childPos,plQuaternion childOrn) +{ + btCompoundShape* compound = (btCompoundShape*) compoundShapeHandle; + btCollisionShape* childShape = (btCollisionShape*) childShapeHandle; + btTransform localTrans; + localTrans.setOrigin(btVector3(childPos[0],childPos[1],childPos[2])); + localTrans.setRotation(btQuaternion(childOrn[0],childOrn[1],childOrn[2],childOrn[3])); + compound->addChildShape(localTrans,childShape); + +} void Bullet2CollisionSdk::deleteShape(plCollisionWorldHandle /*worldHandle*/, plCollisionShapeHandle shapeHandle) { diff --git a/examples/Collision/Internal/Bullet2CollisionSdk.h b/examples/Collision/Internal/Bullet2CollisionSdk.h index 7000d7ddb..9788d0444 100644 --- a/examples/Collision/Internal/Bullet2CollisionSdk.h +++ b/examples/Collision/Internal/Bullet2CollisionSdk.h @@ -25,6 +25,14 @@ public: plReal planeNormalZ, plReal planeConstant); + virtual plCollisionShapeHandle createCapsuleShape(plCollisionWorldHandle worldHandle, + plReal radius, + plReal height, + int capsuleAxis); + + virtual plCollisionShapeHandle createCompoundShape(plCollisionWorldHandle worldHandle); + virtual void addChildShape(plCollisionWorldHandle worldHandle,plCollisionShapeHandle compoundShape, plCollisionShapeHandle childShape,plVector3 childPos,plQuaternion childOrn); + virtual void deleteShape(plCollisionWorldHandle worldHandle, plCollisionShapeHandle shape); virtual void addCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object); diff --git a/examples/Collision/Internal/CollisionSdkInterface.h b/examples/Collision/Internal/CollisionSdkInterface.h index eb07ee7ed..8c3ab5f76 100644 --- a/examples/Collision/Internal/CollisionSdkInterface.h +++ b/examples/Collision/Internal/CollisionSdkInterface.h @@ -23,6 +23,14 @@ public: plReal planeNormalZ, plReal planeConstant) = 0; + virtual plCollisionShapeHandle createCapsuleShape(plCollisionWorldHandle worldHandle, + plReal radius, + plReal height, + int capsuleAxis) = 0; + + virtual plCollisionShapeHandle createCompoundShape(plCollisionWorldHandle worldHandle) = 0; + virtual void addChildShape(plCollisionWorldHandle worldHandle,plCollisionShapeHandle compoundShape, plCollisionShapeHandle childShape,plVector3 childPos,plQuaternion childOrn)=0; + virtual void deleteShape(plCollisionWorldHandle worldHandle, plCollisionShapeHandle shape) = 0; virtual void addCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object)=0; diff --git a/examples/Collision/Internal/RealTimeBullet3CollisionSdk.cpp b/examples/Collision/Internal/RealTimeBullet3CollisionSdk.cpp index b67bc3cba..34b8e7eb0 100644 --- a/examples/Collision/Internal/RealTimeBullet3CollisionSdk.cpp +++ b/examples/Collision/Internal/RealTimeBullet3CollisionSdk.cpp @@ -3,6 +3,7 @@ #include "Bullet3Common/b3AlignedObjectArray.h" #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" //convert the opaque pointer to int struct RTB3_ColliderOpaque2Int @@ -26,9 +27,9 @@ enum RTB3ShapeTypes { RTB3_SHAPE_SPHERE=0, RTB3_SHAPE_PLANE, -// RTB3_SHAPE_CAPSULE, + RTB3_SHAPE_CAPSULE, MAX_NUM_SINGLE_SHAPE_TYPES, - RTB3_SHAPE_COMPOUND_INTERNAL + RTB3_SHAPE_COMPOUND_INTERNAL, }; @@ -48,6 +49,8 @@ struct RTB3CollisionWorld b3AlignedObjectArray m_collidables; b3AlignedObjectArray m_childShapes; + b3AlignedObjectArray m_localSpaceAabbs; + b3AlignedObjectArray m_worldSpaceAabbs; b3AlignedObjectArray m_planeFaces; b3AlignedObjectArray m_compoundOverlappingPairs; int m_nextFreeShapeIndex; @@ -149,7 +152,49 @@ plCollisionShapeHandle RealTimeBullet3CollisionSdk::createPlaneShape(plCollision return 0; } +plCollisionShapeHandle RealTimeBullet3CollisionSdk::createCapsuleShape(plCollisionWorldHandle worldHandle, + plReal radius, + plReal height, + int capsuleAxis) +{ + RTB3CollisionWorld* world = (RTB3CollisionWorld*) worldHandle; + b3Assert(world->m_nextFreeShapeIndex < world->m_childShapes.size() && world->m_nextFreePlaneFaceIndex < world->m_planeFaces.size()); + if (world->m_nextFreeShapeIndex < world->m_childShapes.size() && world->m_nextFreePlaneFaceIndex < world->m_planeFaces.size()) + { + b3GpuChildShape& shape = world->m_childShapes[world->m_nextFreeShapeIndex]; + shape.m_childPosition.setZero(); + shape.m_childOrientation.setValue(0,0,0,1); + shape.m_radius = radius; + shape.m_height = height; + shape.m_shapeIndex = capsuleAxis; + shape.m_shapeType = RTB3_SHAPE_CAPSULE; + return (plCollisionShapeHandle) world->m_nextFreeShapeIndex++; + } + return 0; +} + +plCollisionShapeHandle RealTimeBullet3CollisionSdk::createCompoundShape(plCollisionWorldHandle worldHandle) +{ + RTB3CollisionWorld* world = (RTB3CollisionWorld*) worldHandle; + b3Assert(world->m_nextFreeShapeIndex < world->m_childShapes.size() && world->m_nextFreePlaneFaceIndex < world->m_planeFaces.size()); + + if (world->m_nextFreeShapeIndex < world->m_childShapes.size() && world->m_nextFreePlaneFaceIndex < world->m_planeFaces.size()) + { + b3GpuChildShape& shape = world->m_childShapes[world->m_nextFreeShapeIndex]; + shape.m_childPosition.setZero(); + shape.m_childOrientation.setValue(0,0,0,1); + shape.m_numChildShapes = 0; + shape.m_shapeType = RTB3_SHAPE_COMPOUND_INTERNAL; + return (plCollisionShapeHandle) world->m_nextFreeShapeIndex++; + } + return 0; +} + +void RealTimeBullet3CollisionSdk::addChildShape(plCollisionWorldHandle worldHandle,plCollisionShapeHandle compoundShape, plCollisionShapeHandle childShape,plVector3 childPos,plQuaternion childOrn) +{ + +} void RealTimeBullet3CollisionSdk::deleteShape(plCollisionWorldHandle worldHandle, plCollisionShapeHandle shape) { ///todo @@ -203,7 +248,7 @@ plCollisionObjectHandle RealTimeBullet3CollisionSdk::createCollisionObject( plC } case RTB3_SHAPE_COMPOUND_INTERNAL: { - b3Assert(0); + break; } default: @@ -396,8 +441,8 @@ int RealTimeBullet3CollisionSdk::collide(plCollisionWorldHandle worldHandle,plCo { if (contactCache.numAddedPointsm_childShapes[colA.m_shapeIndex+i].m_shapeType] - [world->m_childShapes[colB.m_shapeIndex+j].m_shapeType](world,colAIndex,colA.m_shapeIndex+i,colBIndex,colB.m_shapeIndex+j,&contactCache); + //funcTbl_detectCollision[world->m_childShapes[colA.m_shapeIndex+i].m_shapeType] + // [world->m_childShapes[colB.m_shapeIndex+j].m_shapeType](world,colAIndex,colA.m_shapeIndex+i,colBIndex,colB.m_shapeIndex+j,&contactCache); } } return contactCache.numAddedPoints; diff --git a/examples/Collision/Internal/RealTimeBullet3CollisionSdk.h b/examples/Collision/Internal/RealTimeBullet3CollisionSdk.h index 0d05181b5..7e65bd8a1 100644 --- a/examples/Collision/Internal/RealTimeBullet3CollisionSdk.h +++ b/examples/Collision/Internal/RealTimeBullet3CollisionSdk.h @@ -23,6 +23,13 @@ public: plReal planeNormalY, plReal planeNormalZ, plReal planeConstant); + virtual plCollisionShapeHandle createCapsuleShape(plCollisionWorldHandle worldHandle, + plReal radius, + plReal height, + int capsuleAxis); + + virtual plCollisionShapeHandle createCompoundShape(plCollisionWorldHandle worldHandle); + virtual void addChildShape(plCollisionWorldHandle worldHandle,plCollisionShapeHandle compoundShape, plCollisionShapeHandle childShape,plVector3 childPos,plQuaternion childOrn); virtual void deleteShape(plCollisionWorldHandle worldHandle, plCollisionShapeHandle shape); diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index a64dee5f9..17b920fa9 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -117,11 +117,9 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Spring constraint","A rigid body with a spring constraint attached", Dof6ConstraintTutorialCreateFunc,0), ExampleEntry(0,"Collision"), - ExampleEntry(1, "Sphere-Sphere C-API (Bullet2)", "Collision C-API using Bullet 2.x backend", CollisionTutorialBullet2CreateFunc,TUT_SPHERE_SPHERE_BULLET2), - ExampleEntry(1, "Sphere-Plane C-API (Bullet2)", "Collision C-API using Bullet 2.x backend", CollisionTutorialBullet2CreateFunc,TUT_SPHERE_PLANE_BULLET2), - ExampleEntry(1, "Sphere-Sphere C-API (Bullet3)", "Collision C-API using Bullet 3.x backend", CollisionTutorialBullet2CreateFunc,TUT_SPHERE_SPHERE_RTB3), - ExampleEntry(1, "Sphere-Plane C-API (Bullet3)", "Collision C-API using Bullet 3.x backend", CollisionTutorialBullet2CreateFunc,TUT_SPHERE_PLANE_RTB3), - + ExampleEntry(1, "Spheres & Plane C-API (Bullet2)", "Collision C-API using Bullet 2.x backend", CollisionTutorialBullet2CreateFunc,TUT_SPHERE_PLANE_BULLET2), + //ExampleEntry(1, "Spheres & Plane C-API (Bullet3)", "Collision C-API using Bullet 3.x backend", CollisionTutorialBullet2CreateFunc,TUT_SPHERE_PLANE_RTB3), + #ifdef INCLUDE_CLOTH_DEMOS diff --git a/examples/Importers/ImportObjDemo/ImportObjExample.cpp b/examples/Importers/ImportObjDemo/ImportObjExample.cpp index 7ddd2f44f..db6ef97e0 100644 --- a/examples/Importers/ImportObjDemo/ImportObjExample.cpp +++ b/examples/Importers/ImportObjDemo/ImportObjExample.cpp @@ -59,7 +59,7 @@ void ImportObjSetup::initPhysics() m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe); - const char* fileName = "sphere8.obj";//sponza_closed.obj";//sphere8.obj"; + const char* fileName = "cube.obj";//sphere8.obj";//sponza_closed.obj";//sphere8.obj"; char relativeFileName[1024]; if (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024)) { @@ -70,7 +70,7 @@ void ImportObjSetup::initPhysics() btVector3 shift(0,0,0); - btVector3 scaling(10,10,10); + btVector3 scaling(1,1,1); // int index=10; { diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 1d76ae427..7553811bc 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -95,6 +95,7 @@ int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_updateFlags |= SIM_PARAM_UPDATE_DELTA_TIME; command->m_physSimParamArgs.m_deltaTime = timeStep; return 0; } @@ -190,7 +191,7 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl } -b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient) +b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -198,7 +199,7 @@ b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandl struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); b3Assert(command); command->m_type =CMD_REQUEST_ACTUAL_STATE; - command->m_requestActualStateInformationCommandArgument.m_bodyUniqueId = 0; + command->m_requestActualStateInformationCommandArgument.m_bodyUniqueId = bodyUniqueId; return (b3SharedMemoryCommandHandle) command; } @@ -245,6 +246,45 @@ int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle return 0; } + +int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); + command->m_updateFlags |=BOX_SHAPE_HAS_HALF_EXTENTS; + + command->m_createBoxShapeArguments.m_halfExtentsX = halfExtentsX; + command->m_createBoxShapeArguments.m_halfExtentsY = halfExtentsY; + command->m_createBoxShapeArguments.m_halfExtentsZ = halfExtentsZ; + + return 0; +} + + +int b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double mass) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); + command->m_updateFlags |=BOX_SHAPE_HAS_MASS; + command->m_createBoxShapeArguments.m_mass = mass; + return 0; +} + + +int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle, int collisionShapeType) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); + command->m_updateFlags |=BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE; + command->m_createBoxShapeArguments.m_collisionShapeType = collisionShapeType; + + return 0; +} + + int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; @@ -313,30 +353,20 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; - b3Assert(command); - b3Assert(command->m_type == CMD_INIT_POSE); - command->m_updateFlags |=INIT_POSE_HAS_JOINT_STATE; + b3Assert(command); + b3Assert(command->m_type == CMD_INIT_POSE); + command->m_updateFlags |=INIT_POSE_HAS_JOINT_STATE; b3JointInfo info; b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId,jointIndex, &info); - command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition; + btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0); + if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0) + { + command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition; + } return 0; } -int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ) -{ - struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; - b3Assert(command); - b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); - command->m_updateFlags |=BOX_SHAPE_HAS_HALF_EXTENTS; - - command->m_createBoxShapeArguments.m_halfExtentsX = halfExtentsX; - command->m_createBoxShapeArguments.m_halfExtentsY = halfExtentsY; - command->m_createBoxShapeArguments.m_halfExtentsZ = halfExtentsZ; - - return 0; -} - b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient) @@ -393,6 +423,7 @@ b3PhysicsClientHandle b3ConnectSharedMemory(int key) return (b3PhysicsClientHandle ) cl; } + void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -436,6 +467,11 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle) bodyId = status->m_dataStreamArguments.m_bodyUniqueId; break; } + case CMD_RIGID_BODY_CREATION_COMPLETED: + { + bodyId = status->m_rigidBodyCreateArgs.m_bodyUniqueId; + break; + } default: { b3Assert(0); @@ -447,7 +483,11 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle) int b3CanSubmitCommand(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; - return (int)cl->canSubmitCommand(); + if (cl) + { + return (int)cl->canSubmitCommand(); + } + return false; } int b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 6d37cfdff..5f008c47e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -15,7 +15,7 @@ B3_DECLARE_HANDLE(b3SharedMemoryStatusHandle); extern "C" { #endif -///make sure to start the server first, before connecting client to physics server +///make sure to start the server first, before connecting client to a physics server over shared memory or UDP b3PhysicsClientHandle b3ConnectSharedMemory(int key); void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient); @@ -84,6 +84,9 @@ b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle ph int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ); int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ); +int b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double mass); +int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle, int collisionShapeType); + ///Initialize (teleport) the pose of a body/robot. You can individually set the base position, base orientation and joint angles. ///This will set all velocities of base and joints to zero. @@ -97,7 +100,7 @@ b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle phys int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable); int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable); -b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient); +b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId); void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state); int b3PickBody(struct SharedMemoryCommand *command, diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index ab444279b..4308e054d 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -194,7 +194,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) case CMD_LOAD_URDF: { - b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "r2d2.urdf");//kuka_lwr/kuka.urdf"); + b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_lwr/kuka.urdf"); //setting the initial position, orientation and other arguments are optional double startPosX = 0; @@ -214,10 +214,26 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } + case CMD_CREATE_RIGID_BODY: + { + b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle); + b3CreateBoxCommandSetStartPosition(commandHandle,0,0,0); + b3CreateBoxCommandSetMass(commandHandle,1); + b3CreateBoxCommandSetCollisionShapeType(commandHandle,COLLISION_SHAPE_TYPE_CYLINDER_Y); + double radius = 0.2; + double halfHeight = 0.5; + b3CreateBoxCommandSetHalfExtents(commandHandle,radius,halfHeight,radius); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + break; + } + case CMD_REQUEST_ACTUAL_STATE: { - b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle); - b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + if (m_selectedBody>=0) + { + b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle,m_selectedBody); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + } break; }; @@ -360,6 +376,7 @@ void PhysicsClientExample::createButtons() createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger); createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger); createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger); + createButton("Create Cylinder Body",CMD_CREATE_RIGID_BODY,isTrigger); createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger); createButton("Initialize Pose",CMD_INIT_POSE, isTrigger); @@ -515,8 +532,8 @@ void PhysicsClientExample::stepSimulation(float deltaTime) { enqueueCommand(CMD_SEND_DESIRED_STATE); enqueueCommand(CMD_STEP_FORWARD_SIMULATION); - enqueueCommand(CMD_REQUEST_DEBUG_LINES); - enqueueCommand(CMD_REQUEST_ACTUAL_STATE); + //enqueueCommand(CMD_REQUEST_DEBUG_LINES); + //enqueueCommand(CMD_REQUEST_ACTUAL_STATE); } } } diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 255bb2bb5..ba1b3127e 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -450,6 +450,11 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { break; } + case CMD_RIGID_BODY_CREATION_COMPLETED: + { + + break; + } case CMD_DEBUG_LINES_OVERFLOW_FAILED: { b3Warning("Error receiving debug lines"); m_data->m_debugLinesFrom.resize(0); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp new file mode 100644 index 000000000..3b55d1582 --- /dev/null +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -0,0 +1,115 @@ +#include "PhysicsLoopBack.h" +#include "PhysicsServer.h" +#include "PhysicsClientSharedMemory.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" + +struct PhysicsLoopBackInternalData +{ + PhysicsClientSharedMemory* m_physicsClient; + PhysicsServerSharedMemory* m_physicsServer; + DummyGUIHelper m_noGfx; + + + PhysicsLoopBackInternalData() + :m_physicsClient(0), + m_physicsServer(0) + { + } +}; + +PhysicsLoopBack::PhysicsLoopBack() +{ + m_data = new PhysicsLoopBackInternalData; + m_data->m_physicsServer = new PhysicsServerSharedMemory(); + m_data->m_physicsClient = new PhysicsClientSharedMemory(); +} + +PhysicsLoopBack::~PhysicsLoopBack() +{ + delete m_data->m_physicsClient; + delete m_data->m_physicsServer; + delete m_data; +} + + +// return true if connection succesfull, can also check 'isConnected' +bool PhysicsLoopBack::connect() +{ + m_data->m_physicsServer->connectSharedMemory(&m_data->m_noGfx); + m_data->m_physicsClient->connect(); + return m_data->m_physicsClient->isConnected(); +} + +////todo: rename to 'disconnect' +void PhysicsLoopBack::disconnectSharedMemory() +{ + m_data->m_physicsClient->disconnectSharedMemory(); + m_data->m_physicsServer->disconnectSharedMemory(true); +} + +bool PhysicsLoopBack::isConnected() const +{ + return m_data->m_physicsClient->isConnected(); +} + +// return non-null if there is a status, nullptr otherwise +const SharedMemoryStatus* PhysicsLoopBack::processServerStatus() +{ + m_data->m_physicsServer->processClientCommands(); + return m_data->m_physicsClient->processServerStatus(); +} + +SharedMemoryCommand* PhysicsLoopBack::getAvailableSharedMemoryCommand() +{ + return m_data->m_physicsClient->getAvailableSharedMemoryCommand(); +} + +bool PhysicsLoopBack::canSubmitCommand() const +{ + return m_data->m_physicsClient->canSubmitCommand(); +} + +bool PhysicsLoopBack::submitClientCommand(const struct SharedMemoryCommand& command) +{ + return m_data->m_physicsClient->submitClientCommand(command); +} + +int PhysicsLoopBack::getNumJoints(int bodyIndex) const +{ + return m_data->m_physicsClient->getNumJoints(bodyIndex); +} + +void PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const +{ + m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info); +} + +///todo: move this out of the +void PhysicsLoopBack::setSharedMemoryKey(int key) +{ + m_data->m_physicsServer->setSharedMemoryKey(key); + m_data->m_physicsClient->setSharedMemoryKey(key); +} + +void PhysicsLoopBack::uploadBulletFileToSharedMemory(const char* data, int len) +{ + m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len); +} + +int PhysicsLoopBack::getNumDebugLines() const +{ + return m_data->m_physicsClient->getNumDebugLines(); +} + +const float* PhysicsLoopBack::getDebugLinesFrom() const +{ + return m_data->m_physicsClient->getDebugLinesFrom(); +} +const float* PhysicsLoopBack::getDebugLinesTo() const +{ + return m_data->m_physicsClient->getDebugLinesTo(); +} +const float* PhysicsLoopBack::getDebugLinesColor() const +{ + return m_data->m_physicsClient->getDebugLinesColor(); +} diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h new file mode 100644 index 000000000..ea66ad177 --- /dev/null +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -0,0 +1,58 @@ +#ifndef PHYSICS_LOOP_BACK_H +#define PHYSICS_LOOP_BACK_H + +//#include "SharedMemoryCommands.h" + + +#include "PhysicsClient.h" +#include "LinearMath/btVector3.h" + +///todo: the PhysicsClient API was designed with shared memory in mind, +///now it become more general we need to move out the shared memory specifics away +///for example naming [disconnectSharedMemory -> disconnect] [ move setSharedMemoryKey to shared memory specific subclass ] + +class PhysicsLoopBack : public PhysicsClient +{ + struct PhysicsLoopBackInternalData* m_data; + +public: + + PhysicsLoopBack(); + + virtual ~PhysicsLoopBack(); + + // return true if connection succesfull, can also check 'isConnected' + virtual bool connect(); + + ////todo: rename to 'disconnect' + virtual void disconnectSharedMemory(); + + virtual bool isConnected() const; + + // return non-null if there is a status, nullptr otherwise + virtual const SharedMemoryStatus* processServerStatus(); + + virtual SharedMemoryCommand* getAvailableSharedMemoryCommand(); + + virtual bool canSubmitCommand() const; + + virtual bool submitClientCommand(const struct SharedMemoryCommand& command); + + virtual int getNumJoints(int bodyIndex) const; + + virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; + + ///todo: move this out of the + virtual void setSharedMemoryKey(int key); + + void uploadBulletFileToSharedMemory(const char* data, int len); + + virtual int getNumDebugLines() const; + + virtual const float* getDebugLinesFrom() const; + virtual const float* getDebugLinesTo() const; + virtual const float* getDebugLinesColor() const; + +}; + +#endif //PHYSICS_LOOP_BACK_H diff --git a/examples/SharedMemory/PhysicsLoopBackC_API.h b/examples/SharedMemory/PhysicsLoopBackC_API.h new file mode 100644 index 000000000..6adb69e9c --- /dev/null +++ b/examples/SharedMemory/PhysicsLoopBackC_API.h @@ -0,0 +1,19 @@ +#ifndef PHYSICS_LOOPBACK_C_API_H +#define PHYSICS_LOOPBACK_C_API_H + +#include "PhysicsClientC_API.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +///think more about naming. The b3ConnectPhysicsLoopback +b3PhysicsClientHandle b3ConnectPhysicsLoopback(int key); + + +#ifdef __cplusplus +} +#endif + +#endif //PHYSICS_LOOPBACK_C_API_H diff --git a/examples/SharedMemory/PhysicsLoopBackC_Api.cpp b/examples/SharedMemory/PhysicsLoopBackC_Api.cpp new file mode 100644 index 000000000..c1b47b960 --- /dev/null +++ b/examples/SharedMemory/PhysicsLoopBackC_Api.cpp @@ -0,0 +1,15 @@ +#include "PhysicsLoopBackC_API.h" + +#include "PhysicsLoopBack.h" + + + +//think more about naming. The b3ConnectPhysicsLoopback +b3PhysicsClientHandle b3ConnectPhysicsLoopback(int key) +{ + PhysicsLoopBack* loopBack = new PhysicsLoopBack(); + loopBack->setSharedMemoryKey(key); + bool connected = loopBack->connect(); + return (b3PhysicsClientHandle )loopBack; +} + diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 97d5aa55d..63bc65275 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -79,12 +79,14 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw struct InteralBodyData { btMultiBody* m_multiBody; + btRigidBody* m_rigidBody; int m_testData; btTransform m_rootLocalInertialFrame; InteralBodyData() :m_multiBody(0), + m_rigidBody(0), m_testData(0) { m_rootLocalInertialFrame.setIdentity(); @@ -1089,14 +1091,10 @@ void PhysicsServerSharedMemory::processClientCommands() } int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; InteralBodyData* body = m_data->getHandle(bodyUniqueId); - btMultiBody* mb = 0; - if (body) + + if (body && body->m_multiBody) { - mb = body->m_multiBody; - } - if (mb) - { - + btMultiBody* mb = body->m_multiBody; SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; @@ -1192,10 +1190,48 @@ void PhysicsServerSharedMemory::processClientCommands() } else { + if (body && body->m_rigidBody) + { + btRigidBody* rb = body->m_rigidBody; + SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); + serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; + int totalDegreeOfFreedomQ = 0; + int totalDegreeOfFreedomU = 0; - b3Warning("Request state but no multibody available"); - SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_FAILED,clientCmd.m_sequenceNumber,timeStamp); - m_data->submitServerStatus(serverCmd); + btTransform tr = rb->getWorldTransform(); + //base position in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; + + //base orientation, quaternion x,y,z,w, in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; + totalDegreeOfFreedomQ +=7;//pos + quaternion + + //base linear velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = rb->getLinearVelocity()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = rb->getLinearVelocity()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = rb->getLinearVelocity()[2]; + + //base angular velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = rb->getAngularVelocity()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = rb->getAngularVelocity()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = rb->getAngularVelocity()[2]; + totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF + + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; + + m_data->submitServerStatus(serverCmd); + } else + { + b3Warning("Request state but no multibody or rigid body available"); + SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_FAILED,clientCmd.m_sequenceNumber,timeStamp); + m_data->submitServerStatus(serverCmd); + } } break; @@ -1246,7 +1282,7 @@ void PhysicsServerSharedMemory::processClientCommands() { b3Printf("Server Init Pose not implemented yet"); } - int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId; + int bodyUniqueId = clientCmd.m_initPoseArgs.m_bodyUniqueId; InteralBodyData* body = m_data->getHandle(bodyUniqueId); if (body && body->m_multiBody) @@ -1301,13 +1337,15 @@ void PhysicsServerSharedMemory::processClientCommands() m_data->m_guiHelper->getRenderInterface()->removeAllInstances(); } deleteDynamicsWorld(); - createEmptyDynamicsWorld(); - + createEmptyDynamicsWorld(); + m_data->exitHandles(); + m_data->initHandles(); SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_RESET_SIMULATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(serverCmd); break; } + case CMD_CREATE_RIGID_BODY: case CMD_CREATE_BOX_COLLISION_SHAPE: { btVector3 halfExtents(1,1,1); @@ -1338,16 +1376,92 @@ void PhysicsServerSharedMemory::processClientCommands() clientCmd.m_createBoxShapeArguments.m_initialOrientation[3])); } + btScalar mass = 0.f; + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_MASS) + { + mass = clientCmd.m_createBoxShapeArguments.m_mass; + } + + int shapeType = COLLISION_SHAPE_TYPE_BOX; + + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE) + { + shapeType = clientCmd.m_createBoxShapeArguments.m_collisionShapeType; + } + btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); m_data->m_worldImporters.push_back(worldImporter); - btCollisionShape* shape = worldImporter->createBoxShape(halfExtents); - btScalar mass = 0.f; + btCollisionShape* shape = 0; + + switch (shapeType) + { + case COLLISION_SHAPE_TYPE_CYLINDER_X: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[0]; + shape = worldImporter->createCylinderShapeX(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CYLINDER_Y: + { + btScalar radius = halfExtents[0]; + btScalar height = halfExtents[1]; + shape = worldImporter->createCylinderShapeY(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CYLINDER_Z: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[2]; + shape = worldImporter->createCylinderShapeZ(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CAPSULE_X: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[0]; + shape = worldImporter->createCapsuleShapeX(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CAPSULE_Y: + { + btScalar radius = halfExtents[0]; + btScalar height = halfExtents[1]; + shape = worldImporter->createCapsuleShapeY(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CAPSULE_Z: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[2]; + shape = worldImporter->createCapsuleShapeZ(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_SPHERE: + { + btScalar radius = halfExtents[0]; + shape = worldImporter->createSphereShape(radius); + break; + } + case COLLISION_SHAPE_TYPE_BOX: + default: + { + shape = worldImporter->createBoxShape(halfExtents); + } + } + + bool isDynamic = (mass>0); - worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); + btRigidBody* rb = worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); + SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_RIGID_BODY_CREATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); + int bodyUniqueId = m_data->allocHandle(); + InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); + serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId; + bodyHandle->m_rootLocalInertialFrame.setIdentity(); + bodyHandle->m_rigidBody = rb; m_data->submitServerStatus(serverCmd); break; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index b009dc760..6016a3ba1 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -202,7 +202,9 @@ enum EnumBoxShapeFlags { BOX_SHAPE_HAS_INITIAL_POSITION=1, BOX_SHAPE_HAS_INITIAL_ORIENTATION=2, - BOX_SHAPE_HAS_HALF_EXTENTS=4 + BOX_SHAPE_HAS_HALF_EXTENTS=4, + BOX_SHAPE_HAS_MASS=8, + BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE=16, }; ///This command will be replaced to allow arbitrary collision shape types struct CreateBoxShapeArgs @@ -211,6 +213,9 @@ struct CreateBoxShapeArgs double m_halfExtentsY; double m_halfExtentsZ; + double m_mass; + int m_collisionShapeType;//see SharedMemoryPublic.h + double m_initialPosition[3]; double m_initialOrientation[4]; }; @@ -240,6 +245,10 @@ struct SharedMemoryCommand }; }; +struct RigidBodyCreateArgs +{ + int m_bodyUniqueId; +}; struct SharedMemoryStatus { @@ -253,6 +262,7 @@ struct SharedMemoryStatus struct BulletDataStreamArgs m_dataStreamArguments; struct SendActualStateArgs m_sendActualStateArgs; struct SendDebugLinesArgs m_sendDebugLinesArgs; + struct RigidBodyCreateArgs m_rigidBodyCreateArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index fcdd9ff73..b9cec004e 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -9,8 +9,8 @@ enum EnumSharedMemoryClientCommand CMD_SEND_BULLET_DATA_STREAM, CMD_CREATE_BOX_COLLISION_SHAPE, // CMD_DELETE_BOX_COLLISION_SHAPE, -// CMD_CREATE_RIGID_BODY, -// CMD_DELETE_RIGID_BODY, + CMD_CREATE_RIGID_BODY, + CMD_DELETE_RIGID_BODY, CMD_CREATE_SENSOR,///enable or disable joint feedback for force/torque sensors // CMD_REQUEST_SENSOR_MEASUREMENTS,//see CMD_REQUEST_ACTUAL_STATE/CMD_ACTUAL_STATE_UPDATE_COMPLETED CMD_INIT_POSE, @@ -57,6 +57,19 @@ enum JointInfoFlags { JOINT_HAS_MOTORIZED_POWER=1, }; + +enum +{ + COLLISION_SHAPE_TYPE_BOX=1, + COLLISION_SHAPE_TYPE_CYLINDER_X, + COLLISION_SHAPE_TYPE_CYLINDER_Y, + COLLISION_SHAPE_TYPE_CYLINDER_Z, + COLLISION_SHAPE_TYPE_CAPSULE_X, + COLLISION_SHAPE_TYPE_CAPSULE_Y, + COLLISION_SHAPE_TYPE_CAPSULE_Z, + COLLISION_SHAPE_TYPE_SPHERE +}; + struct b3JointInfo { char* m_linkName; diff --git a/examples/ThirdPartyLibs/Gwen/Gwen.h b/examples/ThirdPartyLibs/Gwen/Gwen.h index 85f3cc2cd..6d9f7a019 100644 --- a/examples/ThirdPartyLibs/Gwen/Gwen.h +++ b/examples/ThirdPartyLibs/Gwen/Gwen.h @@ -1,27 +1,17 @@ /* - GWEN - Copyright (c) 2010 Facepunch Studios +GWEN modified and adapted for the Bullet Physics Library +Using the Zlib license with permission from Garry Newman - MIT License - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. +Copyright (c) 2010 Facepunch Studios +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 GWEN_GWEN_H diff --git a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h index 5808bdff0..77cdc7b7a 100644 --- a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h +++ b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h @@ -46,7 +46,11 @@ struct b3GpuChildShape { b3Float4 m_childPosition; b3Quat m_childOrientation; - int m_shapeIndex;//used for SHAPE_COMPOUND_OF_CONVEX_HULLS + union + { + int m_shapeIndex;//used for SHAPE_COMPOUND_OF_CONVEX_HULLS + int m_capsuleAxis; + }; union { float m_radius;//used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index aa71e7180..1eeb98178 100644 --- a/test/SharedMemory/premake4.lua +++ b/test/SharedMemory/premake4.lua @@ -1,4 +1,4 @@ - project ("Test_SharedMemoryPhysicsClient") +project ("Test_SharedMemoryPhysicsClient") language "C++" kind "ConsoleApp" @@ -25,4 +25,57 @@ "../../examples/Utils/b3ResourcePath.cpp", "../../examples/Utils/b3ResourcePath.h", } + +project ("Test_PhysicsLoopBack") + + language "C++" + kind "ConsoleApp" + + includedirs {"../../src", "../../examples/SharedMemory", + "../../examples/ThirdPartyLibs"} + defines {"PHYSICS_LOOP_BACK"} + links { + "BulletFileLoader", + "BulletWorldImporter", + "Bullet3Common", + "BulletDynamics", + "BulletCollision", + "LinearMath" + } + + files { + "test.c", + "../../examples/SharedMemory/PhysicsClient.cpp", + "../../examples/SharedMemory/PhysicsClient.h", + "../../examples/SharedMemory/PhysicsServer.cpp", + "../../examples/SharedMemory/PhysicsServer.h", + "../../examples/SharedMemory/PhysicsLoopback.cpp", + "../../examples/SharedMemory/PhysicsLoopback.h", + "../../examples/SharedMemory/PhysicsLoopbackC_Api.cpp", + "../../examples/SharedMemory/PhysicsLoopbackC_Api.h", + "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp", + "../../examples/SharedMemory/PhysicsClientSharedMemory.h", + "../../examples/SharedMemory/PhysicsClientC_API.cpp", + "../../examples/SharedMemory/PhysicsClientC_API.h", + "../../examples/SharedMemory/Win32SharedMemory.cpp", + "../../examples/SharedMemory/Win32SharedMemory.h", + "../../examples/SharedMemory/PosixSharedMemory.cpp", + "../../examples/SharedMemory/PosixSharedMemory.h", + "../../examples/Utils/b3ResourcePath.cpp", + "../../examples/Utils/b3ResourcePath.h", + "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp", + "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp", + "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h", + "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", + "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp", + "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", + "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", + "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", + "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", + "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", + "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", + } \ No newline at end of file diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index 1c7fd4cfb..d3939583f 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -1,13 +1,14 @@ //#include "SharedMemoryCommands.h" #include "PhysicsClientC_API.h" + +#ifdef PHYSICS_LOOP_BACK +#include "PhysicsLoopbackC_API.h" +#endif //PHYSICS_LOOP_BACK + #include "SharedMemoryPublic.h" #include "Bullet3Common/b3Logging.h" #include -struct test -{ - int unused; -}; #include @@ -24,12 +25,18 @@ int main(int argc, char* argv[]) int imuLinkIndex = -1; - b3PhysicsClientHandle sm; + b3PhysicsClientHandle sm=0; int bodyIndex = -1; printf("hello world\n"); - +#ifdef PHYSICS_LOOP_BACK + sm = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY); +#else sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY); +#endif + + + if (b3CanSubmitCommand(sm)) { { @@ -114,7 +121,7 @@ int main(int argc, char* argv[]) { int statusType; - b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm); + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm,bodyIndex); b3SharedMemoryStatusHandle statusHandle; statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); @@ -150,35 +157,35 @@ int main(int argc, char* argv[]) } { - b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm)); - } + b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm,bodyIndex)); + if (sensorJointIndexLeft>=0) + { + + struct b3JointSensorState sensorState; + b3GetJointState(sm,state,sensorJointIndexLeft,&sensorState); + + b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft, + sensorState.m_jointForceTorque[0], + sensorState.m_jointForceTorque[1], + sensorState.m_jointForceTorque[2]); + + } + if (sensorJointIndexRight>=0) + { + struct b3JointSensorState sensorState; + b3GetJointState(sm,state,sensorJointIndexRight,&sensorState); + + b3GetJointInfo(sm,bodyIndex,sensorJointIndexRight,&sensorState); + b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight, + sensorState.m_jointForceTorque[0], + sensorState.m_jointForceTorque[1], + sensorState.m_jointForceTorque[2]); - if (sensorJointIndexLeft>=0) - { - - struct b3JointSensorState info; - - b3GetJointInfo(sm,bodyIndex,sensorJointIndexLeft,&info); - b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft, - info.m_jointForceTorque[0], - info.m_jointForceTorque[1], - info.m_jointForceTorque[2]); - - } + } + } - if (sensorJointIndexRight>=0) - { - struct b3JointSensorState info; - - b3GetJointInfo(sm,bodyIndex,sensorJointIndexRight,&info); - b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight, - info.m_jointForceTorque[0], - info.m_jointForceTorque[1], - info.m_jointForceTorque[2]); - - } { b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));