Update to upstream

This commit is contained in:
Bart Moyaers
2019-06-20 10:13:23 +02:00
18 changed files with 304 additions and 115 deletions

View File

@@ -8,7 +8,7 @@ class RigidBodyBoxes : public CommonRigidBodyBase
{ {
int m_option; int m_option;
int m_numIterations; int m_numIterations;
int m_numBoxes = 4; int m_numBoxes;
btAlignedObjectArray<btRigidBody*> boxes; btAlignedObjectArray<btRigidBody*> boxes;
static btScalar numSolverIterations; static btScalar numSolverIterations;
@@ -38,7 +38,8 @@ btScalar RigidBodyBoxes::numSolverIterations = 50;
RigidBodyBoxes::RigidBodyBoxes(GUIHelperInterface* helper, int option) RigidBodyBoxes::RigidBodyBoxes(GUIHelperInterface* helper, int option)
: CommonRigidBodyBase(helper), : CommonRigidBodyBase(helper),
m_option(option), m_option(option),
m_numIterations(numSolverIterations) m_numIterations(numSolverIterations),
m_numBoxes(4)
{ {
m_guiHelper->setUpAxis(2); m_guiHelper->setUpAxis(2);
} }

View File

@@ -49,6 +49,9 @@ struct CommonRenderInterface
virtual void setLightPosition(const float lightPos[3]) = 0; virtual void setLightPosition(const float lightPos[3]) = 0;
virtual void setLightPosition(const double lightPos[3]) = 0; virtual void setLightPosition(const double lightPos[3]) = 0;
virtual void setShadowMapResolution(int shadowMapResolution) = 0;
virtual void setShadowMapWorldSize(float worldSize) = 0;
virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]){}; virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]){};
virtual void setProjectiveTexture(bool useProjectiveTexture){}; virtual void setProjectiveTexture(bool useProjectiveTexture){};

View File

@@ -257,7 +257,11 @@ void MyKeyboardCallback(int key, int state)
} }
else else
{ {
#ifdef _WIN32
b3ChromeUtilsStopTimingsAndWriteJsonFile("timings"); b3ChromeUtilsStopTimingsAndWriteJsonFile("timings");
#else
b3ChromeUtilsStopTimingsAndWriteJsonFile("/tmp/timings");
#endif
} }
} }

View File

@@ -27,7 +27,7 @@ static bool parseVector4(btVector4& vec4, const std::string& vector_str)
{ {
vec4.setZero(); vec4.setZero();
btArray<std::string> pieces; btArray<std::string> pieces;
btArray<float> rgba; btArray<double> rgba;
btAlignedObjectArray<std::string> strArray; btAlignedObjectArray<std::string> strArray;
urdfIsAnyOf(" ", strArray); urdfIsAnyOf(" ", strArray);
urdfStringSplit(pieces, vector_str, strArray); urdfStringSplit(pieces, vector_str, strArray);
@@ -50,7 +50,7 @@ static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLo
{ {
vec3.setZero(); vec3.setZero();
btArray<std::string> pieces; btArray<std::string> pieces;
btArray<float> rgba; btArray<double> rgba;
btAlignedObjectArray<std::string> strArray; btAlignedObjectArray<std::string> strArray;
urdfIsAnyOf(" ", strArray); urdfIsAnyOf(" ", strArray);
urdfStringSplit(pieces, vector_str, strArray); urdfStringSplit(pieces, vector_str, strArray);

View File

@@ -16,9 +16,8 @@ subject to the following restrictions:
///todo: make this configurable in the gui ///todo: make this configurable in the gui
bool useShadowMap = true; // true;//false;//true; bool useShadowMap = true; // true;//false;//true;
int shadowMapWidth = 4096;
int shadowMapHeight = 4096;
float shadowMapWorldSize = 10;
#include <stdio.h> #include <stdio.h>
struct caster2 struct caster2
@@ -244,11 +243,22 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
b3ResizablePool<b3PublicGraphicsInstance> m_publicGraphicsInstances; b3ResizablePool<b3PublicGraphicsInstance> m_publicGraphicsInstances;
int m_shadowMapWidth;
int m_shadowMapHeight;
float m_shadowMapWorldSize;
bool m_updateShadowMap;
InternalDataRenderer() : m_activeCamera(&m_defaultCamera1), InternalDataRenderer() : m_activeCamera(&m_defaultCamera1),
m_shadowMap(0), m_shadowMap(0),
m_shadowTexture(0), m_shadowTexture(0),
m_renderFrameBuffer(0) m_renderFrameBuffer(0),
m_shadowMapWidth(4096),
m_shadowMapHeight(4096),
m_shadowMapWorldSize(10),
m_updateShadowMap(true)
{ {
m_lightPos = b3MakeVector3(-50, 30, 40); m_lightPos = b3MakeVector3(-50, 30, 40);
m_lightSpecularIntensity.setValue(1, 1, 1); m_lightSpecularIntensity.setValue(1, 1, 1);
@@ -1449,6 +1459,19 @@ void GLInstancingRenderer::setLightPosition(const float lightPos[3])
m_data->m_lightPos[2] = lightPos[2]; m_data->m_lightPos[2] = lightPos[2];
} }
void GLInstancingRenderer::setShadowMapResolution(int shadowMapResolution)
{
m_data->m_shadowMapWidth = shadowMapResolution;
m_data->m_shadowMapHeight = shadowMapResolution;
m_data->m_updateShadowMap = true;
}
void GLInstancingRenderer::setShadowMapWorldSize(float worldSize)
{
m_data->m_shadowMapWorldSize = worldSize;
m_data->m_updateShadowMap = true;
}
void GLInstancingRenderer::setLightPosition(const double lightPos[3]) void GLInstancingRenderer::setLightPosition(const double lightPos[3])
{ {
m_data->m_lightPos[0] = lightPos[0]; m_data->m_lightPos[0] = lightPos[0];
@@ -2071,6 +2094,13 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
glEnable(GL_CULL_FACE); glEnable(GL_CULL_FACE);
glCullFace(GL_FRONT); glCullFace(GL_FRONT);
if (m_data->m_shadowMap && m_data->m_updateShadowMap)
{
m_data->m_updateShadowMap = false;
glDeleteTextures(1, &m_data->m_shadowTexture);
delete m_data->m_shadowMap;
m_data->m_shadowMap = 0;
}
if (!m_data->m_shadowMap) if (!m_data->m_shadowMap)
{ {
glActiveTexture(GL_TEXTURE0); glActiveTexture(GL_TEXTURE0);
@@ -2088,27 +2118,27 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
int size; int size;
glGetIntegerv(GL_MAX_TEXTURE_SIZE, &size); glGetIntegerv(GL_MAX_TEXTURE_SIZE, &size);
if (size < shadowMapWidth) if (size < m_data->m_shadowMapWidth)
{ {
shadowMapWidth = size; m_data->m_shadowMapWidth = size;
} }
if (size < shadowMapHeight) if (size < m_data->m_shadowMapHeight)
{ {
shadowMapHeight = size; m_data->m_shadowMapHeight = size;
} }
GLuint err; GLuint err;
do do
{ {
glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT16, glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT16,
shadowMapWidth, shadowMapHeight, m_data->m_shadowMapWidth, m_data->m_shadowMapHeight,
0, GL_DEPTH_COMPONENT, GL_FLOAT, 0); 0, GL_DEPTH_COMPONENT, GL_FLOAT, 0);
err = glGetError(); err = glGetError();
if (err != GL_NO_ERROR) if (err != GL_NO_ERROR)
{ {
shadowMapHeight >>= 1; m_data->m_shadowMapHeight >>= 1;
shadowMapWidth >>= 1; m_data->m_shadowMapWidth >>= 1;
} }
} while (err != GL_NO_ERROR && shadowMapWidth > 0); } while (err != GL_NO_ERROR && m_data->m_shadowMapWidth > 0);
#endif //OLD_SHADOWMAP_INIT #endif //OLD_SHADOWMAP_INIT
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
@@ -2124,10 +2154,10 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_COMPARE_REF_TO_TEXTURE); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_COMPARE_REF_TO_TEXTURE);
m_data->m_shadowMap = new GLRenderToTexture(); m_data->m_shadowMap = new GLRenderToTexture();
m_data->m_shadowMap->init(shadowMapWidth, shadowMapHeight, m_data->m_shadowTexture, RENDERTEXTURE_DEPTH); m_data->m_shadowMap->init(m_data->m_shadowMapWidth, m_data->m_shadowMapHeight, m_data->m_shadowTexture, RENDERTEXTURE_DEPTH);
} }
m_data->m_shadowMap->enable(); m_data->m_shadowMap->enable();
glViewport(0, 0, shadowMapWidth, shadowMapHeight); glViewport(0, 0, m_data->m_shadowMapWidth, m_data->m_shadowMapHeight);
//glClearColor(1,1,1,1); //glClearColor(1,1,1,1);
glClear(GL_DEPTH_BUFFER_BIT); glClear(GL_DEPTH_BUFFER_BIT);
//glClearColor(0.3,0.3,0.3,1); //glClearColor(0.3,0.3,0.3,1);
@@ -2145,7 +2175,7 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
glCullFace(GL_BACK); glCullFace(GL_BACK);
} }
b3CreateOrtho(-shadowMapWorldSize, shadowMapWorldSize, -shadowMapWorldSize, shadowMapWorldSize, 1, 300, depthProjectionMatrix); //-14,14,-14,14,1,200, depthProjectionMatrix); b3CreateOrtho(-m_data->m_shadowMapWorldSize, m_data->m_shadowMapWorldSize, -m_data->m_shadowMapWorldSize, m_data->m_shadowMapWorldSize, 1, 300, depthProjectionMatrix); //-14,14,-14,14,1,200, depthProjectionMatrix);
float depthViewMatrix[4][4]; float depthViewMatrix[4][4];
b3Vector3 center = b3MakeVector3(0, 0, 0); b3Vector3 center = b3MakeVector3(0, 0, 0);
m_data->m_activeCamera->getCameraTargetPosition(center); m_data->m_activeCamera->getCameraTargetPosition(center);

View File

@@ -120,6 +120,8 @@ public:
virtual void setLightPosition(const float lightPos[3]); virtual void setLightPosition(const float lightPos[3]);
virtual void setLightPosition(const double lightPos[3]); virtual void setLightPosition(const double lightPos[3]);
virtual void setShadowMapResolution(int shadowMapResolution);
virtual void setShadowMapWorldSize(float worldSize);
void setLightSpecularIntensity(const float lightSpecularIntensity[3]); void setLightSpecularIntensity(const float lightSpecularIntensity[3]);
virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]); virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]);
virtual void setProjectiveTexture(bool useProjectiveTexture); virtual void setProjectiveTexture(bool useProjectiveTexture);

View File

@@ -44,7 +44,7 @@ GLuint gltLoadShaderPair(const char *szVertexProg, const char *szFragmentProg)
return 0; return 0;
glDeleteShader(hVertexShader); glDeleteShader(hVertexShader);
glDeleteShader(hFragmentShader); glDeleteShader(hFragmentShader);
return (GLuint)NULL; return (GLuint)0;
} }
assert(glGetError() == GL_NO_ERROR); assert(glGetError() == GL_NO_ERROR);
@@ -62,7 +62,7 @@ GLuint gltLoadShaderPair(const char *szVertexProg, const char *szFragmentProg)
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
glDeleteShader(hVertexShader); glDeleteShader(hVertexShader);
glDeleteShader(hFragmentShader); glDeleteShader(hFragmentShader);
return (GLuint)NULL; return (GLuint)0;
} }
assert(glGetError() == GL_NO_ERROR); assert(glGetError() == GL_NO_ERROR);
@@ -96,7 +96,7 @@ GLuint gltLoadShaderPair(const char *szVertexProg, const char *szFragmentProg)
printf("Warning/Error in GLSL shader:\n"); printf("Warning/Error in GLSL shader:\n");
printf("%s\n", infoLog); printf("%s\n", infoLog);
glDeleteProgram(hReturn); glDeleteProgram(hReturn);
return (GLuint)NULL; return (GLuint)0;
} }
return hReturn; return hReturn;

View File

@@ -26,7 +26,8 @@ public:
virtual void setLightPosition(const float lightPos[3]); virtual void setLightPosition(const float lightPos[3]);
virtual void setLightPosition(const double lightPos[3]); virtual void setLightPosition(const double lightPos[3]);
virtual void setShadowMapResolution(int shadowMapResolution) {}
virtual void setShadowMapWorldSize(float worldSize) {}
virtual void resize(int width, int height); virtual void resize(int width, int height);
virtual void removeAllInstances(); virtual void removeAllInstances();

View File

@@ -10,6 +10,7 @@
#include "Bullet3Common/b3AlignedObjectArray.h" #include "Bullet3Common/b3AlignedObjectArray.h"
#include "GraphicsSharedMemoryBlock.h" #include "GraphicsSharedMemoryBlock.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "SharedMemoryPublic.h"
#define MAX_GRAPHICS_SHARED_MEMORY_BLOCKS 1 #define MAX_GRAPHICS_SHARED_MEMORY_BLOCKS 1
@@ -107,12 +108,16 @@ public:
break; break;
} }
case GFX_CMD_SET_VISUALIZER_FLAG: case GFX_CMD_SET_VISUALIZER_FLAG:
{
if (clientCmd.m_visualizerFlagCommand.m_visualizerFlag!=COV_ENABLE_RENDERING)
{ {
//printf("clientCmd.m_visualizerFlag.m_visualizerFlag: %d, clientCmd.m_visualizerFlag.m_enable %d\n", //printf("clientCmd.m_visualizerFlag.m_visualizerFlag: %d, clientCmd.m_visualizerFlag.m_enable %d\n",
// clientCmd.m_visualizerFlagCommand.m_visualizerFlag, clientCmd.m_visualizerFlagCommand.m_enable); // clientCmd.m_visualizerFlagCommand.m_visualizerFlag, clientCmd.m_visualizerFlagCommand.m_enable);
this->m_guiHelper->setVisualizerFlag(clientCmd.m_visualizerFlagCommand.m_visualizerFlag, clientCmd.m_visualizerFlagCommand.m_enable); this->m_guiHelper->setVisualizerFlag(clientCmd.m_visualizerFlagCommand.m_visualizerFlag, clientCmd.m_visualizerFlagCommand.m_enable);
}
break; break;
} }
case GFX_CMD_UPLOAD_DATA: case GFX_CMD_UPLOAD_DATA:

View File

@@ -26,7 +26,7 @@ typedef unsigned long long int smUint64a_t;
#ifdef __APPLE__ #ifdef __APPLE__
#define GRAPHICS_SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (512 * 1024) #define GRAPHICS_SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (512 * 1024)
#else #else
#define GRAPHICS_SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (4 * 1024 * 1024) #define GRAPHICS_SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (4 * 1024 * 1024)
#endif #endif
struct GraphicsCommand0 struct GraphicsCommand0

View File

@@ -5462,6 +5462,58 @@ B3_SHARED_API void b3ConfigureOpenGLVisualizerSetVisualizationFlags(b3SharedMemo
} }
} }
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetLightPosition(b3SharedMemoryCommandHandle commandHandle, const float lightPosition[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER);
if (command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER)
{
command->m_updateFlags |= COV_SET_LIGHT_POSITION;
command->m_configureOpenGLVisualizerArguments.m_lightPosition[0] = lightPosition[0];
command->m_configureOpenGLVisualizerArguments.m_lightPosition[1] = lightPosition[1];
command->m_configureOpenGLVisualizerArguments.m_lightPosition[2] = lightPosition[2];
}
}
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetShadowMapResolution(b3SharedMemoryCommandHandle commandHandle, int shadowMapResolution)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER);
if (command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER)
{
command->m_updateFlags |= COV_SET_SHADOWMAP_RESOLUTION;
command->m_configureOpenGLVisualizerArguments.m_shadowMapResolution = shadowMapResolution;
}
}
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetShadowMapWorldSize(b3SharedMemoryCommandHandle commandHandle, int shadowMapWorldSize)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER);
if (command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER)
{
command->m_updateFlags |= COV_SET_SHADOWMAP_WORLD_SIZE;
command->m_configureOpenGLVisualizerArguments.m_shadowMapWorldSize = shadowMapWorldSize;
}
}
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetRemoteSyncTransformInterval(b3SharedMemoryCommandHandle commandHandle, double remoteSyncTransformInterval)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER);
if (command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER)
{
command->m_updateFlags |= COV_SET_REMOTE_SYNC_TRANSFORM_INTERVAL;
command->m_configureOpenGLVisualizerArguments.m_remoteSyncTransformInterval = remoteSyncTransformInterval;
}
}
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, const float cameraTargetPosition[3]) B3_SHARED_API void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, const float cameraTargetPosition[3])
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;

View File

@@ -207,6 +207,11 @@ extern "C"
B3_SHARED_API b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer(b3PhysicsClientHandle physClient); B3_SHARED_API b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer2(b3SharedMemoryCommandHandle commandHandle); B3_SHARED_API b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer2(b3SharedMemoryCommandHandle commandHandle);
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetVisualizationFlags(b3SharedMemoryCommandHandle commandHandle, int flag, int enabled); B3_SHARED_API void b3ConfigureOpenGLVisualizerSetVisualizationFlags(b3SharedMemoryCommandHandle commandHandle, int flag, int enabled);
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetLightPosition(b3SharedMemoryCommandHandle commandHandle, const float lightPosition[3]);
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetShadowMapResolution(b3SharedMemoryCommandHandle commandHandle, int shadowMapResolution);
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetShadowMapWorldSize(b3SharedMemoryCommandHandle commandHandle, int shadowMapWorldSize);
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetRemoteSyncTransformInterval(b3SharedMemoryCommandHandle commandHandle, double remoteSyncTransformInterval);
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, const float cameraTargetPosition[/*3*/]); B3_SHARED_API void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, const float cameraTargetPosition[/*3*/]);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestOpenGLVisualizerCameraCommand(b3PhysicsClientHandle physClient); B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestOpenGLVisualizerCameraCommand(b3PhysicsClientHandle physClient);

View File

@@ -1672,6 +1672,9 @@ struct PhysicsServerCommandProcessorInternalData
b3ThreadPool* m_threadPool; b3ThreadPool* m_threadPool;
btScalar m_defaultCollisionMargin; btScalar m_defaultCollisionMargin;
double m_remoteSyncTransformTime;
double m_remoteSyncTransformInterval;
PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc) PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc)
: m_pluginManager(proc), : m_pluginManager(proc),
m_useRealTimeSimulation(false), m_useRealTimeSimulation(false),
@@ -1705,12 +1708,14 @@ struct PhysicsServerCommandProcessorInternalData
m_collisionFilterPlugin(-1), m_collisionFilterPlugin(-1),
m_grpcPlugin(-1), m_grpcPlugin(-1),
m_threadPool(0), m_threadPool(0),
m_defaultCollisionMargin(0.001) m_defaultCollisionMargin(0.001),
m_remoteSyncTransformTime(1. / 30.),
m_remoteSyncTransformInterval(1. / 30.)
{ {
{ {
//register static plugins: //register static plugins:
#ifdef STATIC_LINK_VR_PLUGIN #ifdef STATIC_LINK_VR_PLUGIN
b3PluginFunctions funcs(initPlugin_vrSyncPlugin,exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin); b3PluginFunctions funcs(initPlugin_vrSyncPlugin, exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin);
funcs.m_preTickFunc = preTickPluginCallback_vrSyncPlugin; funcs.m_preTickFunc = preTickPluginCallback_vrSyncPlugin;
m_pluginManager.registerStaticLinkedPlugin("vrSyncPlugin", funcs); m_pluginManager.registerStaticLinkedPlugin("vrSyncPlugin", funcs);
#endif //STATIC_LINK_VR_PLUGIN #endif //STATIC_LINK_VR_PLUGIN
@@ -1718,7 +1723,7 @@ struct PhysicsServerCommandProcessorInternalData
#ifndef SKIP_STATIC_PD_CONTROL_PLUGIN #ifndef SKIP_STATIC_PD_CONTROL_PLUGIN
{ {
//int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, PFN_GET_COLLISION_INTERFACE getCollisionFunc, bool initPlugin) //int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, PFN_GET_COLLISION_INTERFACE getCollisionFunc, bool initPlugin)
b3PluginFunctions funcs(initPlugin_pdControlPlugin,exitPlugin_pdControlPlugin,executePluginCommand_pdControlPlugin); b3PluginFunctions funcs(initPlugin_pdControlPlugin, exitPlugin_pdControlPlugin, executePluginCommand_pdControlPlugin);
funcs.m_preTickFunc = preTickPluginCallback_pdControlPlugin; funcs.m_preTickFunc = preTickPluginCallback_pdControlPlugin;
m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", funcs); m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", funcs);
} }
@@ -1726,9 +1731,9 @@ struct PhysicsServerCommandProcessorInternalData
#ifndef SKIP_COLLISION_FILTER_PLUGIN #ifndef SKIP_COLLISION_FILTER_PLUGIN
{ {
b3PluginFunctions funcs(initPlugin_collisionFilterPlugin,exitPlugin_collisionFilterPlugin, executePluginCommand_collisionFilterPlugin); b3PluginFunctions funcs(initPlugin_collisionFilterPlugin, exitPlugin_collisionFilterPlugin, executePluginCommand_collisionFilterPlugin);
funcs.m_getCollisionFunc = getCollisionInterface_collisionFilterPlugin; funcs.m_getCollisionFunc = getCollisionInterface_collisionFilterPlugin;
m_collisionFilterPlugin = m_pluginManager.registerStaticLinkedPlugin("collisionFilterPlugin", funcs ); m_collisionFilterPlugin = m_pluginManager.registerStaticLinkedPlugin("collisionFilterPlugin", funcs);
m_pluginManager.selectCollisionPlugin(m_collisionFilterPlugin); m_pluginManager.selectCollisionPlugin(m_collisionFilterPlugin);
} }
#endif #endif
@@ -1754,7 +1759,7 @@ struct PhysicsServerCommandProcessorInternalData
#ifndef SKIP_STATIC_TINYRENDERER_PLUGIN #ifndef SKIP_STATIC_TINYRENDERER_PLUGIN
{ {
b3PluginFunctions funcs(initPlugin_tinyRendererPlugin, exitPlugin_tinyRendererPlugin, executePluginCommand_tinyRendererPlugin); b3PluginFunctions funcs(initPlugin_tinyRendererPlugin, exitPlugin_tinyRendererPlugin, executePluginCommand_tinyRendererPlugin);
funcs.m_getRendererFunc=getRenderInterface_tinyRendererPlugin; funcs.m_getRendererFunc = getRenderInterface_tinyRendererPlugin;
int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("tinyRendererPlugin", funcs); int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("tinyRendererPlugin", funcs);
m_pluginManager.selectPluginRenderer(renderPluginId); m_pluginManager.selectPluginRenderer(renderPluginId);
} }
@@ -1769,8 +1774,6 @@ struct PhysicsServerCommandProcessorInternalData
} }
#endif #endif
m_vrControllerEvents.init(); m_vrControllerEvents.init();
m_bodyHandles.exitHandles(); m_bodyHandles.exitHandles();
@@ -1868,7 +1871,6 @@ struct PhysicsServerCommandProcessorInternalData
int dof = 7; int dof = 7;
int veldof = 7; int veldof = 7;
for (int l = 0; l < multiBody->getNumLinks(); l++) for (int l = 0; l < multiBody->getNumLinks(); l++)
{ {
switch (multiBody->getLink(l).m_jointType) switch (multiBody->getLink(l).m_jointType)
@@ -1910,7 +1912,7 @@ struct PhysicsServerCommandProcessorInternalData
} }
btVector3 gravOrg = m_dynamicsWorld->getGravity(); btVector3 gravOrg = m_dynamicsWorld->getGravity();
tVector grav (gravOrg[0], gravOrg[1], gravOrg[2], 0); tVector grav(gravOrg[0], gravOrg[1], gravOrg[2], 0);
rbdModel->SetGravity(grav); rbdModel->SetGravity(grav);
rbdModel->Update(pose, vel); rbdModel->Update(pose, vel);
@@ -7763,7 +7765,13 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
} }
serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
m_data->m_remoteSyncTransformTime += deltaTimeScaled;
if (m_data->m_remoteSyncTransformTime >= m_data->m_remoteSyncTransformInterval)
{
m_data->m_remoteSyncTransformTime = 0;
syncPhysicsToGraphics2(); syncPhysicsToGraphics2();
}
return hasStatus; return hasStatus;
} }
@@ -9058,6 +9066,29 @@ bool PhysicsServerCommandProcessor::processConfigureOpenGLVisualizerCommand(cons
clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[1], clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[1],
clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[2]); clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[2]);
} }
if (m_data->m_guiHelper->getRenderInterface())
{
if (clientCmd.m_updateFlags & COV_SET_LIGHT_POSITION)
{
m_data->m_guiHelper->getRenderInterface()->setLightPosition(clientCmd.m_configureOpenGLVisualizerArguments.m_lightPosition);
}
if (clientCmd.m_updateFlags & COV_SET_SHADOWMAP_RESOLUTION)
{
m_data->m_guiHelper->getRenderInterface()->setShadowMapResolution(clientCmd.m_configureOpenGLVisualizerArguments.m_shadowMapResolution);
}
if (clientCmd.m_updateFlags & COV_SET_SHADOWMAP_WORLD_SIZE)
{
float worldSize = clientCmd.m_configureOpenGLVisualizerArguments.m_shadowMapWorldSize;
m_data->m_guiHelper->getRenderInterface()->setShadowMapWorldSize(worldSize);
}
}
if (clientCmd.m_updateFlags & COV_SET_REMOTE_SYNC_TRANSFORM_INTERVAL)
{
m_data->m_remoteSyncTransformInterval = clientCmd.m_configureOpenGLVisualizerArguments.m_remoteSyncTransformInterval;
}
return hasStatus; return hasStatus;
} }
@@ -12031,6 +12062,7 @@ void PhysicsServerCommandProcessor::addTransformChangedNotifications()
void PhysicsServerCommandProcessor::resetSimulation() void PhysicsServerCommandProcessor::resetSimulation()
{ {
//clean up all data //clean up all data
m_data->m_remoteSyncTransformTime = m_data->m_remoteSyncTransformInterval;
m_data->m_simulationTimestamp = 0; m_data->m_simulationTimestamp = 0;
m_data->m_cachedVUrdfisualShapes.clear(); m_data->m_cachedVUrdfisualShapes.clear();

View File

@@ -905,6 +905,10 @@ enum InternalOpenGLVisualizerUpdateFlags
{ {
COV_SET_CAMERA_VIEW_MATRIX = 1, COV_SET_CAMERA_VIEW_MATRIX = 1,
COV_SET_FLAGS = 2, COV_SET_FLAGS = 2,
COV_SET_LIGHT_POSITION = 4,
COV_SET_SHADOWMAP_RESOLUTION = 8,
COV_SET_SHADOWMAP_WORLD_SIZE = 16,
COV_SET_REMOTE_SYNC_TRANSFORM_INTERVAL = 32,
}; };
struct ConfigureOpenGLVisualizerRequest struct ConfigureOpenGLVisualizerRequest
@@ -913,7 +917,10 @@ struct ConfigureOpenGLVisualizerRequest
double m_cameraPitch; double m_cameraPitch;
double m_cameraYaw; double m_cameraYaw;
double m_cameraTargetPosition[3]; double m_cameraTargetPosition[3];
double m_lightPosition[3];
int m_shadowMapResolution;
int m_shadowMapWorldSize;
double m_remoteSyncTransformInterval;
int m_setFlag; int m_setFlag;
int m_setEnabled; int m_setEnabled;
}; };

View File

@@ -0,0 +1,21 @@
import pybullet as p
import math
import time
dt = 1./240.
p.connect(p.GUI)
p.loadURDF("r2d2.urdf",[0,0,1])
p.loadURDF("plane.urdf")
p.setGravity(0,0,-10)
radius=5
t = 0
p.configureDebugVisualizer(shadowMapWorldSize=5)
p.configureDebugVisualizer(shadowMapResolution=8192)
while (1):
t+=dt
p.configureDebugVisualizer(lightPosition=[radius*math.sin(t),radius*math.cos(t),3])
p.stepSimulation()
time.sleep(dt)

View File

@@ -1,6 +1,5 @@
# Blender v2.76 (sub 0) OBJ File: '' # Blender v2.76 (sub 0) OBJ File: ''
# www.blender.org # www.blender.org
mtllib l_link_mini.mtl
o lower_link o lower_link
v -0.007927 -0.006511 0.072209 v -0.007927 -0.006511 0.072209
v -0.007918 -0.006511 0.072209 v -0.007918 -0.006511 0.072209

View File

@@ -4,7 +4,7 @@
<inertial> <inertial>
<mass value="3.3"/> <mass value="3.3"/>
<origin xyz="0.0 0.0 0.0"/> <origin xyz="0.0 0.0 0.0"/>
<inertia ixx="0.011253" ixy="0" ixz="0" iyy="0.362030" iyz="0" izz="0.042673"/> <inertia ixx="0.011253" ixy="0" ixz="0" iyy="0.036203" iyz="0" izz="0.042673"/>
</inertial> </inertial>
<visual> <visual>
<geometry> <geometry>

View File

@@ -5869,15 +5869,18 @@ static PyObject* pybullet_getDebugVisualizerCamera(PyObject* self, PyObject* arg
static PyObject* pybullet_configureDebugVisualizer(PyObject* self, PyObject* args, PyObject* keywds) static PyObject* pybullet_configureDebugVisualizer(PyObject* self, PyObject* args, PyObject* keywds)
{ {
int flag = 1; int flag = -1;
int enable = -1; int enable = -1;
int shadowMapResolution = -1;
int shadowMapWorldSize = -1;
int physicsClientId = 0; int physicsClientId = 0;
double remoteSyncTransformInterval = -1;
PyObject* pyLightPosition = 0;
b3PhysicsClientHandle sm = 0; b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"flag", "enable", "physicsClientId", NULL}; static char* kwlist[] = {"flag", "enable", "lightPosition", "shadowMapResolution", "shadowMapWorldSize", "remoteSyncTransformInterval", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, if (!PyArg_ParseTupleAndKeywords(args, keywds, "|iiOiidi", kwlist,
&flag, &enable, &physicsClientId)) &flag, &enable, &pyLightPosition, &shadowMapResolution, &shadowMapWorldSize, &remoteSyncTransformInterval, &physicsClientId))
return NULL; return NULL;
sm = getPhysicsClient(physicsClientId); sm = getPhysicsClient(physicsClientId);
@@ -5889,7 +5892,31 @@ static PyObject* pybullet_configureDebugVisualizer(PyObject* self, PyObject* arg
{ {
b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(sm); b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(sm);
if (flag >= 0)
{
b3ConfigureOpenGLVisualizerSetVisualizationFlags(commandHandle, flag, enable); b3ConfigureOpenGLVisualizerSetVisualizationFlags(commandHandle, flag, enable);
}
if (pyLightPosition)
{
float lightPosition[3];
if (pybullet_internalSetVector(pyLightPosition, lightPosition))
{
b3ConfigureOpenGLVisualizerSetLightPosition(commandHandle, lightPosition);
}
}
if (shadowMapResolution > 0)
{
b3ConfigureOpenGLVisualizerSetShadowMapResolution(commandHandle, shadowMapResolution);
}
if (shadowMapWorldSize > 0)
{
b3ConfigureOpenGLVisualizerSetShadowMapWorldSize(commandHandle, shadowMapWorldSize);
}
if (remoteSyncTransformInterval >= 0)
{
b3ConfigureOpenGLVisualizerSetRemoteSyncTransformInterval(commandHandle, remoteSyncTransformInterval);
}
b3SubmitClientCommandAndWaitStatus(sm, commandHandle); b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
} }
Py_INCREF(Py_None); Py_INCREF(Py_None);