From f28150c36894874598548684d876261cce109bae Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 24 Oct 2015 13:48:53 -0700 Subject: [PATCH 1/9] catch setting of invalid joint angles in PhysicsClientC_Api use the proper m_bodyUniqueIndex (todo: fix this field, move to shared command data) --- examples/SharedMemory/PhysicsClientC_API.cpp | 12 ++++++++---- examples/SharedMemory/PhysicsServer.cpp | 2 +- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 1d76ae427..1a0acbcdb 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -313,12 +313,16 @@ 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; } diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 97d5aa55d..878eb57e5 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -1246,7 +1246,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) From 818037c011575bfc2a9962ea4445cd940bf108f9 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 24 Oct 2015 13:49:25 -0700 Subject: [PATCH 2/9] Fix issue related to CMD_RESET_SIMULATION --- examples/SharedMemory/PhysicsServer.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 878eb57e5..a9bdc787e 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -1301,8 +1301,9 @@ 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); From ec1947bbbd38d1068a04f6321290d343f1357ec4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 24 Oct 2015 13:50:11 -0700 Subject: [PATCH 3/9] add SIM_PARAM_UPDATE_DELTA_TIME flag to fix setting the time step in shared memory API --- examples/SharedMemory/PhysicsClientC_API.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 1a0acbcdb..2a81fcdab 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; } From 52edac4ef2c6afb495a60ab1afbc82153ed22dcb Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 24 Oct 2015 14:00:53 -0700 Subject: [PATCH 4/9] example Collision: catch case where no collision world is created --- examples/Collision/CollisionTutorialBullet2.cpp | 5 ++++- examples/Collision/Internal/RealTimeBullet3CollisionSdk.cpp | 3 +++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/examples/Collision/CollisionTutorialBullet2.cpp b/examples/Collision/CollisionTutorialBullet2.cpp index 2d3859ef3..78686a5cb 100644 --- a/examples/Collision/CollisionTutorialBullet2.cpp +++ b/examples/Collision/CollisionTutorialBullet2.cpp @@ -245,7 +245,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/Internal/RealTimeBullet3CollisionSdk.cpp b/examples/Collision/Internal/RealTimeBullet3CollisionSdk.cpp index b67bc3cba..a1de0fa32 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 @@ -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; From 163b797d8b9988d631d68d39203d4e79237379c6 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 25 Oct 2015 11:30:34 -0700 Subject: [PATCH 5/9] Update to ZLib license for GWEN files. Note that our copy of GWEN is modified and GWEN is not under active development anymore. We plan to integrate and modify GWEN further into the Bullet Example Browser or rewrite the GUI and drop GWEN fix cube.obj and cube.mtl to enable textures --- data/cube.mtl | 23 +++--- data/cube.obj | 72 ++++++++++++------- .../ImportObjDemo/ImportObjExample.cpp | 4 +- examples/ThirdPartyLibs/Gwen/Gwen.h | 32 +++------ 4 files changed, 73 insertions(+), 58 deletions(-) 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/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/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 From 57f50f177844e6471837b4afe64710c7ce02b446 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 26 Oct 2015 17:56:21 -0700 Subject: [PATCH 6/9] Create a PhysicsLoopBack that includes both PhysicsClient and PhysicsServer over shared memory. Add a test for PhysicsLoopBack. --- examples/SharedMemory/PhysicsClientC_API.cpp | 7 +- examples/SharedMemory/PhysicsClientC_API.h | 2 +- examples/SharedMemory/PhysicsLoopBack.cpp | 115 ++++++++++++++++++ examples/SharedMemory/PhysicsLoopBack.h | 58 +++++++++ examples/SharedMemory/PhysicsLoopBackC_API.h | 19 +++ .../SharedMemory/PhysicsLoopBackC_Api.cpp | 15 +++ test/SharedMemory/premake4.lua | 55 ++++++++- test/SharedMemory/test.c | 69 ++++++----- 8 files changed, 306 insertions(+), 34 deletions(-) create mode 100644 examples/SharedMemory/PhysicsLoopBack.cpp create mode 100644 examples/SharedMemory/PhysicsLoopBack.h create mode 100644 examples/SharedMemory/PhysicsLoopBackC_API.h create mode 100644 examples/SharedMemory/PhysicsLoopBackC_Api.cpp diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 2a81fcdab..06d1f6778 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -398,6 +398,7 @@ b3PhysicsClientHandle b3ConnectSharedMemory(int key) return (b3PhysicsClientHandle ) cl; } + void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -452,7 +453,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..23aa5f2d4 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); 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/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..ed32209d9 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)) { { @@ -150,35 +157,35 @@ int main(int argc, char* argv[]) } { - b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm)); - } + b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm)); + 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)); From 9cc2b1ec12645e2e67a44e0c5ddabdc63f0ab87d Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Tue, 27 Oct 2015 14:55:46 -0700 Subject: [PATCH 7/9] Physics SharedMemory: Add flags BOX_SHAPE_HAS_MASS and BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE to CMD_CREATE_RIGID_BODY (which maps to CMD_CREATE_BOX_COLLISION_SHAPE for backward compatibility for now) Need to add way to receive world transform --- examples/SharedMemory/PhysicsClientC_API.cpp | 53 +++++++++---- examples/SharedMemory/PhysicsClientC_API.h | 3 + .../SharedMemory/PhysicsClientExample.cpp | 20 ++++- examples/SharedMemory/PhysicsServer.cpp | 76 ++++++++++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 7 +- examples/SharedMemory/SharedMemoryPublic.h | 17 ++++- 6 files changed, 154 insertions(+), 22 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 06d1f6778..639191b14 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -246,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; @@ -328,20 +367,6 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar } -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) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 23aa5f2d4..df456c938 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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. diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index ab444279b..76e533914 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,6 +214,19 @@ 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); @@ -360,6 +373,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 +529,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/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index a9bdc787e..2ecc95c67 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -1309,6 +1309,7 @@ void PhysicsServerSharedMemory::processClientCommands() break; } + case CMD_CREATE_RIGID_BODY: case CMD_CREATE_BOX_COLLISION_SHAPE: { btVector3 halfExtents(1,1,1); @@ -1339,11 +1340,82 @@ 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); m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index b009dc760..49893f2be 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]; }; 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; From 2d5d89d99989c258a02ed3174ad23b0a24615deb Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Tue, 27 Oct 2015 15:46:13 -0700 Subject: [PATCH 8/9] Allow to request the state of a rigid body (position, orientation, lin/ang velocity) through shared memory API PhysicsClientC_API: b3RequestActualStateCommandInit requires a body unique Id as second argument --- examples/SharedMemory/PhysicsClientC_API.cpp | 9 ++- examples/SharedMemory/PhysicsClientC_API.h | 2 +- .../SharedMemory/PhysicsClientExample.cpp | 7 +- .../PhysicsClientSharedMemory.cpp | 5 ++ examples/SharedMemory/PhysicsServer.cpp | 65 +++++++++++++++---- examples/SharedMemory/SharedMemoryCommands.h | 5 ++ test/SharedMemory/test.c | 4 +- 7 files changed, 78 insertions(+), 19 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 639191b14..7553811bc 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -191,7 +191,7 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl } -b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient) +b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -199,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; } @@ -467,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); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index df456c938..5f008c47e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -100,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 76e533914..4308e054d 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -229,8 +229,11 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) 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; }; 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/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 2ecc95c67..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; @@ -1417,10 +1453,15 @@ void PhysicsServerSharedMemory::processClientCommands() 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 49893f2be..6016a3ba1 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -245,6 +245,10 @@ struct SharedMemoryCommand }; }; +struct RigidBodyCreateArgs +{ + int m_bodyUniqueId; +}; struct SharedMemoryStatus { @@ -258,6 +262,7 @@ struct SharedMemoryStatus struct BulletDataStreamArgs m_dataStreamArguments; struct SendActualStateArgs m_sendActualStateArgs; struct SendDebugLinesArgs m_sendDebugLinesArgs; + struct RigidBodyCreateArgs m_rigidBodyCreateArgs; }; }; diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index ed32209d9..d3939583f 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -121,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); @@ -157,7 +157,7 @@ int main(int argc, char* argv[]) } { - b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm)); + b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm,bodyIndex)); if (sensorJointIndexLeft>=0) { From dc81e31e74aabd2a6315b998ae85c0762dccf9e8 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Wed, 28 Oct 2015 11:48:36 -0700 Subject: [PATCH 9/9] upload work-in-progress Collision example using C-API. --- examples/Collision/CollisionSdkC_Api.cpp | 16 ++++ examples/Collision/CollisionSdkC_Api.h | 11 ++- .../Collision/CollisionTutorialBullet2.cpp | 87 +++++++++++++------ examples/Collision/CollisionTutorialBullet2.h | 6 +- .../Internal/Bullet2CollisionSdk.cpp | 46 ++++++++++ .../Collision/Internal/Bullet2CollisionSdk.h | 8 ++ .../Internal/CollisionSdkInterface.h | 8 ++ .../Internal/RealTimeBullet3CollisionSdk.cpp | 52 +++++++++-- .../Internal/RealTimeBullet3CollisionSdk.h | 7 ++ examples/ExampleBrowser/ExampleEntries.cpp | 8 +- .../shared/b3Collidable.h | 6 +- 11 files changed, 208 insertions(+), 47 deletions(-) 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 78686a5cb..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); 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 a1de0fa32..34b8e7eb0 100644 --- a/examples/Collision/Internal/RealTimeBullet3CollisionSdk.cpp +++ b/examples/Collision/Internal/RealTimeBullet3CollisionSdk.cpp @@ -27,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, }; @@ -152,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 @@ -206,7 +248,7 @@ plCollisionObjectHandle RealTimeBullet3CollisionSdk::createCollisionObject( plC } case RTB3_SHAPE_COMPOUND_INTERNAL: { - b3Assert(0); + break; } default: @@ -399,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/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