From c521d816c6e3c7574d330df40e1f3bbffa9eded9 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 14 Nov 2016 07:39:34 -0800 Subject: [PATCH] add user debug line/text features in pybullet + shared memory API: addUserDebugLine, addUserDebugText removeUserDebugItem removeAllUserDebugItems --- data/multibody.bullet | Bin 16496 -> 14712 bytes .../CommonGUIHelperInterface.h | 23 +- examples/ExampleBrowser/OpenGLGuiHelper.h | 18 ++ examples/RoboticsLearning/b3RobotSimAPI.cpp | 15 + examples/SharedMemory/PhysicsClientC_API.cpp | 103 +++++++ examples/SharedMemory/PhysicsClientC_API.h | 12 +- .../PhysicsClientSharedMemory.cpp | 9 + .../PhysicsServerCommandProcessor.cpp | 94 ++++++- .../SharedMemory/PhysicsServerExample.cpp | 220 ++++++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 36 +++ examples/SharedMemory/SharedMemoryPublic.h | 5 + .../TinyRendererVisualShapeConverter.cpp | 14 +- .../TinyRendererVisualShapeConverter.h | 2 +- examples/SharedMemory/udp/main.cpp | 14 +- examples/SoftDemo/SoftDemo.cpp | 4 +- .../main_tinyrenderer_single_example.cpp | 14 + examples/pybullet/pybullet.c | 261 +++++++++++++++++- 17 files changed, 809 insertions(+), 35 deletions(-) diff --git a/data/multibody.bullet b/data/multibody.bullet index c96db53b5cb53cb6128258c7b04bb07a1ebec1dd..90aa451573fd7c8611a929f7947ef633b70b227f 100644 GIT binary patch delta 2243 zcmbtVU1(HS5T2XdCi|17YqG&@Y}3`oQj3OINt=-UcTHDK&6>2`2HO%_Nf#;_7wLc5 zTLf7}B$wNVY@i^dkHxx(+6Om{FCt|PfkM~5<)z@01S!P_!G3ejJ^Lp@iRUtV&iv1r zGc&tpXb&<+V(*TQ?s;c=$LFsP4#ZQV@iY-N*{~hE2AI!3A0{%w^Z9(}OfDAy3F|_X zX}3$GAqJ+n$}bmhmr6lC&*vwhb1U}y{kjwGF?>}c{=>b@uMQejutz`*7uGr1FS05Wl@f!Sh*-1pfl_M z)D~+i@K&?Xm3?wOLZW7=Y;leHBj~E%B=aftt<#RpeQZMk(P!?DKCE#}qAMdWj3|HB zc4x27)q#ADcPl*=M->NqD()&T45PIJ3h=zPGopz^G&u;XA{W>@0kt_|hkk$+cl$TE z%L{?!Y)eJSX`OIyG_Sj?^X{G(!5lscNoCQ6GlQ^bZD{Yb&Pfx)AXcQ@me$re$rOH} zXZcV`gS-w8C-fMGRH?{Z#y4W$Jr>*NL#S;yGrS6@job;`X%;%O_w|ABKl*_ERzx_5 znFfBtQV#295F7^W0K^CaUTg@6@kJFNmmrTMIU`*>nZz>I4pz&VBjtG|TeJFLELrpT zBFM)c-_zc^4RVG`f$H(b6o@BSUxAkwJQ0qC*Lo7b^~8zLxCX}93gS(E{ec*Nj67`u zw(>Ai2~eHim;kpG^>mp_`Z_xN8}GeJh5WtKyBK}196Zo>4Hcl%Tq;#O^~=^!K2;9# zIA6IL3tj2`PL$_G-0k1Bw$K+S*CXI_lV--==nL_sKTVa3Bjja;MJR}JVgtwxoGJK! z70}LV@dMGFfj(}W%dd1S`?DVECl5ZqfPR{y2pMqLPf1XwXcoVN_`H;^G{Yq(6>r%5r@bFhCEE$y6?$Vr=(^ko$-O)b=?s09@(%#37|Eih8=oz9AwHO#QV4XGr2@WqgbCCrCC^dOYn zmn?`yMF?SnKIqeiJrye=11Wq^g1~xN4?$33=iKjpyZ4$nMYD*`<=Z*up5O1D^PO+s zu4H?Kc>URMxUr?Z?P^u^;ZSQhGzI`RT>EhexDILXK|&HCD=~gjD%PN1f6t@<0hv)* z>COPb^Bm?Hh~?SX!xZA=z`(da=YTo>tn*Qs6r*+fj=7NVU?!AQuNt)PDlbbTsXHhb&yRjFK z&Gx9w{1qVeQU}V8$Y66YnnVG~BDm;1sEzg=B}NWI$7f@np{W#R=oL-O7M89z&Yk`t zoIgHo$_#uH{N36T&mVtn{?U5bGROHgu+9qz#8PJ&-DT8$rI~P-E71>-(rOGyUuz~i zTItMAQ^VHlQd0&fQ;O1z!+IDyR7Bp7iyx;=rkya-GQ6vd!HH7S@OH@h-J}n5?#5!hJHIt1 zc`B#I)j0PTM~xJC9kgFGxTHqx4Y>_)pB!^z z&Kp=aZ9((G+Up(~uiX;EiC-|wX^!QjUriMM%!yzmpq_VbW!dyNeZ{V+2Q@hnhA&hv zI7^v0!^B6;VwXi6M{A3?2tAbI;)SHbk~!Yfm<(U|E)%Xpet|FINw48_{mB_LcB9>1 zlb=GQIVir^<$KAGQf+Hu4-qLY;1~s(0$0ROSH6n-p7L0C*vJ{fzJt*^#E1Dm6k>S) z5^cbY-Yx+7TAKP^eIj}e&}ZT$ql?B|GWwHd19R8Ch#6>#`oeCbT@sIe6Uik{#PzItZ zRBns6Kmx~H03Q#?;J`=3<%8`|1J!t5!{9}Vp3Nu^!HggZx8WR&K?h`oVY{f=0!qkJ zHz>k1^a(w}TlfJPK@qxb3EQY61D%2bedwbdm_type) { + case CMD_BULLET_LOADING_COMPLETED: case CMD_SDF_LOADING_COMPLETED: { int i,maxBodies; @@ -1062,6 +1063,107 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l } + +/// Add/remove user-specific debug lines and debug text messages +b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type =CMD_USER_DEBUG_DRAW; + command->m_updateFlags = USER_DEBUG_HAS_LINE; //USER_DEBUG_HAS_TEXT + + command->m_userDebugDrawArgs.m_debugLineFromXYZ[0] = fromXYZ[0]; + command->m_userDebugDrawArgs.m_debugLineFromXYZ[1] = fromXYZ[1]; + command->m_userDebugDrawArgs.m_debugLineFromXYZ[2] = fromXYZ[2]; + + command->m_userDebugDrawArgs.m_debugLineToXYZ[0] = toXYZ[0]; + command->m_userDebugDrawArgs.m_debugLineToXYZ[1] = toXYZ[1]; + command->m_userDebugDrawArgs.m_debugLineToXYZ[2] = toXYZ[2]; + + command->m_userDebugDrawArgs.m_debugLineColorRGB[0] = colorRGB[0]; + command->m_userDebugDrawArgs.m_debugLineColorRGB[1] = colorRGB[1]; + command->m_userDebugDrawArgs.m_debugLineColorRGB[2] = colorRGB[2]; + + command->m_userDebugDrawArgs.m_lineWidth = lineWidth; + command->m_userDebugDrawArgs.m_lifeTime = lifeTime; + + return (b3SharedMemoryCommandHandle) command; +} + +b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime) +{ + + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type =CMD_USER_DEBUG_DRAW; + command->m_updateFlags = USER_DEBUG_HAS_TEXT; + + int len = strlen(txt); + if (lenm_userDebugDrawArgs.m_text,txt); + } else + { + command->m_userDebugDrawArgs.m_text[0] = 0; + } + command->m_userDebugDrawArgs.m_textPositionXYZ[0] = positionXYZ[0]; + command->m_userDebugDrawArgs.m_textPositionXYZ[1] = positionXYZ[1]; + command->m_userDebugDrawArgs.m_textPositionXYZ[2] = positionXYZ[2]; + + command->m_userDebugDrawArgs.m_textColorRGB[0] = colorRGB[0]; + command->m_userDebugDrawArgs.m_textColorRGB[1] = colorRGB[1]; + command->m_userDebugDrawArgs.m_textColorRGB[2] = colorRGB[2]; + + command->m_userDebugDrawArgs.m_textSize = textSize; + + command->m_userDebugDrawArgs.m_lifeTime = lifeTime; + + return (b3SharedMemoryCommandHandle) command; +} + +b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type =CMD_USER_DEBUG_DRAW; + command->m_updateFlags = USER_DEBUG_REMOVE_ONE_ITEM; + command->m_userDebugDrawArgs.m_removeItemUniqueId = debugItemUniqueId; + return (b3SharedMemoryCommandHandle) command; + +} + +b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type =CMD_USER_DEBUG_DRAW; + command->m_updateFlags = USER_DEBUG_REMOVE_ALL; + return (b3SharedMemoryCommandHandle) command; +} + +int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + btAssert(status->m_type == CMD_USER_DEBUG_DRAW_COMPLETED); + if (status->m_type != CMD_USER_DEBUG_DRAW_COMPLETED) + return -1; + + return status->m_userDebugDrawArgs.m_debugItemUniqueId; +} + + ///request an image from a simulated camera, using a software renderer. b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient) { @@ -1627,6 +1729,7 @@ b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle return (b3SharedMemoryCommandHandle)command; } + int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian) { const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index c22b461e5..e82b6824b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -74,14 +74,22 @@ int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); -///Request debug lines for debug visualization. The flags in debugMode are the same as used in Bullet +///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet ///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode); -///Get the pointers to the debug line information, after b3InitRequestDebugLinesCommand returns +///Get the pointers to the physics debug line information, after b3InitRequestDebugLinesCommand returns ///status CMD_DEBUG_LINES_COMPLETED void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines); +/// Add/remove user-specific debug lines and debug text messages +b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime); +b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime); +b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId); +b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient); +///All debug items unique Ids are positive: a negative unique Id means failure. +int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle); + ///request an image from a simulated camera, using a software renderer. b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient); void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 03395317c..e2b072b44 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -722,6 +722,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Load .mjcf failed"); break; } + case CMD_USER_DEBUG_DRAW_COMPLETED: + { + break; + } + case CMD_USER_DEBUG_DRAW_FAILED: + { + b3Warning("User debug draw failed"); + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6ab941616..b51d5c736 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3363,9 +3363,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; - m_data->m_visualConverter.loadTextureFile(clientCmd.m_loadTextureArguments.m_textureFileName); + int uid = m_data->m_visualConverter.loadTextureFile(clientCmd.m_loadTextureArguments.m_textureFileName); - serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED; + if (uid>=0) + { + serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED; + } else + { + serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; + } hasStatus = true; break; @@ -3403,6 +3409,34 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm bool ok = importer->loadFile(relativeFileName); if (ok) { + + + int numRb = importer->getNumRigidBodies(); + serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0; + + for( int i=0;igetRigidBodyByIndex(i); + + if (colObj) + { + btRigidBody* rb = btRigidBody::upcast(colObj); + if (rb) + { + int bodyUniqueId = m_data->allocHandle(); + InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); + colObj->setUserIndex2(bodyUniqueId); + bodyHandle->m_rigidBody = rb; + + if (serverStatusOut.m_sdfLoadedArgs.m_numBodiesm_guiHelper->autogenerateGraphicsObjects(m_data->m_dynamicsWorld); hasStatus = true; @@ -3441,6 +3475,62 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } + case CMD_USER_DEBUG_DRAW: + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED; + hasStatus = true; + + + if (clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT) + { + int uid = m_data->m_guiHelper->addUserDebugText3D(clientCmd.m_userDebugDrawArgs.m_text, + clientCmd.m_userDebugDrawArgs.m_textPositionXYZ, + clientCmd.m_userDebugDrawArgs.m_textColorRGB, + clientCmd.m_userDebugDrawArgs.m_textSize, + clientCmd.m_userDebugDrawArgs.m_lifeTime); + + if (uid>=0) + { + serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + } + + if (clientCmd.m_updateFlags & USER_DEBUG_HAS_LINE) + { + int uid = m_data->m_guiHelper->addUserDebugLine( + clientCmd.m_userDebugDrawArgs.m_debugLineFromXYZ, + clientCmd.m_userDebugDrawArgs.m_debugLineToXYZ, + clientCmd.m_userDebugDrawArgs.m_debugLineColorRGB, + clientCmd.m_userDebugDrawArgs.m_lineWidth, + clientCmd.m_userDebugDrawArgs.m_lifeTime); + + if (uid>=0) + { + serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + } + + + if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ALL) + { + m_data->m_guiHelper->removeAllUserDebugItems(); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + + } + if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM) + { + m_data->m_guiHelper->removeUserDebugItem(clientCmd.m_userDebugDrawArgs.m_removeItemUniqueId); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + + } + + break; + } + + default: { b3Error("Unknown command encountered"); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 828fab3f4..4e3ccf253 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -75,6 +75,10 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIHelperRemoveAllGraphicsInstances, eGUIHelperCopyCameraImageData, eGUIHelperAutogenerateGraphicsObjects, + eGUIUserDebugAddText, + eGUIUserDebugAddLine, + eGUIUserDebugRemoveItem, + eGUIUserDebugRemoveAllItems, }; #include @@ -271,17 +275,43 @@ void* MotionlsMemoryFunc() +struct UserDebugDrawLine +{ + double m_debugLineFromXYZ[3]; + double m_debugLineToXYZ[3]; + double m_debugLineColorRGB[3]; + double m_lineWidth; + + double m_lifeTime; + int m_itemUniqueId; +}; + +struct UserDebugText +{ + char m_text[1024]; + double m_textPositionXYZ[3]; + double m_textColorRGB[3]; + double textSize; + + double m_lifeTime; + int m_itemUniqueId; +}; + + + class MultiThreadedOpenGLGuiHelper : public GUIHelperInterface { CommonGraphicsApp* m_app; b3CriticalSection* m_cs; - + + public: GUIHelperInterface* m_childGuiHelper; + int m_uidGenerator; const unsigned char* m_texels; int m_textureWidth; int m_textureHeight; @@ -305,6 +335,7 @@ public: MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper) :m_app(app) ,m_cs(0), + m_uidGenerator(0), m_texels(0), m_textureId(-1) { @@ -548,6 +579,96 @@ public: virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size) { } + + + + + btAlignedObjectArray m_userDebugText; + + UserDebugText m_tmpText; + + virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double textColorRGB[3], double size, double lifeTime) + { + + m_tmpText.m_itemUniqueId = m_uidGenerator++; + m_tmpText.m_lifeTime = lifeTime; + m_tmpText.textSize = size; + int len = strlen(txt); + strcpy(m_tmpText.m_text,txt); + m_tmpText.m_textPositionXYZ[0] = positionXYZ[0]; + m_tmpText.m_textPositionXYZ[1] = positionXYZ[1]; + m_tmpText.m_textPositionXYZ[2] = positionXYZ[2]; + m_tmpText.m_textColorRGB[0] = textColorRGB[0]; + m_tmpText.m_textColorRGB[1] = textColorRGB[1]; + m_tmpText.m_textColorRGB[2] = textColorRGB[2]; + + m_cs->lock(); + m_cs->setSharedParam(1, eGUIUserDebugAddText); + m_cs->unlock(); + while (m_cs->getSharedParam(1) != eGUIHelperIdle) + { + b3Clock::usleep(150); + } + + return m_userDebugText[m_userDebugText.size()-1].m_itemUniqueId; + } + + btAlignedObjectArray m_userDebugLines; + UserDebugDrawLine m_tmpLine; + + virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime ) + { + m_tmpLine.m_lifeTime = lifeTime; + m_tmpLine.m_lineWidth = lineWidth; + m_tmpLine.m_itemUniqueId = m_uidGenerator++; + m_tmpLine.m_debugLineFromXYZ[0] = debugLineFromXYZ[0]; + m_tmpLine.m_debugLineFromXYZ[1] = debugLineFromXYZ[1]; + m_tmpLine.m_debugLineFromXYZ[2] = debugLineFromXYZ[2]; + + m_tmpLine.m_debugLineToXYZ[0] = debugLineToXYZ[0]; + m_tmpLine.m_debugLineToXYZ[1] = debugLineToXYZ[1]; + m_tmpLine.m_debugLineToXYZ[2] = debugLineToXYZ[2]; + + m_tmpLine.m_debugLineColorRGB[0] = debugLineColorRGB[0]; + m_tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1]; + m_tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2]; + + m_cs->lock(); + m_cs->setSharedParam(1, eGUIUserDebugAddLine); + m_cs->unlock(); + while (m_cs->getSharedParam(1) != eGUIHelperIdle) + { + b3Clock::usleep(150); + } + return m_userDebugLines[m_userDebugLines.size()-1].m_itemUniqueId; + } + + int m_removeDebugItemUid; + + virtual void removeUserDebugItem( int debugItemUniqueId) + { + m_removeDebugItemUid = debugItemUniqueId; + m_cs->lock(); + m_cs->setSharedParam(1, eGUIUserDebugRemoveItem); + m_cs->unlock(); + while (m_cs->getSharedParam(1) != eGUIHelperIdle) + { + b3Clock::usleep(150); + } + + } + virtual void removeAllUserDebugItems( ) + { + m_cs->lock(); + m_cs->setSharedParam(1, eGUIUserDebugRemoveAllItems); + m_cs->unlock(); + while (m_cs->getSharedParam(1) != eGUIHelperIdle) + { + b3Clock::usleep(150); + } + + } + }; @@ -935,10 +1056,68 @@ void PhysicsServerExample::stepSimulation(float deltaTime) m_multiThreadedHelper->getCriticalSection()->unlock(); break; } + + case eGUIUserDebugAddText: + { + m_multiThreadedHelper->m_userDebugText.push_back(m_multiThreadedHelper->m_tmpText); + m_multiThreadedHelper->getCriticalSection()->lock(); + m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle); + m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } + case eGUIUserDebugAddLine: + { + m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine); + m_multiThreadedHelper->getCriticalSection()->lock(); + m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle); + m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } + case eGUIUserDebugRemoveItem: + { + for (int i=0;im_userDebugLines.size();i++) + { + if (m_multiThreadedHelper->m_userDebugLines[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid) + { + m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1); + m_multiThreadedHelper->m_userDebugLines.pop_back(); + break; + } + } + + + for (int i=0;im_userDebugText.size();i++) + { + if (m_multiThreadedHelper->m_userDebugText[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid) + { + m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1); + m_multiThreadedHelper->m_userDebugText.pop_back(); + break; + } + } + + m_multiThreadedHelper->getCriticalSection()->lock(); + m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle); + m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } + case eGUIUserDebugRemoveAllItems: + { + m_multiThreadedHelper->m_userDebugLines.clear(); + m_multiThreadedHelper->m_userDebugText.clear(); + m_multiThreadedHelper->m_uidGenerator = 0; + m_multiThreadedHelper->getCriticalSection()->lock(); + m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle); + m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } case eGUIHelperIdle: + { + break; + } default: { - + btAssert(0); } } @@ -993,15 +1172,38 @@ void PhysicsServerExample::renderScene() //add array of lines //draw all user- 'text3d' messages - if (m_guiHelper) + if (m_multiThreadedHelper) { - btVector3 from(0, 0, 0); - btVector3 toX(1, 1, 1); - btVector3 color(0, 1, 0); - double width = 2; - m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, width); + + for (int i=0;im_userDebugLines.size();i++) + { + btVector3 from; + from.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[0], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[1], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[2]); + btVector3 toX; + toX.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[0], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[1], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[2]); + + btVector3 color; + color.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[2]); - m_guiHelper->getAppInterface()->drawText3D("hi", 1, 1, 1, 1); + + m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth); + } + + for (int i=0;im_userDebugText.size();i++) + { + m_guiHelper->getAppInterface()->drawText3D(m_multiThreadedHelper->m_userDebugText[i].m_text, + m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[0], + m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[1], + m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[2], + m_multiThreadedHelper->m_userDebugText[i].textSize); + + } } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 82925c8f8..69b7916a1 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -500,6 +500,40 @@ struct CreateJointArgs int m_jointType; }; + + +enum EnumUserDebugDrawFlags +{ + USER_DEBUG_HAS_LINE=1, + USER_DEBUG_HAS_TEXT=2, + USER_DEBUG_REMOVE_ONE_ITEM=4, + USER_DEBUG_REMOVE_ALL=8 +}; + +struct UserDebugDrawArgs +{ + double m_debugLineFromXYZ[3]; + double m_debugLineToXYZ[3]; + double m_debugLineColorRGB[3]; + double m_lineWidth; + + double m_lifeTime; + int m_removeItemUniqueId; + + char m_text[MAX_FILENAME_LENGTH]; + double m_textPositionXYZ[3]; + double m_textColorRGB[3]; + double m_textSize; +}; + + + +struct UserDebugDrawResultArgs +{ + int m_debugItemUniqueId; +}; + + struct SharedMemoryCommand { int m_type; @@ -536,6 +570,7 @@ struct SharedMemoryCommand struct UpdateVisualShapeDataArgs m_updateVisualShapeDataArguments; struct LoadTextureArgs m_loadTextureArguments; struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments; + struct UserDebugDrawArgs m_userDebugDrawArgs; struct LoadBunnyArgs m_loadBunnyArguments; }; }; @@ -584,6 +619,7 @@ struct SharedMemoryStatus struct SendOverlappingObjectsArgs m_sendOverlappingObjectsArgs; struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs; struct SendVisualShapeDataArgs m_sendVisualShapeArgs; + struct UserDebugDrawResultArgs m_userDebugDrawArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 47c7e7280..1c2e5835a 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -40,6 +40,8 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_VISUAL_SHAPE_INFO, CMD_UPDATE_VISUAL_SHAPE, CMD_LOAD_TEXTURE, + CMD_USER_DEBUG_DRAW, + //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -100,6 +102,8 @@ enum EnumSharedMemoryServerStatus CMD_VISUAL_SHAPE_UPDATE_FAILED, CMD_LOAD_TEXTURE_COMPLETED, CMD_LOAD_TEXTURE_FAILED, + CMD_USER_DEBUG_DRAW_COMPLETED, + CMD_USER_DEBUG_DRAW_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; @@ -239,6 +243,7 @@ struct b3VisualShapeData char m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN]; double m_localInertiaFrame[7];//pos[3], orn[4] //todo: add more data if necessary (material color etc, although material can be in asset file .obj file) + double m_rgbaColor[4]; }; struct b3VisualShapeInformation diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 9bf123413..643c7c1c0 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -527,7 +527,11 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const visualShape.m_localInertiaFrame[4] = localInertiaFrame.getRotation()[1]; visualShape.m_localInertiaFrame[5] = localInertiaFrame.getRotation()[2]; visualShape.m_localInertiaFrame[6] = localInertiaFrame.getRotation()[3]; - + visualShape.m_rgbaColor[0] = rgbaColor[0]; + visualShape.m_rgbaColor[1] = rgbaColor[1]; + visualShape.m_rgbaColor[2] = rgbaColor[2]; + visualShape.m_rgbaColor[3] = rgbaColor[3]; + convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape); m_data->m_visualShapes.push_back(visualShape); @@ -872,10 +876,14 @@ int TinyRendererVisualShapeConverter::registerTexture(unsigned char* texels, int return m_data->m_textures.size()-1; } -void TinyRendererVisualShapeConverter::loadTextureFile(const char* filename) +int TinyRendererVisualShapeConverter::loadTextureFile(const char* filename) { int width,height,n; unsigned char* image=0; image = stbi_load(filename, &width, &height, &n, 3); - registerTexture(image, width, height); + if (image && (width>=0) && (height>=0)) + { + return registerTexture(image, width, height); + } + return -1; } \ No newline at end of file diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h index afd0a2ae6..48c5c1079 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -38,7 +38,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter void render(); void render(const float viewMat[16], const float projMat[16]); - void loadTextureFile(const char* filename); + int loadTextureFile(const char* filename); int registerTexture(unsigned char* texels, int width, int height); void activateShapeTexture(int shapeUniqueId, int textureUniqueId); void activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId); diff --git a/examples/SharedMemory/udp/main.cpp b/examples/SharedMemory/udp/main.cpp index 39ae4ea79..bc8c6c57d 100644 --- a/examples/SharedMemory/udp/main.cpp +++ b/examples/SharedMemory/udp/main.cpp @@ -94,7 +94,8 @@ int main(int argc, char *argv[]) switch (event.type) { - case ENET_EVENT_TYPE_CONNECT: + case ENET_EVENT_TYPE_CONNECT: + { printf("A new client connected from %x:%u.\n", event.peer->address.host, event.peer->address.port); @@ -103,8 +104,9 @@ int main(int argc, char *argv[]) event.peer->data = (void*)"Client information"; break; - + } case ENET_EVENT_TYPE_RECEIVE: + { if (gVerboseNetworkMessagesServer) { printf("A packet of length %u containing '%s' was " @@ -171,8 +173,9 @@ int main(int argc, char *argv[]) //enet_host_broadcast(server, 0, event.packet); break; - + } case ENET_EVENT_TYPE_DISCONNECT: + { printf("%s disconnected.\n", event.peer->data); /* Reset the peer's client information. */ @@ -180,6 +183,11 @@ int main(int argc, char *argv[]) event.peer->data = NULL; break; + } + default: + { + + } } } else if (serviceResult > 0) diff --git a/examples/SoftDemo/SoftDemo.cpp b/examples/SoftDemo/SoftDemo.cpp index 7a046f9c4..49b59e444 100644 --- a/examples/SoftDemo/SoftDemo.cpp +++ b/examples/SoftDemo/SoftDemo.cpp @@ -2131,12 +2131,12 @@ void SoftDemo::initPhysics() for (int j=0;j MAX_SDF_BODIES) { + PyErr_SetString(SpamError, "loadBullet exceeds body capacity"); + return NULL; + } + + pylist = PyTuple_New(numBodies); + + if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) { + for (i = 0; i < numBodies; i++) { + PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i])); + } + } + return pylist; + + } static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args) @@ -366,7 +383,7 @@ static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) { return v; } -#define MAX_SDF_BODIES 512 + static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args) { const char* sdfFileName = ""; @@ -1376,12 +1393,14 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) { // // // Args: // vector - float[3] which will be set by values from objMat -static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) { +static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) { int i, len; PyObject* seq; + if (objVec==NULL) + return 0; - seq = PySequence_Fast(objMat, "expected a sequence"); - len = PySequence_Size(objMat); + seq = PySequence_Fast(objVec, "expected a sequence"); + len = PySequence_Size(objVec); if (len == 3) { for (i = 0; i < len; i++) { vector[i] = pybullet_internalGetFloatFromSequence(seq, i); @@ -1397,7 +1416,9 @@ static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) { static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { int i, len; PyObject* seq; - + if (obVec==NULL) + return 0; + seq = PySequence_Fast(obVec, "expected a sequence"); len = PySequence_Size(obVec); if (len == 3) { @@ -1415,7 +1436,9 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) { int i, len; PyObject* seq; - + if (obVec==NULL) + return 0; + seq = PySequence_Fast(obVec, "expected a sequence"); len = PySequence_Size(obVec); if (len == 4) { @@ -1429,6 +1452,196 @@ static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) { return 0; } + + +static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject *keywds) +{ + int size = PySequence_Size(args); + + b3SharedMemoryCommandHandle commandHandle; + struct b3ContactInformation contactPointData; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int res = 0; + + PyObject* pyResultList = 0; + char* text; + double posXYZ[3]; + double colorRGB[3]={1,1,1}; + + + PyObject* textPositionObj=0; + PyObject* textColorRGBObj=0; + double textSize = 1.f; + double lifeTime = 0.f; + + static char *kwlist[] = { "text", "textPosition", "textColorRGB", "textSize", "lifeTime", NULL }; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "sO|Odd", kwlist, &text, &textPositionObj, &textColorRGBObj,&textSize, &lifeTime)) + { + return NULL; + } + + res = pybullet_internalSetVectord(textPositionObj,posXYZ); + if (!res) + { + PyErr_SetString(SpamError, "Error converting lineFrom[3]"); + return NULL; + } + + if (textColorRGBObj) + { + res = pybullet_internalSetVectord(textColorRGBObj,colorRGB); + if (!res) + { + PyErr_SetString(SpamError, "Error converting lineTo[3]"); + return NULL; + } + } + + + commandHandle = b3InitUserDebugDrawAddText3D(sm,text,posXYZ,colorRGB,textSize,lifeTime); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) + { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + PyObject* item = PyInt_FromLong(debugItemUniqueId); + return item; + } + + PyErr_SetString(SpamError, "Error in addUserDebugText."); + return NULL; +} + + +static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObject *keywds) +{ + int size = PySequence_Size(args); + + b3SharedMemoryCommandHandle commandHandle; + struct b3ContactInformation contactPointData; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int res = 0; + + PyObject* pyResultList = 0; + + double fromXYZ[3]; + double toXYZ[3]; + double colorRGB[3]={1,1,1}; + + PyObject* lineFromObj=0; + PyObject* lineToObj=0; + PyObject* lineColorRGBObj=0; + double lineWidth = 1.f; + double lifeTime = 0.f; + + static char *kwlist[] = { "lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", NULL }; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Odd", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj,&lineWidth, &lifeTime)) + { + return NULL; + } + + res = pybullet_internalSetVectord(lineFromObj,fromXYZ); + if (!res) + { + PyErr_SetString(SpamError, "Error converting lineFrom[3]"); + return NULL; + } + + res = pybullet_internalSetVectord(lineToObj,toXYZ); + if (!res) + { + PyErr_SetString(SpamError, "Error converting lineTo[3]"); + return NULL; + } + if (lineColorRGBObj) + { + res = pybullet_internalSetVectord(lineColorRGBObj,colorRGB); + } + + commandHandle = b3InitUserDebugDrawAddLine3D(sm,fromXYZ,toXYZ,colorRGB,lineWidth,lifeTime); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) + { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + PyObject* item = PyInt_FromLong(debugItemUniqueId); + return item; + } + + PyErr_SetString(SpamError, "Error in addUserDebugLine."); + return NULL; +} + + + + +static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, PyObject *keywds) +{ + b3SharedMemoryCommandHandle commandHandle; + struct b3ContactInformation contactPointData; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int itemUniqueId; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (!PyArg_ParseTuple(args, "i", &itemUniqueId)) { + PyErr_SetString(SpamError, "Error parsing user debug item unique id"); + return NULL; + } + + commandHandle = b3InitUserDebugDrawRemove(sm,itemUniqueId); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + Py_INCREF(Py_None); + return Py_None; +} + +static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args, PyObject *keywds) +{ + b3SharedMemoryCommandHandle commandHandle; + struct b3ContactInformation contactPointData; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + commandHandle = b3InitUserDebugDrawRemoveAll(sm); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + + Py_INCREF(Py_None); + return Py_None; +} + + + static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args) { int size = PySequence_Size(args); @@ -1509,7 +1722,7 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args) PyTuple_SetItem(visualShapeObList, 6, vec); } - + visualShapeInfo.m_visualShapeData[0].m_rgbaColor[0]; PyTuple_SetItem(pyResultList, i, visualShapeObList); } @@ -2687,6 +2900,30 @@ static PyMethodDef SpamMethods[] = { "axis-aligned bounding box volume (AABB)." "Input are two vectors defining the AABB in world space [min_x,min_y,min_z],[max_x,max_y,max_z]." }, + + + { "addUserDebugLine", (PyCFunction)pybullet_addUserDebugLine, METH_VARARGS | METH_KEYWORDS, + "Add a user debug draw line with lineFrom[3], lineTo[3], lineColorRGB[3], lineWidth, lifeTime. " + "A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item." + }, + + + { "addUserDebugText", (PyCFunction)pybullet_addUserDebugText, METH_VARARGS | METH_KEYWORDS, + "Add a user debug draw line with text, textPosition[3], textSize and lifeTime in seconds " + "A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item." + }, + + { "removeUserDebugItem", (PyCFunction)pybullet_removeUserDebugItem, METH_VARARGS | METH_KEYWORDS, + "remove a user debug draw item, giving its unique id" + }, + + + { "removeAllUserDebugItems", (PyCFunction)pybullet_removeAllUserDebugItems, METH_VARARGS | METH_KEYWORDS, + "remove all user debug draw items" + }, + + + {"getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS, "Return the visual shape information for one object." },