diff --git a/data/sphere2.urdf b/data/sphere2.urdf
index fb0108b3d..ec939e694 100644
--- a/data/sphere2.urdf
+++ b/data/sphere2.urdf
@@ -9,7 +9,7 @@
-
+
@@ -19,55 +19,5 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h
index b9a4dfd26..f76414126 100644
--- a/examples/CommonInterfaces/CommonGUIHelperInterface.h
+++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h
@@ -29,9 +29,10 @@ struct GUIHelperInterface
virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld)=0;
- virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices) =0;
-
+ virtual int registerTexture(const unsigned char* texels, int width, int height)=0;
+ virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) = 0;
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) =0;
+ virtual void removeAllGraphicsInstances()=0;
virtual Common2dCanvasInterface* get2dCanvasInterface()=0;
@@ -73,9 +74,10 @@ struct DummyGUIHelper : public GUIHelperInterface
virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){}
- virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices) { return -1; }
-
- virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) { return -1;}
+ virtual int registerTexture(const unsigned char* texels, int width, int height){return -1;}
+ virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId){return -1;}
+ virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) {return -1;}
+ virtual void removeAllGraphicsInstances(){}
virtual Common2dCanvasInterface* get2dCanvasInterface()
{
diff --git a/examples/CommonInterfaces/CommonRenderInterface.h b/examples/CommonInterfaces/CommonRenderInterface.h
index e1d6170ef..0242a2bbe 100644
--- a/examples/CommonInterfaces/CommonRenderInterface.h
+++ b/examples/CommonInterfaces/CommonRenderInterface.h
@@ -53,6 +53,8 @@ struct CommonRenderInterface
virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex)=0;
virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex)=0;
+ virtual int getTotalNumInstances() const = 0;
+
virtual void writeTransforms()=0;
virtual void enableBlend(bool blend)=0;
virtual void clearZBuffer()=0;
diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp
index 23c225677..0af5c8577 100644
--- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp
+++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp
@@ -199,9 +199,16 @@ void OpenGLGuiHelper::createCollisionObjectGraphicsObject(btCollisionObject* bod
}
}
-int OpenGLGuiHelper::registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices)
+int OpenGLGuiHelper::registerTexture(const unsigned char* texels, int width, int height)
{
- int shapeId = m_data->m_glApp->m_renderer->registerShape(vertices, numvertices,indices,numIndices);
+ int textureId = m_data->m_glApp->m_renderer->registerTexture(texels,width,height);
+ return textureId;
+}
+
+
+int OpenGLGuiHelper::registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
+{
+ int shapeId = m_data->m_glApp->m_renderer->registerShape(vertices, numvertices,indices,numIndices,primitiveType, textureId);
return shapeId;
}
@@ -210,6 +217,10 @@ int OpenGLGuiHelper::registerGraphicsInstance(int shapeIndex, const float* posit
return m_data->m_glApp->m_renderer->registerGraphicsInstance(shapeIndex,position,quaternion,color,scaling);
}
+void OpenGLGuiHelper::removeAllGraphicsInstances()
+{
+ m_data->m_glApp->m_renderer->removeAllInstances();
+}
void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
{
@@ -247,7 +258,7 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli
if (gfxVertices.size() && indices.size())
{
- int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0],gfxVertices.size(),&indices[0],indices.size());
+ int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0],gfxVertices.size(),&indices[0],indices.size(),B3_GL_TRIANGLES,-1);
collisionShape->setUserIndex(shapeId);
}
diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h
index 0184d6ea0..72d784a0c 100644
--- a/examples/ExampleBrowser/OpenGLGuiHelper.h
+++ b/examples/ExampleBrowser/OpenGLGuiHelper.h
@@ -20,11 +20,10 @@ struct OpenGLGuiHelper : public GUIHelperInterface
virtual void createCollisionObjectGraphicsObject(btCollisionObject* body, const btVector3& color);
- virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices);
-
+ virtual int registerTexture(const unsigned char* texels, int width, int height);
+ virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId);
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling);
-
-
+ virtual void removeAllGraphicsInstances();
virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape);
diff --git a/examples/ExtendedTutorials/RigidBodyFromObj.cpp b/examples/ExtendedTutorials/RigidBodyFromObj.cpp
index 64d52793d..9e09f9dd9 100644
--- a/examples/ExtendedTutorials/RigidBodyFromObj.cpp
+++ b/examples/ExtendedTutorials/RigidBodyFromObj.cpp
@@ -134,7 +134,8 @@ void RigidBodyFromObjExample::initPhysics()
int shapeId = m_guiHelper->registerGraphicsShape(&glmesh->m_vertices->at(0).xyzw[0],
glmesh->m_numvertices,
&glmesh->m_indices->at(0),
- glmesh->m_numIndices);
+ glmesh->m_numIndices,
+ B3_GL_TRIANGLES, -1);
shape->setUserIndex(shapeId);
int renderInstance = m_guiHelper->registerGraphicsInstance(shapeId,pos,orn,color,scaling);
body->setUserIndex(renderInstance);
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
index 9b6eb8ae1..555acf530 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
@@ -108,7 +108,8 @@ struct BulletErrorLogger : public ErrorLogger
bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
{
-
+ if (strlen(fileName)==0)
+ return false;
//int argc=0;
char relativeFileName[1024];
@@ -132,7 +133,7 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
std::fstream xml_file(relativeFileName, std::fstream::in);
- while ( xml_file.good() )
+ while ( xml_file.good())
{
std::string line;
std::getline( xml_file, line);
@@ -969,16 +970,17 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
// graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
//graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
- CommonRenderInterface* renderer = m_data->m_guiHelper->getRenderInterface();
+ //CommonRenderInterface* renderer = m_data->m_guiHelper->getRenderInterface();
- if (renderer)
+ if (1)
{
int textureIndex = -1;
if (textures.size())
{
- textureIndex = renderer->registerTexture(textures[0].textureData,textures[0].m_width,textures[0].m_height);
+
+ textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData,textures[0].m_width,textures[0].m_height);
}
- graphicsIndex = renderer->registerShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(),B3_GL_TRIANGLES,textureIndex);
+ graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(),B3_GL_TRIANGLES,textureIndex);
}
}
diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
index 92a917fd7..51b09e079 100644
--- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
+++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
@@ -65,7 +65,8 @@ btAlignedObjectArray gFileNameArray;
struct ImportUrdfInternalData
{
ImportUrdfInternalData()
- :m_numMotors(0)
+ :m_numMotors(0),
+ m_mb(0)
{
for (int i=0;im_rb = creation.getRigidBody();
+ m_data->m_mb = creation.getBulletMultiBody();
+ btMultiBody* mb = m_data->m_mb;
+
if (m_useMultiBody && mb )
{
std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
@@ -337,7 +342,7 @@ void ImportUrdfSetup::initPhysics()
- bool createGround=true;
+ bool createGround=false;
if (createGround)
{
btVector3 groundHalfExtents(20,20,20);
@@ -357,8 +362,7 @@ void ImportUrdfSetup::initPhysics()
m_guiHelper->createRigidBodyGraphicsObject(body,color);
}
- ///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates.
- m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
+
}
#ifdef TEST_MULTIBODY_SERIALIZATION
diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
index 28e9023d8..3d34e53f2 100644
--- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
+++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
@@ -13,6 +13,7 @@
MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper)
:m_bulletMultiBody(0),
+ m_rigidBody(0),
m_guiHelper(guiHelper)
{
}
@@ -31,10 +32,10 @@ class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btSc
{
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
rbci.m_startWorldTransform = initialWorldTrans;
- btRigidBody* body = new btRigidBody(rbci);
- body->forceActivationState(DISABLE_DEACTIVATION);
+ m_rigidBody = new btRigidBody(rbci);
+ m_rigidBody->forceActivationState(DISABLE_DEACTIVATION);
- return body;
+ return m_rigidBody;
}
class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody)
diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
index 7334fbd90..453f97311 100644
--- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
+++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
@@ -24,6 +24,7 @@ class MyMultiBodyCreator : public MultiBodyCreationInterface
protected:
btMultiBody* m_bulletMultiBody;
+ btRigidBody* m_rigidBody;
struct GUIHelperInterface* m_guiHelper;
@@ -62,6 +63,10 @@ public:
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex);
btMultiBody* getBulletMultiBody();
+ btRigidBody* getRigidBody()
+ {
+ return m_rigidBody;
+ }
int getNum6DofConstraints() const
{
diff --git a/examples/MultiThreading/b3PosixThreadSupport.cpp b/examples/MultiThreading/b3PosixThreadSupport.cpp
index 74cdfaba2..c19ab1b48 100644
--- a/examples/MultiThreading/b3PosixThreadSupport.cpp
+++ b/examples/MultiThreading/b3PosixThreadSupport.cpp
@@ -53,7 +53,7 @@ static sem_t* createSem(const char* baseName)
#ifdef NAMED_SEMAPHORES
/// Named semaphore begin
char name[32];
- snprintf(name, 32, "/%s-%d-%4.4d", baseName, getpid(), semCount++);
+ snprintf(name, 32, "/%8.s-%4.d-%4.4d", baseName, getpid(), semCount++);
sem_t* tempSem = sem_open(name, O_CREAT, 0600, 0);
if (tempSem != reinterpret_cast(SEM_FAILED))
diff --git a/examples/MultiThreading/b3Win32ThreadSupport.cpp b/examples/MultiThreading/b3Win32ThreadSupport.cpp
index 508b89348..db1d5915a 100644
--- a/examples/MultiThreading/b3Win32ThreadSupport.cpp
+++ b/examples/MultiThreading/b3Win32ThreadSupport.cpp
@@ -244,10 +244,10 @@ void b3Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threa
threadStatus.m_userPtr=0;
- sprintf(threadStatus.m_eventStartHandleName,"eventStart%s%d",threadConstructionInfo.m_uniqueName,i);
+ sprintf(threadStatus.m_eventStartHandleName,"eventStart%8.s%d",threadConstructionInfo.m_uniqueName,i);
threadStatus.m_eventStartHandle = CreateEventA (0,false,false,threadStatus.m_eventStartHandleName);
- sprintf(threadStatus.m_eventCompletetHandleName,"eventComplete%s%d",threadConstructionInfo.m_uniqueName,i);
+ sprintf(threadStatus.m_eventCompletetHandleName,"eventComplete%8.s%d",threadConstructionInfo.m_uniqueName,i);
threadStatus.m_eventCompletetHandle = CreateEventA (0,false,false,threadStatus.m_eventCompletetHandleName);
m_completeHandles[i] = threadStatus.m_eventCompletetHandle;
diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp
index 24002ad25..eff02ec02 100644
--- a/examples/OpenGLWindow/GLInstancingRenderer.cpp
+++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp
@@ -17,8 +17,8 @@ subject to the following restrictions:
///todo: make this configurable in the gui
bool useShadowMap=true;//false;//true;
-int shadowMapWidth=8192;
-int shadowMapHeight=8192;
+int shadowMapWidth=4096;//8192;
+int shadowMapHeight=4096;
float shadowMapWorldSize=50;
#define MAX_POINTS_IN_BATCH 1024
@@ -386,24 +386,39 @@ void GLInstancingRenderer::writeTransforms()
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
- glFlush();
+ //glFlush();
b3Assert(glGetError() ==GL_NO_ERROR);
+ int totalNumInstances= 0;
+ for (int k=0;km_numGraphicsInstances;
+ }
+
+ int POSITION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
+ int ORIENTATION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
+ int COLOR_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
+// int SCALE_BUFFER_SIZE = (totalNumInstances*sizeof(float)*3);
+
+#if 1
+ glBufferSubData( GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes,totalNumInstances*sizeof(float)*4,
+ &m_data->m_instance_positions_ptr[0]);
+ glBufferSubData( GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE,totalNumInstances*sizeof(float)*4,
+ &m_data->m_instance_quaternion_ptr[0]);
+ glBufferSubData( GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes+ POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE, totalNumInstances*sizeof(float)*4,
+ &m_data->m_instance_colors_ptr[0]);
+ glBufferSubData( GL_ARRAY_BUFFER, m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE,totalNumInstances*sizeof(float)*3,
+ &m_data->m_instance_scale_ptr[0]);
+#else
char* orgBase = (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_READ_WRITE);
if (orgBase)
{
- int totalNumInstances= 0;
-
- for (int k=0;km_numGraphicsInstances;
- }
-
+
m_data->m_totalNumInstances = totalNumInstances;
for (int k=0;km_vbo);
b3Assert(glGetError() ==GL_NO_ERROR);
@@ -1006,7 +1021,7 @@ void GLInstancingRenderer::renderScene()
{
renderSceneInternal(B3_CREATE_SHADOWMAP_RENDERMODE);
- // glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
+ //glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
renderSceneInternal(B3_USE_SHADOWMAP_RENDERMODE);
} else
@@ -1075,6 +1090,9 @@ void GLInstancingRenderer::drawPoints(const float* positions, const float color[
void GLInstancingRenderer::drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float lineWidthIn)
{
+ glActiveTexture(GL_TEXTURE0);
+ glBindTexture(GL_TEXTURE_2D,0);
+
float lineWidth = lineWidthIn;
b3Clamp(lineWidth,(float)lineWidthRange[0],(float)lineWidthRange[1]);
glLineWidth(lineWidth);
@@ -1510,7 +1528,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
int curOffset = 0;
//GLuint lastBindTexture = 0;
-
+
for (int i=0;im_numGraphicsInstances=%d\n",gfxObj->m_numGraphicsInstances);
printf("indexCount=%d\n",indexCount);
*/
+
glUseProgram(createShadowMapInstancingShader);
glUniformMatrix4fv(createShadow_depthMVP, 1, false, &depthMVP[0][0]);
glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances);
@@ -1667,6 +1686,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
glDisable (GL_BLEND);
}
glActiveTexture(GL_TEXTURE0);
+ glBindTexture(GL_TEXTURE_2D,0);
break;
}
default:
@@ -1743,4 +1763,10 @@ void GLInstancingRenderer::setRenderFrameBuffer(unsigned int renderFrameBuffer)
m_data->m_renderFrameBuffer = (GLuint) renderFrameBuffer;
}
+int GLInstancingRenderer::getTotalNumInstances() const
+{
+ return m_data->m_totalNumInstances;
+}
+
+
#endif //NO_OPENGL3
diff --git a/examples/OpenGLWindow/GLInstancingRenderer.h b/examples/OpenGLWindow/GLInstancingRenderer.h
index 66664b844..acaaa7655 100644
--- a/examples/OpenGLWindow/GLInstancingRenderer.h
+++ b/examples/OpenGLWindow/GLInstancingRenderer.h
@@ -125,6 +125,8 @@ public:
virtual int getInstanceCapacity() const;
+ virtual int getTotalNumInstances() const;
+
virtual void enableShadowMap();
virtual void enableBlend(bool blend)
{
diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp
index 74c701aad..dbc90d729 100644
--- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp
+++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp
@@ -64,6 +64,12 @@ void SimpleOpenGL2Renderer::writeSingleInstanceColorToCPU(double* color, int src
{
}
+
+int SimpleOpenGL2Renderer::getTotalNumInstances() const
+{
+ return 0;
+}
+
void SimpleOpenGL2Renderer::getCameraViewMatrix(float viewMat[16]) const
{
b3Assert(0);
diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h b/examples/OpenGLWindow/SimpleOpenGL2Renderer.h
index 5fcd08d3e..0e343a4a6 100644
--- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h
+++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.h
@@ -68,6 +68,8 @@ struct SimpleOpenGL2Renderer : public CommonRenderInterface
virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex);
+ virtual int getTotalNumInstances() const;
+
virtual void writeTransforms();
virtual void drawLine(const double from[4], const double to[4], const double color[4], double lineWidth);
@@ -82,6 +84,7 @@ struct SimpleOpenGL2Renderer : public CommonRenderInterface
virtual void clearZBuffer();
+
virtual struct GLInstanceRendererInternalData* getInternalData()
{
return 0;
diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp
index 3ca2aa5c5..fa4362caf 100644
--- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp
+++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp
@@ -659,6 +659,8 @@ bool PhysicsClientSharedMemory::canSubmitCommand() const {
}
struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() {
+ static int sequence = 0;
+ m_data->m_testBlock1->m_clientCommands[0].m_sequenceNumber = sequence++;
return &m_data->m_testBlock1->m_clientCommands[0];
}
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 6ead18363..4896bd6f1 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -10,7 +10,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
-#include "../CommonInterfaces/CommonRenderInterface.h"
+
#include "btBulletDynamicsCommon.h"
@@ -1756,10 +1756,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_STEP_FORWARD_SIMULATION:
{
-
+
if (m_data->m_verboseOutput)
{
b3Printf("Step simulation request");
+ b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber);
}
///todo(erwincoumans) move this damping inside Bullet
for (int i=0;im_bodyHandles.size();i++)
@@ -1864,16 +1865,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_RESET_SIMULATION:
{
//clean up all data
- if (m_data && m_data->m_guiHelper && m_data->m_guiHelper->getRenderInterface())
- {
- m_data->m_guiHelper->getRenderInterface()->removeAllInstances();
- }
+
+
+
+ if (m_data && m_data->m_guiHelper)
+ {
+ m_data->m_guiHelper->removeAllGraphicsInstances();
+ }
if (m_data)
{
m_data->m_visualConverter.resetAll();
}
+
deleteDynamicsWorld();
createEmptyDynamicsWorld();
+
m_data->exitHandles();
m_data->initHandles();
@@ -2058,6 +2064,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_APPLY_EXTERNAL_FORCE:
{
+ if (m_data->m_verboseOutput)
+ {
+ b3Printf("CMD_APPLY_EXTERNAL_FORCE clientCmd = %d\n", clientCmd.m_sequenceNumber);
+ }
for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i)
{
InteralBodyData* body = m_data->getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]);
diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp
index 7cb76e9c6..003064e94 100644
--- a/examples/SharedMemory/PhysicsServerExample.cpp
+++ b/examples/SharedMemory/PhysicsServerExample.cpp
@@ -9,11 +9,394 @@
#include "SharedMemoryCommon.h"
+#include "../Utils/b3Clock.h"
+#include "../MultiThreading/b3ThreadSupportInterface.h"
+
+int blockme = false;
+int blockme2 = false;
+
+void MotionThreadFunc(void* userPtr,void* lsMemory);
+void* MotionlsMemoryFunc();
+#define MAX_MOTION_NUM_THREADS 1
+enum
+ {
+ numCubesX = 20,
+ numCubesY = 20
+ };
+
+
+enum TestExampleBrowserCommunicationEnums
+{
+ eRequestTerminateMotion= 13,
+ eMotionIsUnInitialized,
+ eMotionIsInitialized,
+ eMotionInitializationFailed,
+ eMotionHasTerminated
+};
+
+enum MultiThreadedGUIHelperCommunicationEnums
+{
+ eGUIHelperIdle= 13,
+ eGUIHelperRegisterTexture,
+ eGUIHelperRegisterGraphicsShape,
+ eGUIHelperRegisterGraphicsInstance,
+ eGUIHelperCreateCollisionShapeGraphicsObject,
+ eGUIHelperCreateCollisionObjectGraphicsObject,
+ eGUIHelperRemoveAllGraphicsInstances,
+};
+
+#include
+//#include "BulletMultiThreaded/PlatformDefinitions.h"
+
+#ifndef _WIN32
+#include "../MultiThreading/b3PosixThreadSupport.h"
+
+b3ThreadSupportInterface* createMotionThreadSupport(int numThreads)
+{
+ b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("MotionThreads",
+ MotionThreadFunc,
+ MotionlsMemoryFunc,
+ numThreads);
+ b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);
+
+ return threadSupport;
+
+}
+
+
+#elif defined( _WIN32)
+#include "../MultiThreading/b3Win32ThreadSupport.h"
+
+b3ThreadSupportInterface* createMotionThreadSupport(int numThreads)
+{
+ b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("MotionThreads",MotionThreadFunc,MotionlsMemoryFunc,numThreads);
+ b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo);
+ return threadSupport;
+
+}
+#endif
+
+
+
+struct MotionArgs
+{
+ MotionArgs()
+ :m_physicsServerPtr(0)
+ {
+ }
+ b3CriticalSection* m_cs;
+
+ PhysicsServerSharedMemory* m_physicsServerPtr;
+ b3AlignedObjectArray m_positions;
+};
+
+struct MotionThreadLocalStorage
+{
+ int threadId;
+};
+
+int skip = 0;
+
+
+void MotionThreadFunc(void* userPtr,void* lsMemory)
+{
+ printf("thread started\n");
+
+ MotionThreadLocalStorage* localStorage = (MotionThreadLocalStorage*) lsMemory;
+
+ MotionArgs* args = (MotionArgs*) userPtr;
+ int workLeft = true;
+ b3Clock clock;
+ clock.reset();
+ bool init = true;
+ if (init)
+ {
+
+ args->m_cs->lock();
+ args->m_cs->setSharedParam(0,eMotionIsInitialized);
+ args->m_cs->unlock();
+
+ double m_x=0;
+ double m_y=0;
+ double m_z=0;
+
+ do
+ {
+
+ double deltaTimeInSeconds = double(clock.getTimeMicroseconds())/1000000.;
+ //if (deltaTimeInSeconds<.3)
+ //if (deltaTimeInSeconds<(1./15.))
+ if (deltaTimeInSeconds<(1./260.))
+ {
+ skip++;
+ //if (deltaTimeInSeconds<.001)
+ continue;
+ }
+
+ m_x+=deltaTimeInSeconds;
+ m_y+=deltaTimeInSeconds;
+ m_z+=deltaTimeInSeconds;
+
+ clock.reset();
+
+
+ int index = 0;
+ index++;
+
+ if (!blockme)
+ {
+ args->m_physicsServerPtr->processClientCommands();
+ }
+
+
+
+
+ } while (args->m_cs->getSharedParam(0)!=eRequestTerminateMotion);
+ } else
+ {
+ args->m_cs->lock();
+ args->m_cs->setSharedParam(0,eMotionInitializationFailed);
+ args->m_cs->unlock();
+ }
+
+
+ args->m_cs->lock();
+ args->m_cs->setSharedParam(0,eMotionHasTerminated);
+ args->m_cs->unlock();
+ printf("finished, #skip = %d\n",skip);
+ skip=0;
+ //do nothing
+
+}
+
+
+
+void* MotionlsMemoryFunc()
+{
+ //don't create local store memory, just return 0
+ return new MotionThreadLocalStorage;
+}
+
+
+
+class MultiThreadedOpenGLGuiHelper : public GUIHelperInterface
+{
+ CommonGraphicsApp* m_app;
+
+ b3CriticalSection* m_cs;
+
+
+public:
+
+ GUIHelperInterface* m_childGuiHelper;
+
+ const unsigned char* m_texels;
+ int m_textureWidth;
+ int m_textureHeight;
+
+
+ int m_shapeIndex;
+ const float* m_position;
+ const float* m_quaternion;
+ const float* m_color;
+ const float* m_scaling;
+
+ const float* m_vertices;
+ int m_numvertices;
+ const int* m_indices;
+ int m_numIndices;
+ int m_primitiveType;
+ int m_textureId;
+ int m_instanceId;
+
+
+ MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper)
+ :m_app(app)
+ ,m_cs(0),
+ m_texels(0),
+ m_textureId(-1)
+ {
+ m_childGuiHelper = guiHelper;;
+
+ }
+
+ virtual ~MultiThreadedOpenGLGuiHelper()
+ {
+ delete m_childGuiHelper;
+ }
+
+ void setCriticalSection(b3CriticalSection* cs)
+ {
+ m_cs = cs;
+ }
+
+ b3CriticalSection* getCriticalSection()
+ {
+ return m_cs;
+ }
+
+ virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color){}
+
+ btCollisionObject* m_obj;
+ btVector3 m_color2;
+
+ virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color)
+ {
+ m_obj = obj;
+ m_color2 = color;
+ m_cs->lock();
+ m_cs->setSharedParam(1,eGUIHelperCreateCollisionObjectGraphicsObject);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+ {
+ }
+
+ }
+
+ btCollisionShape* m_colShape;
+ virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
+ {
+ m_colShape = collisionShape;
+ m_cs->lock();
+ m_cs->setSharedParam(1,eGUIHelperCreateCollisionShapeGraphicsObject);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+ {
+ }
+
+ }
+
+ virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld)
+ {
+ //this check is to prevent a crash, in case we removed all graphics instances, but there are still physics objects.
+ //the check will be obsolete, once we have a better/safer way of synchronizing physics->graphics transforms
+ if ( m_childGuiHelper->getRenderInterface()->getTotalNumInstances()>0)
+ {
+ m_childGuiHelper->syncPhysicsToGraphics(rbWorld);
+ }
+ }
+
+ virtual void render(const btDiscreteDynamicsWorld* rbWorld)
+ {
+ m_childGuiHelper->render(0);
+ }
+
+ virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){}
+
+ virtual int registerTexture(const unsigned char* texels, int width, int height)
+ {
+ m_texels = texels;
+ m_textureWidth = width;
+ m_textureHeight = height;
+
+ m_cs->lock();
+ m_cs->setSharedParam(1,eGUIHelperRegisterTexture);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+ {
+ }
+ return m_textureId;
+ }
+ virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
+ {
+ m_vertices = vertices;
+ m_numvertices = numvertices;
+ m_indices = indices;
+ m_numIndices = numIndices;
+ m_primitiveType = primitiveType;
+ m_textureId = textureId;
+
+ m_cs->lock();
+ m_cs->setSharedParam(1,eGUIHelperRegisterGraphicsShape);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+ {
+ }
+ return m_shapeIndex;
+ }
+ virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
+ {
+ m_shapeIndex = shapeIndex;
+ m_position = position;
+ m_quaternion = quaternion;
+ m_color = color;
+ m_scaling = scaling;
+
+ m_cs->lock();
+ m_cs->setSharedParam(1,eGUIHelperRegisterGraphicsInstance);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+ {
+ }
+ return m_instanceId;
+ }
+
+ virtual void removeAllGraphicsInstances()
+ {
+ m_cs->lock();
+ m_cs->setSharedParam(1,eGUIHelperRemoveAllGraphicsInstances);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+ {
+ }
+ }
+
+ virtual Common2dCanvasInterface* get2dCanvasInterface()
+ {
+ return 0;
+ }
+
+ virtual CommonParameterInterface* getParameterInterface()
+ {
+ return 0;
+ }
+
+ virtual CommonRenderInterface* getRenderInterface()
+ {
+ return 0;
+ }
+
+ virtual CommonGraphicsApp* getAppInterface()
+ {
+ return 0;
+ }
+
+
+ virtual void setUpAxis(int axis)
+ {
+ m_childGuiHelper->setUpAxis(axis);
+ }
+ virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)
+ {
+ }
+
+ virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* width, int* height, int* numPixelsCopied)
+ {
+ if (width)
+ *width = 0;
+ if (height)
+ *height = 0;
+ if (numPixelsCopied)
+ *numPixelsCopied = 0;
+ }
+
+ virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld)
+ {
+ }
+
+ virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
+ {
+ }
+};
+
+
class PhysicsServerExample : public SharedMemoryCommon
{
PhysicsServerSharedMemory m_physicsServer;
-
+ b3ThreadSupportInterface* m_threadSupport;
+ MotionArgs m_args[MAX_MOTION_NUM_THREADS];
+ MultiThreadedOpenGLGuiHelper* m_multiThreadedHelper;
bool m_wantsShutdown;
bool m_isConnected;
@@ -23,7 +406,7 @@ class PhysicsServerExample : public SharedMemoryCommon
public:
- PhysicsServerExample(GUIHelperInterface* helper, SharedMemoryInterface* sharedMem=0, int options=0);
+ PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem=0, int options=0);
virtual ~PhysicsServerExample();
@@ -56,7 +439,7 @@ public:
virtual bool wantsTermination();
virtual bool isConnected();
virtual void renderScene();
- virtual void exitPhysics(){}
+ virtual void exitPhysics();
virtual void physicsDebugDraw(int debugFlags);
@@ -71,7 +454,6 @@ public:
if (!renderer)
{
- btAssert(0);
return false;
}
@@ -91,7 +473,6 @@ public:
if (!renderer)
{
- btAssert(0);
return false;
}
@@ -134,7 +515,7 @@ public:
};
-PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper, SharedMemoryInterface* sharedMem, int options)
+PhysicsServerExample::PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem, int options)
:SharedMemoryCommon(helper),
m_physicsServer(sharedMem),
m_wantsShutdown(false),
@@ -142,6 +523,7 @@ m_isConnected(false),
m_replay(false),
m_options(options)
{
+ m_multiThreadedHelper = helper;
b3Printf("Started PhysicsServer\n");
}
@@ -165,22 +547,80 @@ void PhysicsServerExample::initPhysics()
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
-#if 0
- createEmptyDynamicsWorld();
+
- //todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire
- btVector3 grav(0,0,0);
- grav[upAxis] = 0;//-9.8;
- this->m_dynamicsWorld->setGravity(grav);
-#endif
+ m_threadSupport = createMotionThreadSupport(MAX_MOTION_NUM_THREADS);
+
+
- m_isConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
+ for (int i=0;igetNumTasks();i++)
+ {
+ MotionThreadLocalStorage* storage = (MotionThreadLocalStorage*) m_threadSupport->getThreadLocalMemory(i);
+ b3Assert(storage);
+ storage->threadId = i;
+ //storage->m_sharedMem = data->m_sharedMem;
+ }
+
+
+
+ for (int w=0;wcreateCriticalSection();
+ m_args[w].m_cs->setSharedParam(0,eMotionIsUnInitialized);
+ int numMoving = 0;
+ m_args[w].m_positions.resize(numMoving);
+ m_args[w].m_physicsServerPtr = &m_physicsServer;
+ int index = 0;
+
+ m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &this->m_args[w], w);
+ while (m_args[w].m_cs->getSharedParam(0)==eMotionIsUnInitialized)
+ {
+ }
+ }
+
+ m_args[0].m_cs->setSharedParam(1,eGUIHelperIdle);
+ m_multiThreadedHelper->setCriticalSection(m_args[0].m_cs);
+ m_isConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
}
+void PhysicsServerExample::exitPhysics()
+{
+ for (int i=0;ilock();
+ m_args[i].m_cs->setSharedParam(0,eRequestTerminateMotion);
+ m_args[i].m_cs->unlock();
+ }
+ int numActiveThreads = MAX_MOTION_NUM_THREADS;
+
+ while (numActiveThreads)
+ {
+ int arg0,arg1;
+ if (m_threadSupport->isTaskCompleted(&arg0,&arg1,0))
+ {
+ numActiveThreads--;
+ printf("numActiveThreads = %d\n",numActiveThreads);
+
+ } else
+ {
+ }
+ };
+
+ printf("stopping threads\n");
+
+ delete m_threadSupport;
+ m_threadSupport = 0;
+
+ m_physicsServer.resetDynamicsWorld();
+
+}
+
+
+
bool PhysicsServerExample::wantsTermination()
{
return m_wantsShutdown;
@@ -190,6 +630,90 @@ bool PhysicsServerExample::wantsTermination()
void PhysicsServerExample::stepSimulation(float deltaTime)
{
+ //this->m_physicsServer.processClientCommands();
+
+ //check if any graphics related tasks are requested
+
+ switch (m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
+ {
+ case eGUIHelperCreateCollisionShapeGraphicsObject:
+ {
+ m_multiThreadedHelper->m_childGuiHelper->createCollisionShapeGraphicsObject(m_multiThreadedHelper->m_colShape);
+ m_multiThreadedHelper->getCriticalSection()->lock();
+ m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+ m_multiThreadedHelper->getCriticalSection()->unlock();
+
+ break;
+ }
+ case eGUIHelperCreateCollisionObjectGraphicsObject:
+ {
+ m_multiThreadedHelper->m_childGuiHelper->createCollisionObjectGraphicsObject(m_multiThreadedHelper->m_obj,
+ m_multiThreadedHelper->m_color2);
+ m_multiThreadedHelper->getCriticalSection()->lock();
+ m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+ m_multiThreadedHelper->getCriticalSection()->unlock();
+ break;
+ }
+ case eGUIHelperRegisterTexture:
+ {
+
+ m_multiThreadedHelper->m_textureId = m_multiThreadedHelper->m_childGuiHelper->registerTexture(m_multiThreadedHelper->m_texels,
+ m_multiThreadedHelper->m_textureWidth,m_multiThreadedHelper->m_textureHeight);
+
+ m_multiThreadedHelper->getCriticalSection()->lock();
+ m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+ m_multiThreadedHelper->getCriticalSection()->unlock();
+
+ break;
+ }
+ case eGUIHelperRegisterGraphicsShape:
+ {
+ m_multiThreadedHelper->m_shapeIndex = m_multiThreadedHelper->m_childGuiHelper->registerGraphicsShape(
+ m_multiThreadedHelper->m_vertices,
+ m_multiThreadedHelper->m_numvertices,
+ m_multiThreadedHelper->m_indices,
+ m_multiThreadedHelper->m_numIndices,
+ m_multiThreadedHelper->m_primitiveType,
+ m_multiThreadedHelper->m_textureId);
+
+ m_multiThreadedHelper->getCriticalSection()->lock();
+ m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+ m_multiThreadedHelper->getCriticalSection()->unlock();
+ break;
+ }
+ case eGUIHelperRegisterGraphicsInstance:
+ {
+ m_multiThreadedHelper->m_instanceId = m_multiThreadedHelper->m_childGuiHelper->registerGraphicsInstance(
+ m_multiThreadedHelper->m_shapeIndex,
+ m_multiThreadedHelper->m_position,
+ m_multiThreadedHelper->m_quaternion,
+ m_multiThreadedHelper->m_color,
+ m_multiThreadedHelper->m_scaling);
+
+ m_multiThreadedHelper->getCriticalSection()->lock();
+ m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+ m_multiThreadedHelper->getCriticalSection()->unlock();
+ break;
+ }
+ case eGUIHelperRemoveAllGraphicsInstances:
+ {
+ m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances();
+ m_multiThreadedHelper->getCriticalSection()->lock();
+ m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+ m_multiThreadedHelper->getCriticalSection()->unlock();
+ break;
+ }
+ case eGUIHelperIdle:
+ default:
+ {
+
+ }
+ }
+
+
+
+
+ #if 0
if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK)
{
btClock rtc;
@@ -201,15 +725,27 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
}
} else
{
- for (int i=0;i<10;i++)
+ //for (int i=0;i<10;i++)
m_physicsServer.processClientCommands();
}
+ #endif
+
+ if (!blockme2)
+ {
+ if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
+ {
+ m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms();
+ }
+ }
}
void PhysicsServerExample::renderScene()
{
///debug rendering
+ //m_args[0].m_cs->lock();
+
m_physicsServer.renderScene();
+ //m_args[0].m_cs->unlock();
}
void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags)
@@ -290,7 +826,13 @@ extern int gSharedMemoryKey;
class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options)
{
- PhysicsServerExample* example = new PhysicsServerExample(options.m_guiHelper, options.m_sharedMem, options.m_option);
+
+ MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(options.m_guiHelper->getAppInterface(),options.m_guiHelper);
+
+ PhysicsServerExample* example = new PhysicsServerExample(guiHelperWrapper,
+ options.m_sharedMem,
+ options.m_option);
+
if (gSharedMemoryKey>=0)
{
example->setSharedMemoryKey(gSharedMemoryKey);
diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.cpp b/examples/SharedMemory/PhysicsServerSharedMemory.cpp
index af8101464..0d29fae1e 100644
--- a/examples/SharedMemory/PhysicsServerSharedMemory.cpp
+++ b/examples/SharedMemory/PhysicsServerSharedMemory.cpp
@@ -90,6 +90,11 @@ PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
delete m_data;
}
+void PhysicsServerSharedMemory::resetDynamicsWorld()
+{
+ m_data->m_commandProcessor->deleteDynamicsWorld();
+ m_data->m_commandProcessor ->createEmptyDynamicsWorld();
+}
void PhysicsServerSharedMemory::setSharedMemoryKey(int key)
{
m_data->m_sharedMemoryKey = key;
diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.h b/examples/SharedMemory/PhysicsServerSharedMemory.h
index 3db5ff497..2c08124d0 100644
--- a/examples/SharedMemory/PhysicsServerSharedMemory.h
+++ b/examples/SharedMemory/PhysicsServerSharedMemory.h
@@ -43,6 +43,7 @@ public:
void enableCommandLogging(bool enable, const char* fileName);
void replayFromLogFile(const char* fileName);
+ void resetDynamicsWorld();
};
diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua
index be3c7102c..c2ceb1a05 100644
--- a/examples/SharedMemory/premake4.lua
+++ b/examples/SharedMemory/premake4.lua
@@ -76,12 +76,12 @@ myfiles =
"../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
"../Importers/ImportColladaDemo/ColladaGraphicsInstance.h",
"../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
- "../ThirdPartyLibs/tinyxml/tinystr.cpp",
- "../ThirdPartyLibs/tinyxml/tinyxml.cpp",
- "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
- "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
-"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
- "../ThirdPartyLibs/stb_image/stb_image.cpp",
+ "../ThirdPartyLibs/tinyxml/tinystr.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxml.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+ "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+ "../ThirdPartyLibs/stb_image/stb_image.cpp",
}
files {
@@ -90,55 +90,197 @@ files {
}
+files {
+ "../MultiThreading/b3ThreadSupportInterface.cpp",
+ "../MultiThreading/b3ThreadSupportInterface.h"
+ }
+ if os.is("Windows") then
-project "App_SharedMemoryPhysics_VR"
+ files {
+ "../MultiThreading/b3Win32ThreadSupport.cpp",
+ "../MultiThreading/b3Win32ThreadSupport.h"
+ }
+ --links {"winmm"}
+ --defines {"__WINDOWS_MM__", "WIN32"}
+ end
-defines {"B3_USE_STANDALONE_EXAMPLE","BT_ENABLE_VR"}
+ if os.is("Linux") then
+ files {
+ "../MultiThreading/b3PosixThreadSupport.cpp",
+ "../MultiThreading/b3PosixThreadSupport.h"
+ }
+
+ links {"pthread"}
+ end
+
+ if os.is("MacOSX") then
+ files {
+ "../MultiThreading/b3PosixThreadSupport.cpp",
+ "../MultiThreading/b3PosixThreadSupport.h"
+ }
+
+ links {"pthread"}
+ --links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
+ --defines {"__MACOSX_CORE__"}
+ end
+
+
+project "App_SharedMemoryPhysics_GUI"
if _OPTIONS["ios"] then
- kind "WindowedApp"
-else
- kind "ConsoleApp"
+ kind "WindowedApp"
+else
+ kind "ConsoleApp"
end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+
+includedirs {"../../src"}
-includedirs {
- ".","../../src", "../ThirdPartyLibs",
- "../ThirdPartyLibs/openvr/headers",
- "../ThirdPartyLibs/openvr/samples/shared"
- }
-
links {
- "Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api"
+ "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
}
-
-language "C++"
-
-
initOpenGL()
initGlew()
+language "C++"
-files
-{
- myfiles,
- "../StandaloneMain/hellovr_opengl_main.cpp",
+files {
+ myfiles,
+ "../StandaloneMain/main_opengl_single_example.cpp",
"../ExampleBrowser/OpenGLGuiHelper.cpp",
"../ExampleBrowser/GL_ShapeDrawer.cpp",
"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
- "../ThirdPartyLibs/openvr/samples/shared/lodepng.cpp",
- "../ThirdPartyLibs/openvr/samples/shared/lodepng.h",
- "../ThirdPartyLibs/openvr/samples/shared/Matrices.cpp",
- "../ThirdPartyLibs/openvr/samples/shared/Matrices.h",
- "../ThirdPartyLibs/openvr/samples/shared/pathtools.cpp",
- "../ThirdPartyLibs/openvr/samples/shared/pathtools.h",
- "../ThirdPartyLibs/openvr/samples/shared/Vectors.h",
}
-if os.is("Windows") then
- libdirs {"../ThirdPartyLibs/openvr/lib/win32"}
-end
if os.is("Linux") then initX11() end
if os.is("MacOSX") then
links{"Cocoa.framework"}
+end
+
+
+files {
+ "../MultiThreading/b3ThreadSupportInterface.cpp",
+ "../MultiThreading/b3ThreadSupportInterface.h"
+ }
+if os.is("Windows") then
+
+ files {
+ "../MultiThreading/b3Win32ThreadSupport.cpp",
+ "../MultiThreading/b3Win32ThreadSupport.h"
+ }
+ --links {"winmm"}
+ --defines {"__WINDOWS_MM__", "WIN32"}
+end
+
+if os.is("Linux") then
+ files {
+ "../MultiThreading/b3PosixThreadSupport.cpp",
+ "../MultiThreading/b3PosixThreadSupport.h"
+ }
+
+ links {"pthread"}
+end
+
+if os.is("MacOSX") then
+ files {
+ "../MultiThreading/b3PosixThreadSupport.cpp",
+ "../MultiThreading/b3PosixThreadSupport.h"
+ }
+
+ links {"pthread"}
+ --links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
+ --defines {"__MACOSX_CORE__"}
+end
+
+if os.is("Windows") then
+ project "App_SharedMemoryPhysics_VR"
+ --for now, only enable VR under Windows, until compilation issues are resolved on Mac/Linux
+ defines {"B3_USE_STANDALONE_EXAMPLE","BT_ENABLE_VR"}
+
+ if _OPTIONS["ios"] then
+ kind "WindowedApp"
+ else
+ kind "ConsoleApp"
+ end
+
+ includedirs {
+ ".","../../src", "../ThirdPartyLibs",
+ "../ThirdPartyLibs/openvr/headers",
+ "../ThirdPartyLibs/openvr/samples/shared"
+ }
+
+ links {
+ "Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api"
+ }
+
+
+ language "C++"
+
+
+ initOpenGL()
+ initGlew()
+
+
+ files
+ {
+ myfiles,
+ "../StandaloneMain/hellovr_opengl_main.cpp",
+ "../ExampleBrowser/OpenGLGuiHelper.cpp",
+ "../ExampleBrowser/GL_ShapeDrawer.cpp",
+ "../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+ "../ThirdPartyLibs/openvr/samples/shared/lodepng.cpp",
+ "../ThirdPartyLibs/openvr/samples/shared/lodepng.h",
+ "../ThirdPartyLibs/openvr/samples/shared/Matrices.cpp",
+ "../ThirdPartyLibs/openvr/samples/shared/Matrices.h",
+ "../ThirdPartyLibs/openvr/samples/shared/pathtools.cpp",
+ "../ThirdPartyLibs/openvr/samples/shared/pathtools.h",
+ "../ThirdPartyLibs/openvr/samples/shared/Vectors.h",
+ }
+ if os.is("Windows") then
+ libdirs {"../ThirdPartyLibs/openvr/lib/win32"}
+ end
+
+ if os.is("Linux") then initX11() end
+
+ if os.is("MacOSX") then
+ links{"Cocoa.framework"}
+ end
+
+
+ files {
+ "../MultiThreading/b3ThreadSupportInterface.cpp",
+ "../MultiThreading/b3ThreadSupportInterface.h"
+ }
+ if os.is("Windows") then
+
+ files {
+ "../MultiThreading/b3Win32ThreadSupport.cpp",
+ "../MultiThreading/b3Win32ThreadSupport.h"
+ }
+ --links {"winmm"}
+ --defines {"__WINDOWS_MM__", "WIN32"}
+ end
+
+ if os.is("Linux") then
+ files {
+ "../MultiThreading/b3PosixThreadSupport.cpp",
+ "../MultiThreading/b3PosixThreadSupport.h"
+ }
+
+ links {"pthread"}
+ end
+
+ if os.is("MacOSX") then
+ files {
+ "../MultiThreading/b3PosixThreadSupport.cpp",
+ "../MultiThreading/b3PosixThreadSupport.h"
+ }
+
+ links {"pthread"}
+ --links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
+ --defines {"__MACOSX_CORE__"}
+ end
+
+
end
\ No newline at end of file
diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp
index 547f813fd..6fe0aca97 100644
--- a/examples/StandaloneMain/hellovr_opengl_main.cpp
+++ b/examples/StandaloneMain/hellovr_opengl_main.cpp
@@ -28,7 +28,7 @@ int gSharedMemoryKey = -1;
#include "pathtools.h"
CommonExampleInterface* sExample;
-OpenGLGuiHelper* sGuiPtr = 0;
+GUIHelperInterface* sGuiPtr = 0;
#if defined(POSIX)
@@ -375,7 +375,9 @@ bool CMainApplication::BInit()
*/
m_app = new SimpleOpenGL3App("SimpleOpenGL3App",m_nWindowWidth,m_nWindowHeight,true);
+
sGuiPtr = new OpenGLGuiHelper(m_app,false);
+ //sGuiPtr = new DummyGUIHelper;
CommonExampleOptions options(sGuiPtr);
@@ -1461,6 +1463,8 @@ void CMainApplication::SetupDistortion()
//-----------------------------------------------------------------------------
void CMainApplication::RenderStereoTargets()
{
+ sExample->stepSimulation(1./60.);
+
glClearColor( 0.15f, 0.15f, 0.18f, 1.0f ); // nice background color, but not black
glEnable( GL_MULTISAMPLE );
@@ -1476,33 +1480,40 @@ void CMainApplication::RenderStereoTargets()
rotYtoZ.rotateX(-90);
}
+ RenderScene( vr::Eye_Left );
+
// Left Eye
{
Matrix4 viewMatLeft = m_mat4eyePosLeft * m_mat4HMDPose * rotYtoZ;
m_app->m_instancingRenderer->getActiveCamera()->setVRCamera(viewMatLeft.get(),m_mat4ProjectionLeft.get());
- m_app->m_instancingRenderer->updateCamera();
+ m_app->m_instancingRenderer->updateCamera(m_app->getUpAxis());
}
glBindFramebuffer( GL_FRAMEBUFFER, leftEyeDesc.m_nRenderFramebufferId );
glViewport(0, 0, m_nRenderWidth, m_nRenderHeight );
+
m_app->m_window->startRendering();
- RenderScene( vr::Eye_Left );
- DrawGridData gridUp;
- gridUp.upAxis = m_app->getUpAxis();
- m_app->drawGrid(gridUp);
- sExample->stepSimulation(1./60.);
- sExample->renderScene();
+
+
+
+
+
m_app->m_instancingRenderer->setRenderFrameBuffer((unsigned int)leftEyeDesc.m_nRenderFramebufferId);
- m_app->m_instancingRenderer->renderScene();
+ sExample->renderScene();
+ //m_app->m_instancingRenderer->renderScene();
+ DrawGridData gridUp;
+ gridUp.upAxis = m_app->getUpAxis();
+ m_app->drawGrid(gridUp);
+
glBindFramebuffer( GL_FRAMEBUFFER, 0 );
@@ -1522,22 +1533,25 @@ void CMainApplication::RenderStereoTargets()
// Right Eye
+ RenderScene( vr::Eye_Right );
+
{
Matrix4 viewMatRight = m_mat4eyePosRight * m_mat4HMDPose * rotYtoZ;
m_app->m_instancingRenderer->getActiveCamera()->setVRCamera(viewMatRight.get(),m_mat4ProjectionRight.get());
- m_app->m_instancingRenderer->updateCamera();
+ m_app->m_instancingRenderer->updateCamera(m_app->getUpAxis());
}
-
+
glBindFramebuffer( GL_FRAMEBUFFER, rightEyeDesc.m_nRenderFramebufferId );
glViewport(0, 0, m_nRenderWidth, m_nRenderHeight );
m_app->m_window->startRendering();
- RenderScene( vr::Eye_Right );
- m_app->drawGrid(gridUp);
+
+
m_app->m_instancingRenderer->setRenderFrameBuffer((unsigned int)rightEyeDesc.m_nRenderFramebufferId);
-
- m_app->m_renderer->renderScene();
+ //m_app->m_renderer->renderScene();
+ sExample->renderScene();
+ m_app->drawGrid(gridUp);
glBindFramebuffer( GL_FRAMEBUFFER, 0 );
diff --git a/examples/StandaloneMain/main_opengl_single_example.cpp b/examples/StandaloneMain/main_opengl_single_example.cpp
index a7410433c..d331e7607 100644
--- a/examples/StandaloneMain/main_opengl_single_example.cpp
+++ b/examples/StandaloneMain/main_opengl_single_example.cpp
@@ -32,6 +32,7 @@ subject to the following restrictions:
#include "../ExampleBrowser/OpenGLGuiHelper.h"
CommonExampleInterface* example;
+int gSharedMemoryKey=-1;
b3MouseMoveCallback prevMouseMoveCallback = 0;
static void OnMouseMove( float x, float y)
@@ -57,6 +58,20 @@ static void OnMouseDown(int button, int state, float x, float y) {
}
}
+class LessDummyGuiHelper : public DummyGUIHelper
+{
+ CommonGraphicsApp* m_app;
+public:
+ virtual CommonGraphicsApp* getAppInterface()
+ {
+ return m_app;
+ }
+
+ LessDummyGuiHelper(CommonGraphicsApp* app)
+ :m_app(app)
+ {
+ }
+};
int main(int argc, char* argv[])
{
@@ -69,11 +84,13 @@ int main(int argc, char* argv[])
app->m_window->setMouseMoveCallback((b3MouseMoveCallback)OnMouseMove);
OpenGLGuiHelper gui(app,false);
-
+ //LessDummyGuiHelper gui(app);
+ //DummyGUIHelper gui;
+
CommonExampleOptions options(&gui);
example = StandaloneExampleCreateFunc(options);
- example->initPhysics();
+ example->initPhysics();
example->resetCamera();
do
diff --git a/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp b/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp
index 1fa1b0ccd..2ab5d3992 100644
--- a/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp
+++ b/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp
@@ -105,14 +105,20 @@ public:
}
- virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices)
+
+ virtual int registerTexture(const unsigned char* texels, int width, int height)
+ {
+ return -1;
+ }
+
+ virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
{
- int shapeIndex = OpenGLGuiHelper::registerGraphicsShape(vertices,numvertices,indices,numIndices);
+ int shapeIndex = OpenGLGuiHelper::registerGraphicsShape(vertices,numvertices,indices,numIndices,primitiveType, textureId);
if (shapeIndex>=0)
{
TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer);
- float rgbaColor[4] = {1,1,1,1};
- swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
+ float rgbaColor[4] = {1,1,1,1};
+ swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
//swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices);
m_swRenderObjects.insert(shapeIndex,swObj);
}
diff --git a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp
index 565b771e7..d4e0fd77b 100644
--- a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp
+++ b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp
@@ -37,6 +37,12 @@ static btVector4 sMyColors[4] =
//btVector4(1,1,0,1),
};
+struct TinyRendererTexture
+{
+ const unsigned char* m_texels;
+ int m_width;
+ int m_height;
+};
struct TinyRendererGUIHelper : public GUIHelperInterface
{
@@ -45,6 +51,7 @@ struct TinyRendererGUIHelper : public GUIHelperInterface
btHashMap m_swRenderObjects;
btHashMap m_swInstances;
+ btHashMap m_textures;
int m_swWidth;
int m_swHeight;
@@ -151,7 +158,8 @@ struct TinyRendererGUIHelper : public GUIHelperInterface
if (gfxVertices.size() && indices.size())
{
- int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0],gfxVertices.size(),&indices[0],indices.size());
+ int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0],gfxVertices.size(),&indices[0],indices.size(),
+ 1,-1);
collisionShape->setUserIndex(shapeId);
}
}
@@ -249,18 +257,41 @@ struct TinyRendererGUIHelper : public GUIHelperInterface
virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){}
- virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices)
+ virtual int registerTexture(const unsigned char* texels, int width, int height)
+ {
+ //do we need to make a copy?
+ int textureId = m_textures.size();
+ TinyRendererTexture t;
+ t.m_texels = texels;
+ t.m_width = width;
+ t.m_height = height;
+ this->m_textures.insert(textureId,t);
+ return textureId;
+ }
+
+ virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
{
int shapeIndex = m_swRenderObjects.size();
TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer);
float rgbaColor[4] = {1,1,1,1};
- swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
+
+ //if (textureId>=0)
+ //{
+ // swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
+ //} else
+ {
+ swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
+ }
//swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices);
m_swRenderObjects.insert(shapeIndex,swObj);
return shapeIndex;
}
+ virtual void removeAllGraphicsInstances()
+ {
+ }
+
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
{
int colIndex = m_colObjUniqueIndex++;
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 9f2919ded..771d1b16c 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -172,6 +172,8 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
&startOrnX,&startOrnY,&startOrnZ, &startOrnW))
return NULL;
}
+
+ if (strlen(urdfFileName))
{
// printf("(%f, %f, %f) (%f, %f, %f, %f)\n", startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW);
@@ -185,12 +187,16 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType!=CMD_URDF_LOADING_COMPLETED)
- {
- PyErr_SetString(SpamError, "Cannot load URDF file.");
- return NULL;
- }
+ {
+ PyErr_SetString(SpamError, "Cannot load URDF file.");
+ return NULL;
+ }
bodyIndex = b3GetStatusBodyIndex(statusHandle);
- }
+ } else
+ {
+ PyErr_SetString(SpamError, "Empty filename, method expects 1, 4 or 8 arguments.");
+ return NULL;
+ }
return PyLong_FromLong(bodyIndex);
}
diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c
index 6bc517c8e..fab7129cc 100644
--- a/test/SharedMemory/test.c
+++ b/test/SharedMemory/test.c
@@ -106,7 +106,7 @@ void testSharedMemory(b3PhysicsClientHandle sm)
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
//setting the initial position, orientation and other arguments are optional
- startPosX =0;
+ startPosX =2;
startPosY =0;
startPosZ = 1;
ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);