mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 13:50:04 +00:00
pybullet, more robust multi-server connections
Windows shared memory: allow to use custom key. Improve GUI performance on Windows, submit letters in text as a batch (fewer draw-calls) quadruped.py: first try to connect to SHARED_MEMORY, if it fails (<0) use GUI increase Chrome about://tracing json export capacity (press 'p' in Example Browser) UDP physics server: add --port and --sharedMemoryKey command-line arguments PhysicsServerExample: add --sharedMemoryKey command-line option (for VR example too) ExampleBrowser: sleep a few milliseconds if rendering is too fast, use --minUpdateTimeMicroSecs=0 to disable
This commit is contained in:
parent
da2cc483b4
commit
82995a8343
examples
ExampleBrowser
Importers
OpenGLWindow
SharedMemory
PhysicsClientSharedMemory.cppPhysicsServerCommandProcessor.cppPhysicsServerExample.cppWin32SharedMemory.cppmain.cpp
udp
StandaloneMain
pybullet
@ -224,6 +224,8 @@ enum TestExampleBrowserCommunicationEnums
|
||||
eExampleBrowserHasTerminated
|
||||
};
|
||||
|
||||
static double gMinUpdateTimeMicroSecs = 4000.;
|
||||
|
||||
void ExampleBrowserThreadFunc(void* userPtr,void* lsMemory)
|
||||
{
|
||||
printf("ExampleBrowserThreadFunc started\n");
|
||||
@ -254,8 +256,20 @@ void ExampleBrowserThreadFunc(void* userPtr,void* lsMemory)
|
||||
do
|
||||
{
|
||||
float deltaTimeInSeconds = clock.getTimeMicroseconds()/1000000.f;
|
||||
clock.reset();
|
||||
exampleBrowser->update(deltaTimeInSeconds);
|
||||
{
|
||||
if (deltaTimeInSeconds > 0.1)
|
||||
{
|
||||
deltaTimeInSeconds = 0.1;
|
||||
}
|
||||
if (deltaTimeInSeconds < (gMinUpdateTimeMicroSecs/1e6))
|
||||
{
|
||||
clock.usleep(gMinUpdateTimeMicroSecs/10.);
|
||||
} else
|
||||
{
|
||||
clock.reset();
|
||||
exampleBrowser->update(deltaTimeInSeconds);
|
||||
}
|
||||
}
|
||||
|
||||
} while (!exampleBrowser->requestedExit() && (args->m_cs->getSharedParam(0)!=eRequestTerminateExampleBrowser));
|
||||
} else
|
||||
|
@ -163,7 +163,7 @@ FILE* gTimingFile = 0;
|
||||
#define __STDC_FORMAT_MACROS
|
||||
#endif //__STDC_FORMAT_MACROS
|
||||
#include <inttypes.h>
|
||||
#define BT_TIMING_CAPACITY 65536
|
||||
#define BT_TIMING_CAPACITY 16*65536
|
||||
static bool m_firstTiming = true;
|
||||
|
||||
|
||||
@ -266,7 +266,10 @@ struct btTimings
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
if (m_timings[0].size()==0)
|
||||
{
|
||||
m_timings[0].resize(BT_TIMING_CAPACITY);
|
||||
}
|
||||
|
||||
int slot = m_numTimings++;
|
||||
|
||||
@ -279,7 +282,7 @@ struct btTimings
|
||||
|
||||
int m_numTimings;
|
||||
int m_activeBuffer;
|
||||
btTiming m_timings[2][BT_TIMING_CAPACITY];
|
||||
btAlignedObjectArray<btTiming> m_timings[1];
|
||||
};
|
||||
|
||||
btTimings gTimings[BT_MAX_THREAD_COUNT];
|
||||
@ -729,11 +732,13 @@ void MyComboBoxCallback(int comboId, const char* item)
|
||||
|
||||
}
|
||||
|
||||
//in case of multi-threading, don't submit messages while the GUI is rendering (causing crashes)
|
||||
static bool gBlockGuiMessages = false;
|
||||
|
||||
void MyGuiPrintf(const char* msg)
|
||||
{
|
||||
printf("b3Printf: %s\n",msg);
|
||||
if (!gDisableDemoSelection)
|
||||
if (!gDisableDemoSelection && !gBlockGuiMessages)
|
||||
{
|
||||
gui2->textOutput(msg);
|
||||
gui2->forceUpdateScrollBars();
|
||||
@ -745,7 +750,7 @@ void MyGuiPrintf(const char* msg)
|
||||
void MyStatusBarPrintf(const char* msg)
|
||||
{
|
||||
printf("b3Printf: %s\n", msg);
|
||||
if (!gDisableDemoSelection)
|
||||
if (!gDisableDemoSelection && !gBlockGuiMessages)
|
||||
{
|
||||
bool isLeft = true;
|
||||
gui2->setStatusBarMessage(msg,isLeft);
|
||||
@ -756,7 +761,7 @@ void MyStatusBarPrintf(const char* msg)
|
||||
void MyStatusBarError(const char* msg)
|
||||
{
|
||||
printf("Warning: %s\n", msg);
|
||||
if (!gDisableDemoSelection)
|
||||
if (!gDisableDemoSelection && !gBlockGuiMessages)
|
||||
{
|
||||
bool isLeft = false;
|
||||
gui2->setStatusBarMessage(msg,isLeft);
|
||||
@ -1016,7 +1021,22 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
|
||||
b3CommandLineArgs args(argc,argv);
|
||||
|
||||
loadCurrentSettings(startFileName, args);
|
||||
if (args.CheckCmdLineFlag("nogui"))
|
||||
{
|
||||
renderGrid = false;
|
||||
renderGui = false;
|
||||
}
|
||||
if (args.CheckCmdLineFlag("tracing"))
|
||||
{
|
||||
m_firstTiming = true;
|
||||
gProfileDisabled = false;//true;
|
||||
b3SetCustomEnterProfileZoneFunc(MyEnterProfileZoneFunc);
|
||||
b3SetCustomLeaveProfileZoneFunc(MyLeaveProfileZoneFunc);
|
||||
|
||||
//also for Bullet 2.x API
|
||||
btSetCustomEnterProfileZoneFunc(MyEnterProfileZoneFunc);
|
||||
btSetCustomLeaveProfileZoneFunc(MyLeaveProfileZoneFunc);
|
||||
}
|
||||
args.GetCmdLineArgument("fixed_timestep",gFixedTimeStep);
|
||||
args.GetCmdLineArgument("png_skip_frames", gPngSkipFrames);
|
||||
///The OpenCL rigid body pipeline is experimental and
|
||||
@ -1029,6 +1049,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
|
||||
enable_experimental_opencl = true;
|
||||
gAllExamples->initOpenCLExampleEntries();
|
||||
}
|
||||
|
||||
if (args.CheckCmdLineFlag("disable_retina"))
|
||||
{
|
||||
gAllowRetina = false;
|
||||
@ -1456,7 +1477,11 @@ void OpenGLExampleBrowser::update(float deltaTime)
|
||||
|
||||
if (m_internalData->m_gui)
|
||||
{
|
||||
gBlockGuiMessages = true;
|
||||
m_internalData->m_gui->draw(s_instancingRenderer->getScreenWidth(), s_instancingRenderer->getScreenHeight());
|
||||
|
||||
|
||||
gBlockGuiMessages = false;
|
||||
}
|
||||
|
||||
if (sUseOpenGL2)
|
||||
|
@ -19,13 +19,15 @@
|
||||
|
||||
#include "LinearMath/btAlignedAllocator.h"
|
||||
|
||||
static double gMinUpdateTimeMicroSecs = 1000.;
|
||||
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
{
|
||||
b3CommandLineArgs args(argc, argv);
|
||||
b3Clock clock;
|
||||
|
||||
args.GetCmdLineArgument("minUpdateTimeMicroSecs",gMinUpdateTimeMicroSecs);
|
||||
|
||||
ExampleEntriesAll examples;
|
||||
examples.initExampleEntries();
|
||||
@ -45,9 +47,18 @@ int main(int argc, char* argv[])
|
||||
do
|
||||
{
|
||||
float deltaTimeInSeconds = clock.getTimeMicroseconds() / 1000000.f;
|
||||
clock.reset();
|
||||
exampleBrowser->update(deltaTimeInSeconds);
|
||||
|
||||
if (deltaTimeInSeconds > 0.1)
|
||||
{
|
||||
deltaTimeInSeconds = 0.1;
|
||||
}
|
||||
if (deltaTimeInSeconds < (gMinUpdateTimeMicroSecs/1e6))
|
||||
{
|
||||
b3Clock::usleep(gMinUpdateTimeMicroSecs/10.);
|
||||
} else
|
||||
{
|
||||
clock.reset();
|
||||
exampleBrowser->update(deltaTimeInSeconds);
|
||||
}
|
||||
} while (!exampleBrowser->requestedExit());
|
||||
}
|
||||
delete exampleBrowser;
|
||||
|
@ -8,7 +8,6 @@
|
||||
#include "Bullet3Common/b3FileUtils.h"
|
||||
#include "../../ThirdPartyLibs/stb_image/stb_image.h"
|
||||
|
||||
|
||||
bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData)
|
||||
{
|
||||
|
||||
|
@ -447,6 +447,8 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
|
||||
|
||||
btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix)
|
||||
{
|
||||
BT_PROFILE("convertURDFToCollisionShape");
|
||||
|
||||
btCollisionShape* shape = 0;
|
||||
|
||||
switch (collision->m_geometry.m_type)
|
||||
@ -677,6 +679,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
||||
|
||||
if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
||||
{
|
||||
BT_PROFILE("convert trimesh");
|
||||
btTriangleMesh* meshInterface = new btTriangleMesh();
|
||||
for (int i=0;i<glmesh->m_numIndices/3;i++)
|
||||
{
|
||||
@ -692,6 +695,8 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
||||
shape = trimesh;
|
||||
} else
|
||||
{
|
||||
BT_PROFILE("convert btConvexHullShape");
|
||||
|
||||
btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
|
||||
convexHull->optimizeConvexHull();
|
||||
//convexHull->initializePolyhedralFeatures();
|
||||
@ -727,6 +732,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
||||
|
||||
static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture>& texturesOut)
|
||||
{
|
||||
BT_PROFILE("convertURDFToVisualShapeInternal");
|
||||
|
||||
|
||||
GLInstanceGraphicsShape* glmesh = 0;
|
||||
@ -981,6 +987,8 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
||||
//if we have a convex, tesselate into localVertices/localIndices
|
||||
if ((glmesh==0) && convexColShape)
|
||||
{
|
||||
BT_PROFILE("convexColShape");
|
||||
|
||||
btShapeHull* hull = new btShapeHull(convexColShape);
|
||||
hull->buildHull(0.0);
|
||||
{
|
||||
@ -1029,7 +1037,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
||||
|
||||
if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0)
|
||||
{
|
||||
|
||||
BT_PROFILE("glmesh");
|
||||
int baseIndex = verticesOut.size();
|
||||
|
||||
|
||||
|
@ -13,8 +13,13 @@ struct PrimInternalData
|
||||
GLint m_positionAttribute;
|
||||
GLint m_textureAttribute;
|
||||
GLuint m_vertexBuffer;
|
||||
GLuint m_vertexArrayObject;
|
||||
GLuint m_vertexBuffer2;
|
||||
|
||||
GLuint m_vertexArrayObject;
|
||||
GLuint m_vertexArrayObject2;
|
||||
|
||||
GLuint m_indexBuffer;
|
||||
GLuint m_indexBuffer2;
|
||||
GLuint m_texturehandle;
|
||||
};
|
||||
|
||||
|
@ -48,10 +48,20 @@ static const char* fragmentShader3D= \
|
||||
|
||||
|
||||
static unsigned int s_indexData[6] = {0,1,2,0,2,3};
|
||||
#define MAX_VERTICES2 8192
|
||||
struct PrimInternalData2
|
||||
{
|
||||
PrimInternalData2()
|
||||
:m_numVerticesText(0),
|
||||
m_numVerticesRect(0)
|
||||
{
|
||||
}
|
||||
int m_numVerticesText;
|
||||
int m_numVerticesRect;
|
||||
PrimVertex m_verticesText[MAX_VERTICES2];
|
||||
PrimVertex m_verticesRect[MAX_VERTICES2];
|
||||
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
GLPrimitiveRenderer::GLPrimitiveRenderer(int screenWidth, int screenHeight)
|
||||
:m_screenWidth(screenWidth),
|
||||
@ -59,6 +69,7 @@ m_screenHeight(screenHeight)
|
||||
{
|
||||
|
||||
m_data = new PrimInternalData;
|
||||
m_data2 = new PrimInternalData2;
|
||||
|
||||
m_data->m_shaderProg = gltLoadShaderPair(vertexShader3D,fragmentShader3D);
|
||||
|
||||
@ -109,6 +120,13 @@ void GLPrimitiveRenderer::loadBufferData()
|
||||
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer);
|
||||
glBufferData(GL_ARRAY_BUFFER, 4 * sizeof(PrimVertex), vertexData, GL_DYNAMIC_DRAW);
|
||||
|
||||
glGenVertexArrays(1, &m_data->m_vertexArrayObject2);
|
||||
glBindVertexArray(m_data->m_vertexArrayObject2);
|
||||
glGenBuffers(1, &m_data->m_vertexBuffer2);
|
||||
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer2);
|
||||
glBufferData(GL_ARRAY_BUFFER, MAX_VERTICES2 * sizeof(PrimVertex), 0, GL_DYNAMIC_DRAW);
|
||||
|
||||
|
||||
assert(glGetError()==GL_NO_ERROR);
|
||||
|
||||
|
||||
@ -116,7 +134,23 @@ void GLPrimitiveRenderer::loadBufferData()
|
||||
glGenBuffers(1, &m_data->m_indexBuffer);
|
||||
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer);
|
||||
glBufferData(GL_ELEMENT_ARRAY_BUFFER,6*sizeof(int), s_indexData,GL_STATIC_DRAW);
|
||||
|
||||
|
||||
unsigned int indexData[MAX_VERTICES2*2];
|
||||
int count=0;
|
||||
for (int i=0;i<MAX_VERTICES2;i+=4)
|
||||
{
|
||||
indexData[count++]=i;
|
||||
indexData[count++]=i+1;
|
||||
indexData[count++]=i+2;
|
||||
|
||||
indexData[count++]=i;
|
||||
indexData[count++]=i+2;
|
||||
indexData[count++]=i+3;
|
||||
}
|
||||
glGenBuffers(1, &m_data->m_indexBuffer2);
|
||||
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer2);
|
||||
glBufferData(GL_ELEMENT_ARRAY_BUFFER,count*sizeof(int), indexData,GL_STATIC_DRAW);
|
||||
|
||||
glEnableVertexAttribArray(m_data->m_positionAttribute);
|
||||
glEnableVertexAttribArray(m_data->m_colourAttribute);
|
||||
assert(glGetError()==GL_NO_ERROR);
|
||||
@ -182,6 +216,7 @@ GLPrimitiveRenderer::~GLPrimitiveRenderer()
|
||||
glBindTexture(GL_TEXTURE_2D,0);
|
||||
glDeleteProgram(m_data->m_shaderProg);
|
||||
delete m_data;
|
||||
delete m_data2;
|
||||
}
|
||||
|
||||
void GLPrimitiveRenderer::drawLine()
|
||||
@ -299,6 +334,180 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
|
||||
|
||||
}
|
||||
|
||||
void GLPrimitiveRenderer::drawTexturedRect3D2Text( bool useRGBA)
|
||||
{
|
||||
drawTexturedRect3D2(&m_data2->m_verticesText[0],m_data2->m_numVerticesText,useRGBA);
|
||||
m_data2->m_numVerticesText = 0;
|
||||
}
|
||||
|
||||
void GLPrimitiveRenderer::drawTexturedRect3D2( PrimVertex* vertices, int numVertices, bool useRGBA)
|
||||
{
|
||||
//B3_PROFILE("drawTexturedRect3D2");
|
||||
if (numVertices==0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
//B3_PROFILE("GLPrimitiveRenderer::drawTexturedRect3D");
|
||||
|
||||
assert(glGetError()==GL_NO_ERROR);
|
||||
float identity[16]={1,0,0,0,
|
||||
0,1,0,0,
|
||||
0,0,1,0,
|
||||
0,0,0,1};
|
||||
|
||||
glUseProgram(m_data->m_shaderProg);
|
||||
|
||||
glUniformMatrix4fv(m_data->m_viewmatUniform, 1, false, identity);
|
||||
glUniformMatrix4fv(m_data->m_projMatUniform, 1, false, identity);
|
||||
|
||||
assert(glGetError()==GL_NO_ERROR);
|
||||
|
||||
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer2);
|
||||
glBindVertexArray(m_data->m_vertexArrayObject2);
|
||||
|
||||
bool useFiltering = false;
|
||||
if (useFiltering)
|
||||
{
|
||||
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
|
||||
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
|
||||
} else
|
||||
{
|
||||
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
|
||||
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
|
||||
}
|
||||
|
||||
/* PrimVertex vertexData[4] = {
|
||||
v0,v1,v2,v3
|
||||
};
|
||||
*/
|
||||
|
||||
glBufferSubData(GL_ARRAY_BUFFER, 0,numVertices * sizeof(PrimVertex), vertices);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
assert(glGetError()==GL_NO_ERROR);
|
||||
|
||||
PrimVec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) );
|
||||
if (useRGBA)
|
||||
{
|
||||
p.p[0] = 1.f;
|
||||
p.p[1] = 1.f;
|
||||
}
|
||||
|
||||
glUniform2fv(m_data->m_positionUniform, 1, (const GLfloat *)&p);
|
||||
|
||||
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
|
||||
|
||||
assert(glGetError()==GL_NO_ERROR);
|
||||
|
||||
glEnableVertexAttribArray(m_data->m_positionAttribute);
|
||||
assert(glGetError()==GL_NO_ERROR);
|
||||
|
||||
glEnableVertexAttribArray(m_data->m_colourAttribute);
|
||||
assert(glGetError()==GL_NO_ERROR);
|
||||
|
||||
glEnableVertexAttribArray(m_data->m_textureAttribute);
|
||||
|
||||
glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)0);
|
||||
glVertexAttribPointer(m_data->m_colourAttribute , 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)sizeof(PrimVec4));
|
||||
glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)(sizeof(PrimVec4)+sizeof(PrimVec4)));
|
||||
assert(glGetError()==GL_NO_ERROR);
|
||||
|
||||
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer2);
|
||||
|
||||
//glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
|
||||
int indexCount = (numVertices/4)*6;
|
||||
assert(glGetError()==GL_NO_ERROR);
|
||||
|
||||
|
||||
glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0);
|
||||
assert(glGetError()==GL_NO_ERROR);
|
||||
|
||||
|
||||
glBindVertexArray(0);
|
||||
assert(glGetError()==GL_NO_ERROR);
|
||||
|
||||
glBindBuffer(GL_ARRAY_BUFFER, 0);
|
||||
assert(glGetError()==GL_NO_ERROR);
|
||||
|
||||
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
|
||||
assert(glGetError()==GL_NO_ERROR);
|
||||
|
||||
//glDisableVertexAttribArray(m_data->m_textureAttribute);
|
||||
assert(glGetError()==GL_NO_ERROR);
|
||||
|
||||
glUseProgram(0);
|
||||
|
||||
assert(glGetError()==GL_NO_ERROR);
|
||||
|
||||
|
||||
}
|
||||
|
||||
void GLPrimitiveRenderer::drawTexturedRect2a(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA)
|
||||
{
|
||||
|
||||
PrimVertex vertexData[4] = {
|
||||
{ PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)},
|
||||
{ PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)},
|
||||
{ PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)},
|
||||
{ PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)}
|
||||
};
|
||||
|
||||
int sz = m_data2->m_numVerticesText;
|
||||
|
||||
m_data2->m_verticesRect[m_data2->m_numVerticesRect++]=vertexData[0];
|
||||
m_data2->m_verticesRect[m_data2->m_numVerticesRect++]=vertexData[1];
|
||||
m_data2->m_verticesRect[m_data2->m_numVerticesRect++]=vertexData[2];
|
||||
m_data2->m_verticesRect[m_data2->m_numVerticesRect++]=vertexData[3];
|
||||
|
||||
|
||||
if (m_data2->m_numVerticesRect>=MAX_VERTICES2)
|
||||
{
|
||||
flushBatchedRects();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void GLPrimitiveRenderer::flushBatchedRects()
|
||||
{
|
||||
if (m_data2->m_numVerticesRect==0)
|
||||
return;
|
||||
|
||||
glActiveTexture(GL_TEXTURE0);
|
||||
assert(glGetError()==GL_NO_ERROR);
|
||||
glBindTexture(GL_TEXTURE_2D,m_data->m_texturehandle);
|
||||
drawTexturedRect3D2(m_data2->m_verticesRect, m_data2->m_numVerticesRect,0);
|
||||
m_data2->m_numVerticesRect=0;
|
||||
}
|
||||
void GLPrimitiveRenderer::drawTexturedRect2(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA)
|
||||
{
|
||||
|
||||
PrimVertex vertexData[4] = {
|
||||
{ PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)},
|
||||
{ PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)},
|
||||
{ PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)},
|
||||
{ PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)}
|
||||
};
|
||||
|
||||
int sz = m_data2->m_numVerticesText;
|
||||
|
||||
m_data2->m_verticesText[m_data2->m_numVerticesText++]=vertexData[0];
|
||||
m_data2->m_verticesText[m_data2->m_numVerticesText++]=vertexData[1];
|
||||
m_data2->m_verticesText[m_data2->m_numVerticesText++]=vertexData[2];
|
||||
m_data2->m_verticesText[m_data2->m_numVerticesText++]=vertexData[3];
|
||||
|
||||
|
||||
if (m_data2->m_numVerticesText>=MAX_VERTICES2)
|
||||
{
|
||||
drawTexturedRect3D2(m_data2->m_verticesText, m_data2->m_numVerticesText,useRGBA);
|
||||
m_data2->m_numVerticesText=0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA)
|
||||
{
|
||||
|
@ -5,6 +5,8 @@
|
||||
|
||||
struct PrimVec2
|
||||
{
|
||||
PrimVec2()
|
||||
{}
|
||||
PrimVec2(float x, float y)
|
||||
{
|
||||
p[0] = x;
|
||||
@ -28,12 +30,21 @@ struct PrimVec4
|
||||
float p[4];
|
||||
};
|
||||
|
||||
typedef struct
|
||||
struct PrimVertex
|
||||
{
|
||||
PrimVertex(const PrimVec4 & p, const PrimVec4 & c, const PrimVec2& u)
|
||||
:position(p),
|
||||
colour(c),
|
||||
uv(u)
|
||||
{
|
||||
}
|
||||
|
||||
PrimVertex()
|
||||
{}
|
||||
PrimVec4 position;
|
||||
PrimVec4 colour;
|
||||
PrimVec2 uv;
|
||||
} PrimVertex;
|
||||
} ;
|
||||
|
||||
|
||||
class GLPrimitiveRenderer
|
||||
@ -42,7 +53,7 @@ class GLPrimitiveRenderer
|
||||
int m_screenHeight;
|
||||
|
||||
struct PrimInternalData* m_data;
|
||||
|
||||
struct PrimInternalData2* m_data2;
|
||||
void loadBufferData();
|
||||
|
||||
public:
|
||||
@ -56,6 +67,14 @@ public:
|
||||
void drawLine();//float from[4], float to[4], float color[4]);
|
||||
void setScreenSize(int width, int height);
|
||||
|
||||
void drawTexturedRect2(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA=0);
|
||||
void drawTexturedRect2a(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA=0);
|
||||
void flushBatchedRects();
|
||||
|
||||
|
||||
void drawTexturedRect3D2Text(bool useRGBA = true);
|
||||
void drawTexturedRect3D2(PrimVertex* vertices, int numVertices, bool useRGBA = true);
|
||||
|
||||
PrimInternalData* getData()
|
||||
{
|
||||
return m_data;
|
||||
|
@ -161,6 +161,7 @@ public:
|
||||
|
||||
virtual void StartClip()
|
||||
{
|
||||
|
||||
if (m_useTrueTypeFont)
|
||||
sth_flush_draw(m_font);
|
||||
Gwen::Rect rect = ClipRegion();
|
||||
@ -181,6 +182,7 @@ public:
|
||||
|
||||
virtual void EndClip()
|
||||
{
|
||||
|
||||
if (m_useTrueTypeFont)
|
||||
sth_flush_draw(m_font);
|
||||
glDisable( GL_SCISSOR_TEST );
|
||||
@ -196,16 +198,23 @@ public:
|
||||
}
|
||||
virtual void DrawFilledRect( Gwen::Rect rect )
|
||||
{
|
||||
// BT_PROFILE("GWEN_DrawFilledRect");
|
||||
Translate( rect );
|
||||
|
||||
m_primitiveRenderer->drawRect(rect.x, rect.y+m_yOffset, rect.x+rect.w, rect.y+rect.h+m_yOffset, m_currentColor);
|
||||
m_primitiveRenderer->drawRect(rect.x, rect.y+m_yOffset,
|
||||
rect.x+rect.w, rect.y+rect.h+m_yOffset, m_currentColor);
|
||||
|
||||
|
||||
// m_primitiveRenderer->drawTexturedRect2a(rect.x, rect.y+m_yOffset,
|
||||
// rect.x+rect.w, rect.y+rect.h+m_yOffset, m_currentColor,0,0,1,1);
|
||||
// m_yOffset+=rect.h+10;
|
||||
|
||||
}
|
||||
|
||||
void RenderText( Gwen::Font* pFont, Gwen::Point rasterPos, const Gwen::UnicodeString& text )
|
||||
{
|
||||
|
||||
// BT_PROFILE("GWEN_RenderText");
|
||||
|
||||
Gwen::String str = Gwen::Utility::UnicodeToString(text);
|
||||
const char* unicodeText = (const char*)str.c_str();
|
||||
|
||||
@ -246,32 +255,33 @@ public:
|
||||
|
||||
glBindTexture(GL_TEXTURE_2D,m_fontTextureId);
|
||||
float width = r.x;
|
||||
|
||||
while (unicodeText[pos])
|
||||
{
|
||||
|
||||
int c = unicodeText[pos];
|
||||
r.h = m_currentFont->m_CharHeight;
|
||||
r.w = m_currentFont->m_CharWidth[c]+extraSpacing;
|
||||
Gwen::Rect rect = r;
|
||||
Translate( rect );
|
||||
|
||||
m_primitiveRenderer->drawTexturedRect(rect.x, rect.y+m_yOffset, rect.x+rect.w, rect.y+rect.h+m_yOffset, m_currentColor,m_currentFont->m_CharU0[c],m_currentFont->m_CharV0[c],m_currentFont->m_CharU1[c],m_currentFont->m_CharV1[c]);
|
||||
m_primitiveRenderer->drawTexturedRect2(rect.x, rect.y+m_yOffset, rect.x+rect.w, rect.y+rect.h+m_yOffset, m_currentColor,m_currentFont->m_CharU0[c],m_currentFont->m_CharV0[c],m_currentFont->m_CharU1[c],m_currentFont->m_CharV1[c]);
|
||||
|
||||
//DrawTexturedRect(0,r,m_currentFont->m_CharU0[c],m_currentFont->m_CharV0[c],m_currentFont->m_CharU1[c],m_currentFont->m_CharV1[c]);
|
||||
// DrawFilledRect(r);
|
||||
|
||||
|
||||
|
||||
width += r.w;
|
||||
r.x = width;
|
||||
pos++;
|
||||
|
||||
}
|
||||
{
|
||||
m_primitiveRenderer->drawTexturedRect3D2Text(false);
|
||||
}
|
||||
glBindTexture(GL_TEXTURE_2D,0);
|
||||
}
|
||||
|
||||
}
|
||||
Gwen::Point MeasureText( Gwen::Font* pFont, const Gwen::UnicodeString& text )
|
||||
{
|
||||
// BT_PROFILE("GWEN_MeasureText");
|
||||
Gwen::String str = Gwen::Utility::UnicodeToString(text);
|
||||
const char* unicodeText = (const char*)str.c_str();
|
||||
|
||||
@ -341,7 +351,7 @@ public:
|
||||
|
||||
virtual void DrawTexturedRect( Gwen::Texture* pTexture, Gwen::Rect rect, float u1=0.0f, float v1=0.0f, float u2=1.0f, float v2=1.0f )
|
||||
{
|
||||
|
||||
// BT_PROFILE("DrawTexturedRect");
|
||||
Translate( rect );
|
||||
|
||||
//float eraseColor[4] = {0,0,0,0};
|
||||
|
@ -207,7 +207,7 @@ bool PhysicsClientSharedMemory::connect() {
|
||||
m_data->m_isConnected = true;
|
||||
}
|
||||
} else {
|
||||
b3Error("Cannot connect to shared memory");
|
||||
b3Warning("Cannot connect to shared memory");
|
||||
return false;
|
||||
}
|
||||
#if 0
|
||||
|
@ -1044,6 +1044,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
|
||||
bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
|
||||
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes)
|
||||
{
|
||||
BT_PROFILE("loadURDF");
|
||||
btAssert(m_data->m_dynamicsWorld);
|
||||
if (!m_data->m_dynamicsWorld)
|
||||
{
|
||||
|
@ -239,6 +239,10 @@ struct MotionArgs
|
||||
}
|
||||
}
|
||||
b3CriticalSection* m_cs;
|
||||
b3CriticalSection* m_cs2;
|
||||
b3CriticalSection* m_cs3;
|
||||
b3CriticalSection* m_csGUI;
|
||||
|
||||
|
||||
btAlignedObjectArray<MyMouseCommand> m_mouseCommands;
|
||||
|
||||
@ -289,16 +293,21 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
|
||||
|
||||
do
|
||||
{
|
||||
BT_PROFILE("loop");
|
||||
deltaTimeInSeconds+= double(clock.getTimeMicroseconds())/1000000.;
|
||||
clock.reset();
|
||||
|
||||
if (deltaTimeInSeconds<(1./5000.))
|
||||
{
|
||||
|
||||
skip++;
|
||||
skip1++;
|
||||
//if (skip1>105)
|
||||
if (skip1>5)
|
||||
{
|
||||
BT_PROFILE("b3Clock::usleep(250)");
|
||||
b3Clock::usleep(250);
|
||||
skip1 = 0;
|
||||
}
|
||||
} else
|
||||
{
|
||||
@ -358,9 +367,9 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
|
||||
//b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime);
|
||||
}
|
||||
|
||||
clock.reset();
|
||||
|
||||
|
||||
args->m_cs->lock();
|
||||
args->m_csGUI->lock();
|
||||
|
||||
int numSendVrControllers = 0;
|
||||
for (int i=0;i<MAX_VR_CONTROLLERS;i++)
|
||||
@ -383,14 +392,16 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
|
||||
}
|
||||
}
|
||||
|
||||
args->m_cs->unlock();
|
||||
|
||||
args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds, args->m_sendVrControllerEvents,numSendVrControllers);
|
||||
args->m_csGUI->unlock();
|
||||
{
|
||||
BT_PROFILE("stepSimulationRealTime");
|
||||
args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds, args->m_sendVrControllerEvents,numSendVrControllers);
|
||||
}
|
||||
deltaTimeInSeconds = 0;
|
||||
|
||||
}
|
||||
|
||||
args->m_cs->lock();
|
||||
args->m_csGUI->lock();
|
||||
for (int i = 0; i < args->m_mouseCommands.size(); i++)
|
||||
{
|
||||
switch (args->m_mouseCommands[i].m_type)
|
||||
@ -418,10 +429,12 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
|
||||
}
|
||||
}
|
||||
args->m_mouseCommands.clear();
|
||||
args->m_cs->unlock();
|
||||
args->m_csGUI->unlock();
|
||||
|
||||
|
||||
args->m_physicsServerPtr->processClientCommands();
|
||||
{
|
||||
BT_PROFILE("processClientCommands");
|
||||
args->m_physicsServerPtr->processClientCommands();
|
||||
}
|
||||
|
||||
} while (args->m_cs->getSharedParam(0)!=eRequestTerminateMotion);
|
||||
} else
|
||||
@ -478,9 +491,10 @@ class MultiThreadedOpenGLGuiHelper : public GUIHelperInterface
|
||||
CommonGraphicsApp* m_app;
|
||||
|
||||
b3CriticalSection* m_cs;
|
||||
|
||||
b3CriticalSection* m_cs2;
|
||||
b3CriticalSection* m_cs3;
|
||||
b3CriticalSection* m_csGUI;
|
||||
|
||||
|
||||
|
||||
public:
|
||||
|
||||
@ -506,11 +520,43 @@ public:
|
||||
int m_textureId;
|
||||
int m_instanceId;
|
||||
|
||||
|
||||
void mainThreadRelease()
|
||||
{
|
||||
BT_PROFILE("mainThreadRelease");
|
||||
|
||||
getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
|
||||
getCriticalSection3()->lock();
|
||||
getCriticalSection2()->unlock();
|
||||
getCriticalSection()->lock();
|
||||
getCriticalSection2()->lock();
|
||||
getCriticalSection()->unlock();
|
||||
getCriticalSection3()->unlock();
|
||||
|
||||
}
|
||||
|
||||
void workerThreadWait()
|
||||
{
|
||||
BT_PROFILE("workerThreadWait");
|
||||
m_cs2->lock();
|
||||
m_cs->unlock();
|
||||
m_cs2->unlock();
|
||||
m_cs3->lock();
|
||||
m_cs3->unlock();
|
||||
|
||||
|
||||
|
||||
while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(100);
|
||||
}
|
||||
}
|
||||
|
||||
MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper)
|
||||
:m_app(app)
|
||||
,m_cs(0),
|
||||
m_cs2(0),
|
||||
m_cs3(0),
|
||||
m_csGUI(0),
|
||||
m_uidGenerator(0),
|
||||
m_texels(0),
|
||||
m_textureId(-1)
|
||||
@ -534,6 +580,32 @@ public:
|
||||
return m_cs;
|
||||
}
|
||||
|
||||
|
||||
void setCriticalSection2(b3CriticalSection* cs)
|
||||
{
|
||||
m_cs2 = cs;
|
||||
}
|
||||
|
||||
b3CriticalSection* getCriticalSection2()
|
||||
{
|
||||
return m_cs2;
|
||||
}
|
||||
|
||||
void setCriticalSection3(b3CriticalSection* cs)
|
||||
{
|
||||
m_cs3 = cs;
|
||||
}
|
||||
|
||||
void setCriticalSectionGUI(b3CriticalSection* cs)
|
||||
{
|
||||
m_csGUI = cs;
|
||||
}
|
||||
|
||||
|
||||
b3CriticalSection* getCriticalSection3()
|
||||
{
|
||||
return m_cs3;
|
||||
}
|
||||
btRigidBody* m_body;
|
||||
btVector3 m_color3;
|
||||
virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color)
|
||||
@ -542,11 +614,8 @@ public:
|
||||
m_color3 = color;
|
||||
m_cs->lock();
|
||||
m_cs->setSharedParam(1,eGUIHelperCreateRigidBodyGraphicsObject);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(1000);
|
||||
}
|
||||
workerThreadWait();
|
||||
|
||||
}
|
||||
|
||||
btCollisionObject* m_obj;
|
||||
@ -558,11 +627,7 @@ public:
|
||||
m_color2 = color;
|
||||
m_cs->lock();
|
||||
m_cs->setSharedParam(1,eGUIHelperCreateCollisionObjectGraphicsObject);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(1000);
|
||||
}
|
||||
workerThreadWait();
|
||||
|
||||
}
|
||||
|
||||
@ -572,11 +637,7 @@ public:
|
||||
m_colShape = collisionShape;
|
||||
m_cs->lock();
|
||||
m_cs->setSharedParam(1,eGUIHelperCreateCollisionShapeGraphicsObject);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(1000);
|
||||
}
|
||||
workerThreadWait();
|
||||
|
||||
}
|
||||
|
||||
@ -584,7 +645,7 @@ public:
|
||||
{
|
||||
//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)
|
||||
if ( m_childGuiHelper->getRenderInterface() && m_childGuiHelper->getRenderInterface()->getTotalNumInstances()>0)
|
||||
{
|
||||
m_childGuiHelper->syncPhysicsToGraphics(rbWorld);
|
||||
}
|
||||
@ -608,11 +669,10 @@ public:
|
||||
|
||||
m_cs->lock();
|
||||
m_cs->setSharedParam(1,eGUIHelperRegisterTexture);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(1000);
|
||||
}
|
||||
|
||||
workerThreadWait();
|
||||
|
||||
|
||||
return m_textureId;
|
||||
}
|
||||
virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
|
||||
@ -626,11 +686,8 @@ public:
|
||||
|
||||
m_cs->lock();
|
||||
m_cs->setSharedParam(1,eGUIHelperRegisterGraphicsShape);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(1000);
|
||||
}
|
||||
workerThreadWait();
|
||||
|
||||
return m_shapeIndex;
|
||||
}
|
||||
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
|
||||
@ -643,11 +700,7 @@ public:
|
||||
|
||||
m_cs->lock();
|
||||
m_cs->setSharedParam(1,eGUIHelperRegisterGraphicsInstance);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(1000);
|
||||
}
|
||||
workerThreadWait();
|
||||
return m_instanceId;
|
||||
}
|
||||
|
||||
@ -655,11 +708,7 @@ public:
|
||||
{
|
||||
m_cs->lock();
|
||||
m_cs->setSharedParam(1,eGUIHelperRemoveAllGraphicsInstances);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(1000);
|
||||
}
|
||||
workerThreadWait();
|
||||
}
|
||||
|
||||
virtual Common2dCanvasInterface* get2dCanvasInterface()
|
||||
@ -730,11 +779,7 @@ public:
|
||||
m_numPixelsCopied = numPixelsCopied;
|
||||
|
||||
m_cs->setSharedParam(1,eGUIHelperCopyCameraImageData);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(1000);
|
||||
}
|
||||
workerThreadWait();
|
||||
}
|
||||
|
||||
|
||||
@ -745,11 +790,7 @@ public:
|
||||
m_dynamicsWorld = rbWorld;
|
||||
m_cs->lock();
|
||||
m_cs->setSharedParam(1, eGUIHelperAutogenerateGraphicsObjects);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1) != eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(1000);
|
||||
}
|
||||
workerThreadWait();
|
||||
}
|
||||
|
||||
virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
|
||||
@ -780,11 +821,7 @@ public:
|
||||
|
||||
m_cs->lock();
|
||||
m_cs->setSharedParam(1, eGUIUserDebugAddText);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1) != eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(150);
|
||||
}
|
||||
workerThreadWait();
|
||||
|
||||
return m_userDebugText[m_userDebugText.size()-1].m_itemUniqueId;
|
||||
}
|
||||
@ -811,11 +848,7 @@ public:
|
||||
|
||||
m_cs->lock();
|
||||
m_cs->setSharedParam(1, eGUIUserDebugAddLine);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1) != eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(150);
|
||||
}
|
||||
workerThreadWait();
|
||||
return m_userDebugLines[m_userDebugLines.size()-1].m_itemUniqueId;
|
||||
}
|
||||
|
||||
@ -826,22 +859,14 @@ public:
|
||||
m_removeDebugItemUid = debugItemUniqueId;
|
||||
m_cs->lock();
|
||||
m_cs->setSharedParam(1, eGUIUserDebugRemoveItem);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1) != eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(150);
|
||||
}
|
||||
workerThreadWait();
|
||||
|
||||
}
|
||||
virtual void removeAllUserDebugItems( )
|
||||
{
|
||||
m_cs->lock();
|
||||
m_cs->setSharedParam(1, eGUIUserDebugRemoveAllItems);
|
||||
m_cs->unlock();
|
||||
while (m_cs->getSharedParam(1) != eGUIHelperIdle)
|
||||
{
|
||||
b3Clock::usleep(150);
|
||||
}
|
||||
workerThreadWait();
|
||||
|
||||
}
|
||||
|
||||
@ -934,9 +959,10 @@ public:
|
||||
cmd.m_rayFrom = rayFrom;
|
||||
cmd.m_rayTo = rayTo;
|
||||
cmd.m_type = MyMouseMove;
|
||||
m_args[0].m_cs->lock();
|
||||
m_args[0].m_csGUI->lock();
|
||||
m_args[0].m_mouseCommands.push_back(cmd);
|
||||
m_args[0].m_cs->unlock();
|
||||
m_args[0].m_csGUI->unlock();
|
||||
|
||||
return false;
|
||||
};
|
||||
|
||||
@ -969,10 +995,11 @@ public:
|
||||
cmd.m_rayFrom = rayFrom;
|
||||
cmd.m_rayTo = rayTo;
|
||||
cmd.m_type = MyMouseButtonDown;
|
||||
m_args[0].m_cs->lock();
|
||||
|
||||
m_args[0].m_csGUI->lock();
|
||||
m_args[0].m_mouseCommands.push_back(cmd);
|
||||
m_args[0].m_cs->unlock();
|
||||
|
||||
m_args[0].m_csGUI->unlock();
|
||||
|
||||
|
||||
}
|
||||
} else
|
||||
@ -984,9 +1011,10 @@ public:
|
||||
cmd.m_rayFrom.setValue(0,0,0);
|
||||
cmd.m_rayTo.setValue(0, 0, 0);
|
||||
cmd.m_type = MyMouseButtonUp;
|
||||
m_args[0].m_cs->lock();
|
||||
|
||||
m_args[0].m_csGUI->lock();
|
||||
m_args[0].m_mouseCommands.push_back(cmd);
|
||||
m_args[0].m_cs->unlock();
|
||||
m_args[0].m_csGUI->unlock();
|
||||
//remove p2p
|
||||
}
|
||||
}
|
||||
@ -1005,6 +1033,13 @@ public:
|
||||
{
|
||||
b3CommandLineArgs args(argc,argv);
|
||||
loadCurrentSettingsVR(args);
|
||||
int shmemKey;
|
||||
|
||||
if (args.GetCmdLineArgument("sharedMemoryKey", shmemKey))
|
||||
{
|
||||
setSharedMemoryKey(shmemKey);
|
||||
}
|
||||
|
||||
if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0]))
|
||||
{
|
||||
printf("camPosX=%f\n", gVRTeleportPos1[0]);
|
||||
@ -1035,7 +1070,7 @@ public:
|
||||
|
||||
if (args.CheckCmdLineFlag("norobotassets"))
|
||||
{
|
||||
gCreateDefaultRobotAssets = false;
|
||||
// gCreateDefaultRobotAssets = false;
|
||||
}
|
||||
|
||||
|
||||
@ -1149,6 +1184,10 @@ void PhysicsServerExample::initPhysics()
|
||||
for (int w=0;w<MAX_MOTION_NUM_THREADS;w++)
|
||||
{
|
||||
m_args[w].m_cs = m_threadSupport->createCriticalSection();
|
||||
m_args[w].m_cs2 = m_threadSupport->createCriticalSection();
|
||||
m_args[w].m_cs3 = m_threadSupport->createCriticalSection();
|
||||
m_args[w].m_csGUI = m_threadSupport->createCriticalSection();
|
||||
|
||||
m_args[w].m_cs->setSharedParam(0,eMotionIsUnInitialized);
|
||||
int numMoving = 0;
|
||||
m_args[w].m_positions.resize(numMoving);
|
||||
@ -1165,6 +1204,13 @@ void PhysicsServerExample::initPhysics()
|
||||
|
||||
m_args[0].m_cs->setSharedParam(1,eGUIHelperIdle);
|
||||
m_multiThreadedHelper->setCriticalSection(m_args[0].m_cs);
|
||||
m_multiThreadedHelper->setCriticalSection2(m_args[0].m_cs2);
|
||||
m_multiThreadedHelper->setCriticalSection3(m_args[0].m_cs3);
|
||||
m_multiThreadedHelper->setCriticalSectionGUI(m_args[0].m_csGUI);
|
||||
|
||||
m_args[0].m_cs2->lock();
|
||||
|
||||
|
||||
m_isConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
|
||||
}
|
||||
|
||||
@ -1213,8 +1259,35 @@ bool PhysicsServerExample::wantsTermination()
|
||||
|
||||
void PhysicsServerExample::stepSimulation(float deltaTime)
|
||||
{
|
||||
BT_PROFILE("PhysicsServerExample::stepSimulation");
|
||||
|
||||
//this->m_physicsServer.processClientCommands();
|
||||
|
||||
for (int i = m_multiThreadedHelper->m_userDebugLines.size()-1;i>=0;i--)
|
||||
{
|
||||
if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime)
|
||||
{
|
||||
m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime -= deltaTime;
|
||||
if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime<=0)
|
||||
{
|
||||
m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1);
|
||||
m_multiThreadedHelper->m_userDebugLines.pop_back();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = m_multiThreadedHelper->m_userDebugText.size()-1;i>=0;i--)
|
||||
{
|
||||
if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime)
|
||||
{
|
||||
m_multiThreadedHelper->m_userDebugText[i].m_lifeTime -= deltaTime;
|
||||
if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime<=0)
|
||||
{
|
||||
m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1);
|
||||
m_multiThreadedHelper->m_userDebugText.pop_back();
|
||||
}
|
||||
}
|
||||
}
|
||||
//check if any graphics related tasks are requested
|
||||
|
||||
switch (m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
|
||||
@ -1222,27 +1295,21 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
|
||||
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();
|
||||
|
||||
m_multiThreadedHelper->mainThreadRelease();
|
||||
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();
|
||||
m_multiThreadedHelper->mainThreadRelease();
|
||||
|
||||
break;
|
||||
}
|
||||
case eGUIHelperCreateRigidBodyGraphicsObject:
|
||||
{
|
||||
m_multiThreadedHelper->m_childGuiHelper->createRigidBodyGraphicsObject(m_multiThreadedHelper->m_body,m_multiThreadedHelper->m_color3);
|
||||
m_multiThreadedHelper->getCriticalSection()->lock();
|
||||
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
|
||||
m_multiThreadedHelper->getCriticalSection()->unlock();
|
||||
m_multiThreadedHelper->mainThreadRelease();
|
||||
break;
|
||||
}
|
||||
case eGUIHelperRegisterTexture:
|
||||
@ -1250,11 +1317,7 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
|
||||
|
||||
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();
|
||||
|
||||
m_multiThreadedHelper->mainThreadRelease();
|
||||
break;
|
||||
}
|
||||
case eGUIHelperRegisterGraphicsShape:
|
||||
@ -1266,10 +1329,7 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
|
||||
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();
|
||||
m_multiThreadedHelper->mainThreadRelease();
|
||||
break;
|
||||
}
|
||||
case eGUIHelperRegisterGraphicsInstance:
|
||||
@ -1280,21 +1340,19 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
|
||||
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();
|
||||
m_multiThreadedHelper->mainThreadRelease();
|
||||
break;
|
||||
}
|
||||
case eGUIHelperRemoveAllGraphicsInstances:
|
||||
{
|
||||
m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances();
|
||||
int numRenderInstances = m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances();
|
||||
b3Assert(numRenderInstances==0);
|
||||
if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
|
||||
{
|
||||
int numRenderInstances = m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances();
|
||||
b3Assert(numRenderInstances==0);
|
||||
}
|
||||
m_multiThreadedHelper->mainThreadRelease();
|
||||
|
||||
m_multiThreadedHelper->getCriticalSection()->lock();
|
||||
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
|
||||
m_multiThreadedHelper->getCriticalSection()->unlock();
|
||||
break;
|
||||
}
|
||||
|
||||
@ -1312,34 +1370,26 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
|
||||
m_multiThreadedHelper->m_destinationWidth,
|
||||
m_multiThreadedHelper->m_destinationHeight,
|
||||
m_multiThreadedHelper->m_numPixelsCopied);
|
||||
m_multiThreadedHelper->getCriticalSection()->lock();
|
||||
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
|
||||
m_multiThreadedHelper->getCriticalSection()->unlock();
|
||||
m_multiThreadedHelper->mainThreadRelease();
|
||||
break;
|
||||
}
|
||||
case eGUIHelperAutogenerateGraphicsObjects:
|
||||
{
|
||||
m_multiThreadedHelper->m_childGuiHelper->autogenerateGraphicsObjects(m_multiThreadedHelper->m_dynamicsWorld);
|
||||
m_multiThreadedHelper->getCriticalSection()->lock();
|
||||
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
|
||||
m_multiThreadedHelper->getCriticalSection()->unlock();
|
||||
m_multiThreadedHelper->mainThreadRelease();
|
||||
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();
|
||||
m_multiThreadedHelper->mainThreadRelease();
|
||||
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();
|
||||
m_multiThreadedHelper->mainThreadRelease();
|
||||
break;
|
||||
}
|
||||
case eGUIUserDebugRemoveItem:
|
||||
@ -1365,9 +1415,7 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
|
||||
}
|
||||
}
|
||||
|
||||
m_multiThreadedHelper->getCriticalSection()->lock();
|
||||
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
|
||||
m_multiThreadedHelper->getCriticalSection()->unlock();
|
||||
m_multiThreadedHelper->mainThreadRelease();
|
||||
break;
|
||||
}
|
||||
case eGUIUserDebugRemoveAllItems:
|
||||
@ -1375,9 +1423,7 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
|
||||
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();
|
||||
m_multiThreadedHelper->mainThreadRelease();
|
||||
break;
|
||||
}
|
||||
case eGUIHelperIdle:
|
||||
@ -1670,9 +1716,11 @@ void PhysicsServerExample::renderScene()
|
||||
vrOffset[13]= trInv.getOrigin()[1];
|
||||
vrOffset[14]= trInv.getOrigin()[2];
|
||||
|
||||
this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->
|
||||
if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
|
||||
{
|
||||
m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->
|
||||
getActiveCamera()->setVRCameraOffsetTransform(vrOffset);
|
||||
|
||||
}
|
||||
m_physicsServer.renderScene();
|
||||
|
||||
for (int i=0;i<MAX_VR_CONTROLLERS;i++)
|
||||
@ -1939,7 +1987,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
|
||||
|
||||
}
|
||||
|
||||
m_args[0].m_cs->lock();
|
||||
m_args[0].m_csGUI->lock();
|
||||
m_args[0].m_vrControllerEvents[controllerId].m_controllerId = controllerId;
|
||||
m_args[0].m_vrControllerEvents[controllerId].m_pos[0] = trTotal.getOrigin()[0];
|
||||
m_args[0].m_vrControllerEvents[controllerId].m_pos[1] = trTotal.getOrigin()[1];
|
||||
@ -1957,7 +2005,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
|
||||
m_args[0].m_vrControllerEvents[controllerId].m_buttons[button]|=eButtonReleased;
|
||||
m_args[0].m_vrControllerEvents[controllerId].m_buttons[button] &= ~eButtonIsDown;
|
||||
}
|
||||
m_args[0].m_cs->unlock();
|
||||
m_args[0].m_csGUI->unlock();
|
||||
}
|
||||
|
||||
|
||||
@ -2009,7 +2057,7 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[
|
||||
m_args[0].m_vrControllerOrn[controllerId] = trTotal.getRotation();
|
||||
}
|
||||
|
||||
m_args[0].m_cs->lock();
|
||||
m_args[0].m_csGUI->lock();
|
||||
m_args[0].m_vrControllerEvents[controllerId].m_controllerId = controllerId;
|
||||
m_args[0].m_vrControllerEvents[controllerId].m_pos[0] = trTotal.getOrigin()[0];
|
||||
m_args[0].m_vrControllerEvents[controllerId].m_pos[1] = trTotal.getOrigin()[1];
|
||||
@ -2020,7 +2068,7 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[
|
||||
m_args[0].m_vrControllerEvents[controllerId].m_orn[3] = trTotal.getRotation()[3];
|
||||
m_args[0].m_vrControllerEvents[controllerId].m_numMoveEvents++;
|
||||
m_args[0].m_vrControllerEvents[controllerId].m_analogAxis = analogAxis;
|
||||
m_args[0].m_cs->unlock();
|
||||
m_args[0].m_csGUI->unlock();
|
||||
|
||||
}
|
||||
B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc)
|
||||
|
@ -4,18 +4,17 @@
|
||||
#include "Bullet3Common/b3Scalar.h"
|
||||
|
||||
#include <windows.h>
|
||||
|
||||
#include <stdio.h>
|
||||
//see also https://msdn.microsoft.com/en-us/library/windows/desktop/aa366551%28v=vs.85%29.aspx
|
||||
|
||||
//TCHAR szName[]=TEXT("Global\\MyFileMappingObject2");
|
||||
TCHAR szName[]=TEXT("MyFileMappingObject2");
|
||||
|
||||
struct Win32SharedMemoryInteralData
|
||||
{
|
||||
HANDLE m_hMapFile;
|
||||
|
||||
void* m_buf;
|
||||
|
||||
TCHAR m_szName[1024];
|
||||
|
||||
Win32SharedMemoryInteralData()
|
||||
:m_hMapFile(0),
|
||||
m_buf(0)
|
||||
@ -36,10 +35,18 @@ void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCre
|
||||
{
|
||||
b3Assert(m_internalData->m_buf==0);
|
||||
|
||||
#ifdef UNICODE
|
||||
swprintf_s (m_internalData->m_szName,"MyFileMappingObject%d",key);
|
||||
#else
|
||||
|
||||
sprintf(m_internalData->m_szName,"MyFileMappingObject%d",key);
|
||||
#endif
|
||||
|
||||
|
||||
m_internalData->m_hMapFile = OpenFileMapping(
|
||||
FILE_MAP_ALL_ACCESS, // read/write access
|
||||
FALSE, // do not inherit the name
|
||||
szName); // name of mapping object
|
||||
m_internalData->m_szName); // name of mapping object
|
||||
|
||||
if (m_internalData->m_hMapFile==NULL)
|
||||
{
|
||||
@ -51,10 +58,10 @@ void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCre
|
||||
PAGE_READWRITE, // read/write access
|
||||
0, // maximum object size (high-order DWORD)
|
||||
size, // maximum object size (low-order DWORD)
|
||||
szName); // name of mapping object
|
||||
m_internalData->m_szName); // name of mapping object
|
||||
} else
|
||||
{
|
||||
b3Error("Could not create file mapping object (%d).\n",GetLastError());
|
||||
b3Warning("Could not create file mapping object (%d).\n",GetLastError());
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -68,7 +75,7 @@ void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCre
|
||||
|
||||
if (m_internalData->m_buf == NULL)
|
||||
{
|
||||
b3Error("Could not map view of file (%d).\n",GetLastError());
|
||||
b3Warning("Could not map view of file (%d).\n",GetLastError());
|
||||
CloseHandle(m_internalData->m_hMapFile);
|
||||
return 0;
|
||||
}
|
||||
|
@ -72,6 +72,7 @@ int main(int argc, char* argv[])
|
||||
CommonExampleOptions options(&noGfx);
|
||||
|
||||
args.GetCmdLineArgument("shared_memory_key", gSharedMemoryKey);
|
||||
args.GetCmdLineArgument("sharedMemoryKey", gSharedMemoryKey);
|
||||
|
||||
// options.m_option |= PHYSICS_SERVER_ENABLE_COMMAND_LOGGING;
|
||||
// options.m_option |= PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG;
|
||||
|
@ -2,13 +2,16 @@
|
||||
#include <stdio.h>
|
||||
#include <enet/enet.h>
|
||||
#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
|
||||
#include "Bullet3Common/b3CommandLineArgs.h"
|
||||
|
||||
#ifdef NO_SHARED_MEMORY
|
||||
#include "PhysicsServerCommandProcessor.h"
|
||||
typedef PhysicsServerCommandProcessor MyCommandProcessor;
|
||||
#else
|
||||
#include "SharedMemoryCommandProcessor.h"
|
||||
typedef SharedMemoryCommandProcessor MyCommandProcessor ;
|
||||
typedef SharedMemoryCommandProcessor MyCommandProcessor;
|
||||
#endif //NO_SHARED_MEMORY
|
||||
|
||||
#include "SharedMemoryCommands.h"
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
#include "PhysicsServerCommandProcessor.h"
|
||||
@ -32,11 +35,28 @@ void MySerializeInt(unsigned int sz, unsigned char* output)
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
b3CommandLineArgs parseArgs(argc,argv);
|
||||
|
||||
DummyGUIHelper guiHelper;
|
||||
PhysicsCommandProcessorInterface* sm = new MyCommandProcessor;
|
||||
MyCommandProcessor* sm = new MyCommandProcessor;
|
||||
sm->setGuiHelper(&guiHelper);
|
||||
|
||||
|
||||
int port = 1234;
|
||||
if (parseArgs.GetCmdLineArgument("port",port))
|
||||
{
|
||||
printf("Using UDP port %d\n", port);
|
||||
}
|
||||
|
||||
gVerboseNetworkMessagesServer = parseArgs.CheckCmdLineFlag("verbose");
|
||||
|
||||
#ifndef NO_SHARED_MEMORY
|
||||
int key = 0;
|
||||
if (parseArgs.GetCmdLineArgument("sharedMemoryKey",key))
|
||||
{
|
||||
sm->setSharedMemoryKey(key);
|
||||
}
|
||||
#endif//NO_SHARED_MEMORY
|
||||
|
||||
// PhysicsDirect* sm = new PhysicsDirect(sdk);
|
||||
|
||||
//PhysicsClientSharedMemory* sm = new PhysicsClientSharedMemory();
|
||||
@ -65,7 +85,7 @@ int main(int argc, char *argv[])
|
||||
/* enet_address_set_host (& address, "x.x.x.x"); */
|
||||
address.host = ENET_HOST_ANY;
|
||||
/* Bind the server to port 1234. */
|
||||
address.port = 1234;
|
||||
address.port = port;
|
||||
|
||||
server = enet_host_create(&address,
|
||||
32, /* number of clients */
|
||||
|
@ -10,7 +10,7 @@
|
||||
#include "Bullet3Common/b3Transform.h"
|
||||
#include "Bullet3Common/b3CommandLineArgs.h"
|
||||
|
||||
|
||||
#include "../Utils/b3Clock.h"
|
||||
#include "../ExampleBrowser/OpenGLGuiHelper.h"
|
||||
#include "../CommonInterfaces/CommonExampleInterface.h"
|
||||
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
|
||||
@ -156,6 +156,7 @@ private:
|
||||
uint32_t m_nWindowWidth;
|
||||
uint32_t m_nWindowHeight;
|
||||
bool m_hasContext;
|
||||
b3Clock m_clock;
|
||||
|
||||
private: // OpenGL bookkeeping
|
||||
int m_iTrackedControllerCount;
|
||||
@ -1620,7 +1621,10 @@ void CMainApplication::RenderStereoTargets()
|
||||
{
|
||||
B3_PROFILE("CMainApplication::RenderStereoTargets");
|
||||
|
||||
sExample->stepSimulation(1./60.);
|
||||
btScalar dtSec = btScalar(m_clock.getTimeInSeconds());
|
||||
dtSec = b3Min(dtSec,0.1);
|
||||
sExample->stepSimulation(dtSec);
|
||||
m_clock.reset();
|
||||
|
||||
glClearColor( 0.15f, 0.15f, 0.18f, 1.0f ); // nice background color, but not black
|
||||
glEnable( GL_MULTISAMPLE );
|
||||
|
@ -99,6 +99,7 @@ int main(int argc, char* argv[])
|
||||
app->m_instancingRenderer->updateCamera(app->getUpAxis());
|
||||
|
||||
btScalar dtSec = btScalar(clock.getTimeInSeconds());
|
||||
dtSec = b3Min(dtSec,0.1);
|
||||
example->stepSimulation(dtSec);
|
||||
clock.reset();
|
||||
|
||||
|
@ -30,9 +30,31 @@ enum eCONNECT_METHOD {
|
||||
static PyObject* SpamError;
|
||||
|
||||
#define MAX_PHYSICS_CLIENTS 1024
|
||||
static b3PhysicsClientHandle sPhysicsClients[MAX_PHYSICS_CLIENTS] = {0};
|
||||
static b3PhysicsClientHandle sPhysicsClients1[MAX_PHYSICS_CLIENTS] = {0};
|
||||
static int sNumPhysicsClients=0;
|
||||
|
||||
b3PhysicsClientHandle getPhysicsClient(int physicsClientId)
|
||||
{
|
||||
b3PhysicsClientHandle sm;
|
||||
if ((physicsClientId <0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients1[physicsClientId]))
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
sm = sPhysicsClients1[physicsClientId];
|
||||
if (sm)
|
||||
{
|
||||
if (b3CanSubmitCommand(sm))
|
||||
{
|
||||
return sm;
|
||||
}
|
||||
//broken connection?
|
||||
b3DisconnectSharedMemory(sm);
|
||||
sPhysicsClients1[physicsClientId] = 0;
|
||||
sNumPhysicsClients--;
|
||||
}
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) {
|
||||
double v = 0.0;
|
||||
@ -161,13 +183,12 @@ static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args, PyObjec
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
{
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
@ -281,22 +302,23 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
for (i=0;i<MAX_PHYSICS_CLIENTS;i++)
|
||||
if (sm && b3CanSubmitCommand(sm))
|
||||
{
|
||||
if (sPhysicsClients[i]==0)
|
||||
for (i=0;i<MAX_PHYSICS_CLIENTS;i++)
|
||||
{
|
||||
freeIndex = i;
|
||||
break;
|
||||
if (sPhysicsClients1[i]==0)
|
||||
{
|
||||
freeIndex = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (freeIndex>=0)
|
||||
{
|
||||
sPhysicsClients1[freeIndex] = sm;
|
||||
sNumPhysicsClients++;
|
||||
}
|
||||
}
|
||||
|
||||
if (freeIndex>=0)
|
||||
{
|
||||
sPhysicsClients[freeIndex] = sm;
|
||||
sNumPhysicsClients++;
|
||||
}
|
||||
|
||||
return PyInt_FromLong(freeIndex);
|
||||
}
|
||||
|
||||
@ -311,20 +333,20 @@ static PyObject* pybullet_disconnectPhysicsServer(PyObject* self,
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
|
||||
{
|
||||
b3DisconnectSharedMemory(sm);
|
||||
sm = 0;
|
||||
}
|
||||
|
||||
sPhysicsClients[physicsClientId] = 0;
|
||||
sPhysicsClients1[physicsClientId] = 0;
|
||||
sNumPhysicsClients--;
|
||||
|
||||
Py_INCREF(Py_None);
|
||||
@ -342,17 +364,12 @@ static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args,PyObject *key
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
if (0==sm)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
@ -397,12 +414,12 @@ static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args,PyObject *ke
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
command = b3LoadBulletCommandInit(sm, bulletFileName);
|
||||
@ -448,12 +465,12 @@ static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args, PyObject* k
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
command = b3SaveBulletCommandInit(sm, bulletFileName);
|
||||
@ -485,12 +502,12 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
command = b3LoadMJCFCommandInit(sm, mjcfFileName);
|
||||
@ -522,12 +539,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
{
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
|
||||
@ -669,13 +686,12 @@ b3PhysicsClientHandle sm=0;
|
||||
}
|
||||
}
|
||||
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
if (strlen(urdfFileName)) {
|
||||
// printf("(%f, %f, %f) (%f, %f, %f, %f)\n",
|
||||
@ -736,12 +752,12 @@ b3PhysicsClientHandle sm = 0;
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
commandHandle = b3LoadSdfCommandInit(sm, sdfFileName);
|
||||
@ -780,12 +796,13 @@ static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObje
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
{
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(
|
||||
@ -807,12 +824,13 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) {
|
||||
double kd = 1.0;
|
||||
int valid = 0;
|
||||
int physicsClientId = 0;
|
||||
b3PhysicsClientHandle sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
if (0 == sm) {
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
b3PhysicsClientHandle sm;
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
size = PySequence_Size(args);
|
||||
if (size == 4) {
|
||||
@ -985,12 +1003,12 @@ b3PhysicsClientHandle sm = 0;
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
{
|
||||
int numJoints;
|
||||
@ -1066,12 +1084,12 @@ static PyObject* pybullet_setRealTimeSimulation(PyObject* self,
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
{
|
||||
@ -1106,12 +1124,12 @@ b3PhysicsClientHandle sm = 0;
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
{
|
||||
@ -1146,12 +1164,12 @@ static PyObject* pybullet_setGravity(PyObject* self, PyObject* args, PyObject* k
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
|
||||
@ -1172,18 +1190,20 @@ static PyObject* pybullet_setTimeStep(PyObject* self, PyObject* args, PyObject*
|
||||
double timeStep = 0.001;
|
||||
int ret;
|
||||
int physicsClientId = 0;
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
|
||||
static char *kwlist[] = { "timeStep", "physicsClientId", NULL };
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist,&timeStep, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
{
|
||||
b3PhysicsClientHandle sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
@ -1202,18 +1222,20 @@ pybullet_setDefaultContactERP(PyObject* self, PyObject* args,PyObject* keywds)
|
||||
{
|
||||
double defaultContactERP=0.005;
|
||||
int physicsClientId = 0;
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
|
||||
static char *kwlist[] = { "defaultContactERP", "physicsClientId", NULL };
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist,&defaultContactERP, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
{
|
||||
b3PhysicsClientHandle sm = sPhysicsClients[physicsClientId];
|
||||
int ret;
|
||||
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
@ -1366,12 +1388,12 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self,
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
if (0 == pybullet_internalGetBasePositionAndOrientation(
|
||||
bodyUniqueId, basePosition, baseOrientation,sm)) {
|
||||
@ -1427,12 +1449,12 @@ static PyObject* pybullet_getBaseVelocity(PyObject* self,
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
if (0 == pybullet_internalGetBaseVelocity(
|
||||
bodyUniqueId, baseLinearVelocity, baseAngularVelocity,sm)) {
|
||||
@ -1481,12 +1503,12 @@ static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args, PyObject*
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
{
|
||||
@ -1511,12 +1533,12 @@ b3PhysicsClientHandle sm = 0;
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
{
|
||||
@ -1546,12 +1568,12 @@ static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args, PyObject*
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
{
|
||||
struct b3BodyInfo info;
|
||||
@ -1587,12 +1609,12 @@ static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args, PyObject*
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
numJoints = b3GetNumJoints(sm, bodyUniqueId);
|
||||
|
||||
@ -1619,12 +1641,12 @@ static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args, PyObje
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
{
|
||||
@ -1674,12 +1696,12 @@ static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyOb
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
if (linVelObj || angVelObj)
|
||||
@ -1737,12 +1759,12 @@ static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self,
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
{
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
@ -1828,12 +1850,12 @@ int physicsClientId = 0;
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
{
|
||||
@ -1909,12 +1931,12 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args,PyObject*
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
{
|
||||
@ -1994,12 +2016,12 @@ b3PhysicsClientHandle sm = 0;
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
{
|
||||
{
|
||||
@ -2097,12 +2119,12 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
res = pybullet_internalSetVectord(textPositionObj,posXYZ);
|
||||
@ -2168,13 +2190,12 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
res = pybullet_internalSetVectord(lineFromObj,fromXYZ);
|
||||
if (!res)
|
||||
@ -2228,13 +2249,12 @@ static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, Py
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
commandHandle = b3InitUserDebugDrawRemove(sm,itemUniqueId);
|
||||
|
||||
@ -2258,12 +2278,12 @@ static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
commandHandle = b3InitUserDebugDrawRemoveAll(sm);
|
||||
@ -2294,12 +2314,12 @@ static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject *keyw
|
||||
&rayFromObj, &rayToObj,&physicsClientId))
|
||||
return NULL;
|
||||
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
pybullet_internalSetVectord(rayFromObj,from);
|
||||
pybullet_internalSetVectord(rayToObj,to);
|
||||
@ -2407,13 +2427,12 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject *
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
|
||||
commandHandle = b3RequestVREventsCommandInit(sm);
|
||||
@ -2488,12 +2507,12 @@ static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, Py
|
||||
&objectUniqueId, &linkIndex, &objectColorRGBObj,&physicsClientId))
|
||||
return NULL;
|
||||
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
if (objectColorRGBObj)
|
||||
{
|
||||
@ -2531,12 +2550,12 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
{
|
||||
@ -2634,13 +2653,12 @@ static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args,Py
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
|
||||
{
|
||||
@ -2677,12 +2695,12 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args, PyObject*
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
{
|
||||
|
||||
@ -2823,12 +2841,12 @@ static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args,
|
||||
&aabbMinOb, &aabbMaxOb,&physicsClientId))
|
||||
return NULL;
|
||||
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
pybullet_internalSetVectord(aabbMinOb, aabbMin);
|
||||
@ -2890,12 +2908,12 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
|
||||
&bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB,&physicsClientId))
|
||||
return NULL;
|
||||
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
commandHandle = b3InitClosestDistanceQuery(sm);
|
||||
b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA);
|
||||
@ -2939,13 +2957,12 @@ static PyObject* pybullet_removeUserConstraint(PyObject* self, PyObject* args, P
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
commandHandle = b3InitRemoveUserConstraintCommand(sm,userConstraintUniqueId);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
@ -3017,13 +3034,12 @@ static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, P
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
pybullet_internalSetVectord(jointAxisObj,jointAxis);
|
||||
pybullet_internalSetVectord(parentFramePositionObj,parentFramePosition);
|
||||
@ -3091,14 +3107,12 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py
|
||||
&bodyUniqueIdA, &bodyUniqueIdB,&physicsClientId))
|
||||
return NULL;
|
||||
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
|
||||
commandHandle = b3InitRequestContactPointInformation(sm);
|
||||
b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA);
|
||||
@ -3150,12 +3164,12 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
command = b3InitRequestCameraImage(sm);
|
||||
b3RequestCameraImageSetPixelResolution(command, width, height);
|
||||
@ -3478,21 +3492,15 @@ static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) {
|
||||
|
||||
// inialize cmd
|
||||
b3SharedMemoryCommandHandle command;
|
||||
b3PhysicsClientHandle sm;
|
||||
int physicsClientId = 0;
|
||||
b3PhysicsClientHandle sm=0;
|
||||
if (0 == sPhysicsClients[physicsClientId])
|
||||
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
|
||||
if (0 == sm) {
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
command = b3InitRequestCameraImage(sm);
|
||||
|
||||
if (size == 2) // only set camera resolution
|
||||
@ -3725,12 +3733,12 @@ static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args,PyOb
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
{
|
||||
PyObject* seq;
|
||||
int len, i;
|
||||
@ -3793,12 +3801,12 @@ static PyObject* pybullet_applyExternalTorque(PyObject* self, PyObject* args,PyO
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
{
|
||||
PyObject* seq;
|
||||
@ -3982,12 +3990,12 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
{
|
||||
double pos[3];
|
||||
double ori[4]={0,1.0,0,0};
|
||||
@ -4067,12 +4075,12 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self,
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId]))
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
sm = sPhysicsClients[physicsClientId];
|
||||
|
||||
{
|
||||
int szObPos = PySequence_Size(objPositionsQ);
|
||||
|
@ -2,11 +2,15 @@ import pybullet as p
|
||||
import time
|
||||
import math
|
||||
|
||||
p.connect(p.SHARED_MEMORY)
|
||||
|
||||
physId = p.connect(p.SHARED_MEMORY)
|
||||
if (physId<0):
|
||||
p.connect(p.GUI)
|
||||
|
||||
p.loadURDF("plane.urdf")
|
||||
p.setGravity(0,0,-1)
|
||||
p.setRealTimeSimulation(0)
|
||||
quadruped = p.loadURDF("quadruped/quadruped.urdf",10,-2,2)
|
||||
quadruped = p.loadURDF("quadruped/quadruped.urdf",1,-2,2)
|
||||
#p.getNumJoints(1)
|
||||
#right front leg
|
||||
p.resetJointState(quadruped,0,1.57)
|
||||
@ -82,6 +86,7 @@ jump_amp = 0.5
|
||||
#jump
|
||||
t_end = time.time() + 10
|
||||
i=0
|
||||
t=0
|
||||
while time.time() < t_end:
|
||||
t = time.time()
|
||||
p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain)
|
||||
|
Loading…
Reference in New Issue
Block a user