Add Bullet C API and pybullet API to set projective texture matrices.

This commit is contained in:
yunfeibai
2018-03-18 18:45:54 -07:00
parent fd7aa8d0e1
commit 37696dd87e
12 changed files with 82 additions and 7 deletions

View File

@@ -94,6 +94,7 @@ struct GUIHelperInterface
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels, int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied){} int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied){}
virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16])=0;
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) =0; virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) =0;
@@ -179,6 +180,10 @@ struct DummyGUIHelper : public GUIHelperInterface
*numPixelsCopied = 0; *numPixelsCopied = 0;
} }
virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16])
{
}
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld)
{ {
} }

View File

@@ -49,6 +49,7 @@ 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 setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]){};
virtual void renderScene()=0; virtual void renderScene()=0;
virtual void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE){}; virtual void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE){};

View File

@@ -1098,6 +1098,10 @@ bool OpenGLGuiHelper::getCameraInfo(int* width, int* height, float viewMatrix[16
return false; return false;
} }
void OpenGLGuiHelper::setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16])
{
m_data->m_glApp->m_renderer->setProjectiveTextureMatrices(viewMatrix, projectionMatrix);
}
void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,

View File

@@ -62,6 +62,8 @@ struct OpenGLGuiHelper : public GUIHelperInterface
int startPixelIndex, int destinationWidth, int startPixelIndex, int destinationWidth,
int destinationHeight, int* numPixelsCopied); int destinationHeight, int* numPixelsCopied);
virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]);
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) ; virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) ;
virtual void drawText3D( const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag); virtual void drawText3D( const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag);

View File

@@ -225,6 +225,8 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
GLfloat m_projectionMatrix[16]; GLfloat m_projectionMatrix[16];
GLfloat m_viewMatrix[16]; GLfloat m_viewMatrix[16];
GLfloat m_projectiveTextureProjectionMatrix[16];
GLfloat m_projectiveTextureViewMatrix[16];
GLfloat m_viewMatrixInverse[16]; GLfloat m_viewMatrixInverse[16];
b3Vector3 m_lightPos; b3Vector3 m_lightPos;
@@ -257,6 +259,8 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
m_projectionMatrix[i]=0; m_projectionMatrix[i]=0;
m_viewMatrix[i]=0; m_viewMatrix[i]=0;
m_viewMatrixInverse[i]=0; m_viewMatrixInverse[i]=0;
m_projectiveTextureProjectionMatrix[i]=0;
m_projectiveTextureViewMatrix[i]=0;
} }
} }
@@ -1466,6 +1470,14 @@ void GLInstancingRenderer::setLightPosition(const double lightPos[3])
m_data->m_lightPos[2] = lightPos[2]; m_data->m_lightPos[2] = lightPos[2];
} }
void GLInstancingRenderer::setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16])
{
for (int i = 0; i < 16; i++)
{
m_data->m_projectiveTextureViewMatrix[i] = viewMatrix[i];
m_data->m_projectiveTextureProjectionMatrix[i] = projectionMatrix[i];
}
}
void GLInstancingRenderer::updateCamera(int upAxis) void GLInstancingRenderer::updateCamera(int upAxis)
{ {
@@ -2180,7 +2192,7 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
// TODO: Expose the projective texture matrix setup. Temporarily set it to be the same as camera view projection matrix. // TODO: Expose the projective texture matrix setup. Temporarily set it to be the same as camera view projection matrix.
GLfloat textureMVP[16]; GLfloat textureMVP[16];
b3Matrix4x4Mul16(m_data->m_projectionMatrix,m_data->m_viewMatrix,textureMVP); b3Matrix4x4Mul16(m_data->m_projectiveTextureProjectionMatrix,m_data->m_projectiveTextureViewMatrix,textureMVP);
//float m_frustumZNear=0.1; //float m_frustumZNear=0.1;
//float m_frustumZFar=100.f; //float m_frustumZFar=100.f;

View File

@@ -131,6 +131,7 @@ 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]);
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 resize(int width, int height); virtual void resize(int width, int height);
virtual int getScreenWidth() virtual int getScreenWidth()

View File

@@ -3081,6 +3081,19 @@ B3_SHARED_API void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle com
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_SHADOW; command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_SHADOW;
} }
B3_SHARED_API void b3RequestCameraImageSetProjectiveTextureMatrices(b3SharedMemoryCommandHandle commandHandle, float viewMatrix[16], float projectionMatrix[16])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
for (int i=0;i<16;i++)
{
command->m_requestPixelDataArguments.m_projectiveTextureProjectionMatrix[i] = projectionMatrix[i];
command->m_requestPixelDataArguments.m_projectiveTextureViewMatrix[i] = viewMatrix[i];
}
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_PROJECTIVE_TEXTURE_MATRICES;
}
B3_SHARED_API void b3ComputePositionFromViewMatrix(const float viewMatrix[16], float cameraPosition[3], float cameraTargetPosition[3], float cameraUp[3]) B3_SHARED_API void b3ComputePositionFromViewMatrix(const float viewMatrix[16], float cameraPosition[3], float cameraTargetPosition[3], float cameraUp[3])
{ {
b3Matrix3x3 r(viewMatrix[0], viewMatrix[4], viewMatrix[8], viewMatrix[1], viewMatrix[5], viewMatrix[9], viewMatrix[2], viewMatrix[6], viewMatrix[10]); b3Matrix3x3 r(viewMatrix[0], viewMatrix[4], viewMatrix[8], viewMatrix[1], viewMatrix[5], viewMatrix[9], viewMatrix[2], viewMatrix[6], viewMatrix[10]);

View File

@@ -3157,6 +3157,8 @@ bool PhysicsServerCommandProcessor::processRequestCameraImageCommand(const struc
serverStatusOut.m_numDataStreamBytes = numRequestedPixels * totalBytesPerPixel; serverStatusOut.m_numDataStreamBytes = numRequestedPixels * totalBytesPerPixel;
float viewMat[16]; float viewMat[16];
float projMat[16]; float projMat[16];
float projTextureViewMat[16];
float projTextureProjMat[16];
for (int i=0;i<16;i++) for (int i=0;i<16;i++)
{ {
viewMat[i] = clientCmd.m_requestPixelDataArguments.m_viewMatrix[i]; viewMat[i] = clientCmd.m_requestPixelDataArguments.m_viewMatrix[i];
@@ -3195,6 +3197,23 @@ bool PhysicsServerCommandProcessor::processRequestCameraImageCommand(const struc
{ {
useShadowMap = false; useShadowMap = false;
useProjectiveTexture = true; useProjectiveTexture = true;
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_PROJECTIVE_TEXTURE_MATRICES)!=0)
{
for (int i=0;i<16;i++)
{
projTextureViewMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureViewMatrix[i];
projTextureProjMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureProjectionMatrix[i];
}
}
else // If no specified matrices for projective texture, then use the camera matrices.
{
for (int i=0;i<16;i++)
{
projTextureViewMat[i] = viewMat[i];
projTextureProjMat[i] = projMat[i];
}
}
this->m_data->m_guiHelper->setProjectiveTextureMatrices(projTextureViewMat, projTextureProjMat);
} }
else else
{ {

View File

@@ -1120,6 +1120,11 @@ public:
workerThreadWait(); workerThreadWait();
} }
virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16])
{
m_childGuiHelper->getAppInterface()->m_renderer->setProjectiveTextureMatrices(viewMatrix, projectionMatrix);
}
btDiscreteDynamicsWorld* m_dynamicsWorld; btDiscreteDynamicsWorld* m_dynamicsWorld;
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld)

View File

@@ -240,6 +240,8 @@ struct RequestPixelDataArgs
float m_lightSpecularCoeff; float m_lightSpecularCoeff;
int m_hasShadow; int m_hasShadow;
int m_flags; int m_flags;
float m_projectiveTextureViewMatrix[16];
float m_projectiveTextureProjectionMatrix[16];
}; };
enum EnumRequestPixelDataUpdateFlags enum EnumRequestPixelDataUpdateFlags
@@ -254,6 +256,7 @@ enum EnumRequestPixelDataUpdateFlags
REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF=128, REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF=128,
REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF=256, REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF=256,
REQUEST_PIXEL_ARGS_HAS_FLAGS = 512, REQUEST_PIXEL_ARGS_HAS_FLAGS = 512,
REQUEST_PIXEL_ARGS_HAS_PROJECTIVE_TEXTURE_MATRICES=1024,
//don't exceed (1<<15), because this enum is shared with EnumRenderer in SharedMemoryPublic.h //don't exceed (1<<15), because this enum is shared with EnumRenderer in SharedMemoryPublic.h

View File

@@ -25,8 +25,12 @@ if (useRealTimeSimulation):
while 1: while 1:
if (useRealTimeSimulation): if (useRealTimeSimulation):
p.getCameraImage(300, 300, lightColor=[1.0,0.0,0.0], renderer=p.ER_BULLET_HARDWARE_OPENGL, flags=p.ER_USE_PROJECTIVE_TEXTURE) camera = p.getDebugVisualizerCamera()
viewMat = camera[2]
projMat = camera[3]
#An example of setting the view matrix for the projective texture.
#viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])
p.getCameraImage(300, 300, renderer=p.ER_BULLET_HARDWARE_OPENGL, flags=p.ER_USE_PROJECTIVE_TEXTURE, projectiveTextureView=viewMat, projectiveTextureProj=projMat)
p.setGravity(0,0,0) p.setGravity(0,0,0)
sleep(0.01) # Time in seconds.
else: else:
p.stepSimulation() p.stepSimulation()

View File

@@ -6761,10 +6761,12 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec
{ {
/// request an image from a simulated camera, using software or hardware renderer. /// request an image from a simulated camera, using software or hardware renderer.
struct b3CameraImageData imageData; struct b3CameraImageData imageData;
PyObject *objViewMat = 0, *objProjMat = 0, *lightDirObj = 0, *lightColorObj = 0; PyObject *objViewMat = 0, *objProjMat = 0, *lightDirObj = 0, *lightColorObj = 0, *objProjectiveTextureView = 0, *objProjectiveTextureProj = 0;
int width, height; int width, height;
float viewMatrix[16]; float viewMatrix[16];
float projectionMatrix[16]; float projectionMatrix[16];
float projectiveTextureView[16];
float projectiveTextureProj[16];
float lightDir[3]; float lightDir[3];
float lightColor[3]; float lightColor[3];
float lightDist = -1; float lightDist = -1;
@@ -6779,9 +6781,9 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec
int physicsClientId = 0; int physicsClientId = 0;
b3PhysicsClientHandle sm = 0; b3PhysicsClientHandle sm = 0;
// set camera resolution, optionally view, projection matrix, light direction, light color, light distance, shadow // set camera resolution, optionally view, projection matrix, light direction, light color, light distance, shadow
static char* kwlist[] = {"width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", "renderer", "flags", "physicsClientId", NULL}; static char* kwlist[] = {"width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", "renderer", "flags", "projectiveTextureView", "projectiveTextureProj", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOOOfifffiii", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj, &lightColorObj, &lightDist, &hasShadow, &lightAmbientCoeff, &lightDiffuseCoeff, &lightSpecularCoeff, &renderer, &flags, &physicsClientId)) if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOOOfifffiiOOi", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj, &lightColorObj, &lightDist, &hasShadow, &lightAmbientCoeff, &lightDiffuseCoeff, &lightSpecularCoeff, &renderer, &flags, &objProjectiveTextureView, &objProjectiveTextureProj, &physicsClientId))
{ {
return NULL; return NULL;
} }
@@ -6837,6 +6839,10 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec
{ {
b3RequestCameraImageSetFlags(command, flags); b3RequestCameraImageSetFlags(command, flags);
} }
if (objProjectiveTextureView && objProjectiveTextureProj && pybullet_internalSetMatrix(objProjectiveTextureView, projectiveTextureView) && (pybullet_internalSetMatrix(objProjectiveTextureProj, projectiveTextureProj)))
{
b3RequestCameraImageSetProjectiveTextureMatrices(command, projectiveTextureView, projectiveTextureProj);
}
if (renderer>=0) if (renderer>=0)
{ {
b3RequestCameraImageSelectRenderer(command, renderer);//renderer could be ER_BULLET_HARDWARE_OPENGL b3RequestCameraImageSelectRenderer(command, renderer);//renderer could be ER_BULLET_HARDWARE_OPENGL