Set the perspective projection for projective texture mapping. Change the demo to two bears with the same view projection matrix for the camera and the projector.
This commit is contained in:
32
data/teddy_large.urdf
Normal file
32
data/teddy_large.urdf
Normal file
@@ -0,0 +1,32 @@
|
|||||||
|
<?xml version="0.0" ?>
|
||||||
|
<robot name="cube.urdf">
|
||||||
|
<link name="baseLink">
|
||||||
|
<contact>
|
||||||
|
<lateral_friction value="0.5"/>
|
||||||
|
<rolling_friction value="0.0"/>
|
||||||
|
<contact_cfm value="0.0"/>
|
||||||
|
<contact_erp value="1.0"/>
|
||||||
|
</contact>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.07 0.05 0.03"/>
|
||||||
|
<mass value=".1"/>
|
||||||
|
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="teddy2_VHACD_CHs.obj" scale="3 3 3"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="red">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="teddy2_VHACD_CHs.obj" scale="3 3 3"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
</robot>
|
||||||
|
|
||||||
@@ -29,4 +29,3 @@
|
|||||||
</collision>
|
</collision>
|
||||||
</link>
|
</link>
|
||||||
</robot>
|
</robot>
|
||||||
|
|
||||||
|
|||||||
@@ -17,7 +17,6 @@ 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;
|
||||||
float projectiveTextureViewSize = 10;
|
|
||||||
bool useProjectiveTexture = false;
|
bool useProjectiveTexture = false;
|
||||||
int shadowMapWidth= 4096;
|
int shadowMapWidth= 4096;
|
||||||
int shadowMapHeight= 4096;
|
int shadowMapHeight= 4096;
|
||||||
@@ -228,9 +227,6 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
|
|||||||
|
|
||||||
b3Vector3 m_lightPos;
|
b3Vector3 m_lightPos;
|
||||||
b3Vector3 m_lightSpecularIntensity;
|
b3Vector3 m_lightSpecularIntensity;
|
||||||
|
|
||||||
b3Vector3 m_projectorPos;
|
|
||||||
b3Vector3 m_projectorDir;
|
|
||||||
|
|
||||||
GLuint m_defaultTexturehandle;
|
GLuint m_defaultTexturehandle;
|
||||||
b3AlignedObjectArray<InternalTextureHandle> m_textureHandles;
|
b3AlignedObjectArray<InternalTextureHandle> m_textureHandles;
|
||||||
@@ -252,8 +248,6 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
|
|||||||
{
|
{
|
||||||
m_lightPos=b3MakeVector3(-50,30,40);
|
m_lightPos=b3MakeVector3(-50,30,40);
|
||||||
m_lightSpecularIntensity.setValue(1,1,1);
|
m_lightSpecularIntensity.setValue(1,1,1);
|
||||||
|
|
||||||
m_projectorPos=b3MakeVector3(-50,30,40);
|
|
||||||
|
|
||||||
//clear to zero to make it obvious if the matrix is used uninitialized
|
//clear to zero to make it obvious if the matrix is used uninitialized
|
||||||
for (int i=0;i<16;i++)
|
for (int i=0;i<16;i++)
|
||||||
@@ -336,7 +330,7 @@ static GLint projectiveTexture_cameraPositionIn = 0;
|
|||||||
static GLint projectiveTexture_materialShininessIn = 0;
|
static GLint projectiveTexture_materialShininessIn = 0;
|
||||||
|
|
||||||
static GLint projectiveTexture_ProjectionMatrix=0;
|
static GLint projectiveTexture_ProjectionMatrix=0;
|
||||||
static GLint projectiveTexture_DepthBiasModelViewMatrix=0;
|
static GLint projectiveTexture_TextureMVP=0;
|
||||||
static GLint projectiveTexture_uniform_texture_diffuse = 0;
|
static GLint projectiveTexture_uniform_texture_diffuse = 0;
|
||||||
static GLint projectiveTexture_shadowMap = 0;
|
static GLint projectiveTexture_shadowMap = 0;
|
||||||
|
|
||||||
@@ -1241,7 +1235,7 @@ void GLInstancingRenderer::InitShaders()
|
|||||||
projectiveTexture_materialSpecularColor = glGetUniformLocation(projectiveTextureInstancingShader, "materialSpecularColorIn");
|
projectiveTexture_materialSpecularColor = glGetUniformLocation(projectiveTextureInstancingShader, "materialSpecularColorIn");
|
||||||
projectiveTexture_MVP = glGetUniformLocation(projectiveTextureInstancingShader, "MVP");
|
projectiveTexture_MVP = glGetUniformLocation(projectiveTextureInstancingShader, "MVP");
|
||||||
projectiveTexture_ProjectionMatrix = glGetUniformLocation(projectiveTextureInstancingShader, "ProjectionMatrix");
|
projectiveTexture_ProjectionMatrix = glGetUniformLocation(projectiveTextureInstancingShader, "ProjectionMatrix");
|
||||||
projectiveTexture_DepthBiasModelViewMatrix = glGetUniformLocation(projectiveTextureInstancingShader, "DepthBiasModelViewProjectionMatrix");
|
projectiveTexture_TextureMVP = glGetUniformLocation(projectiveTextureInstancingShader, "TextureMVP");
|
||||||
projectiveTexture_uniform_texture_diffuse = glGetUniformLocation(projectiveTextureInstancingShader, "Diffuse");
|
projectiveTexture_uniform_texture_diffuse = glGetUniformLocation(projectiveTextureInstancingShader, "Diffuse");
|
||||||
projectiveTexture_shadowMap = glGetUniformLocation(projectiveTextureInstancingShader,"shadowMap");
|
projectiveTexture_shadowMap = glGetUniformLocation(projectiveTextureInstancingShader,"shadowMap");
|
||||||
projectiveTexture_lightPosIn = glGetUniformLocation(projectiveTextureInstancingShader,"lightPosIn");
|
projectiveTexture_lightPosIn = glGetUniformLocation(projectiveTextureInstancingShader,"lightPosIn");
|
||||||
@@ -2168,17 +2162,9 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
|
|||||||
GLfloat depthBiasMVP[4][4];
|
GLfloat depthBiasMVP[4][4];
|
||||||
b3Matrix4x4Mul(biasMatrix,depthMVP,depthBiasMVP);
|
b3Matrix4x4Mul(biasMatrix,depthMVP,depthBiasMVP);
|
||||||
|
|
||||||
// Compute projection matrix for texture projector
|
// TODO: Expose the projective texture matrix setup. Temporarily set it to be the same as camera view projection matrix.
|
||||||
float textureViewMatrix[4][4];
|
GLfloat textureMVP[16];
|
||||||
b3Vector3 projectorDir = m_data->m_projectorDir.normalize();
|
b3Matrix4x4Mul16(m_data->m_projectionMatrix,m_data->m_viewMatrix,textureMVP);
|
||||||
float projectorDist = 0;
|
|
||||||
b3CreateLookAt(m_data->m_projectorPos,center,up, &textureViewMatrix[0][0]);
|
|
||||||
GLfloat textureModelMatrix[4][4];
|
|
||||||
b3CreateDiagonalMatrix(1.f, textureModelMatrix);
|
|
||||||
b3Matrix4x4Mul(textureViewMatrix, textureModelMatrix, textureModelViewMatrix);
|
|
||||||
b3CreateOrtho(-projectiveTextureViewSize,projectiveTextureViewSize,-projectiveTextureViewSize,projectiveTextureViewSize,1,300,textureProjectionMatrix);
|
|
||||||
GLfloat textureMVP[4][4];
|
|
||||||
b3Matrix4x4Mul(textureProjectionMatrix, textureModelViewMatrix, textureMVP);
|
|
||||||
|
|
||||||
//float m_frustumZNear=0.1;
|
//float m_frustumZNear=0.1;
|
||||||
//float m_frustumZFar=100.f;
|
//float m_frustumZFar=100.f;
|
||||||
@@ -2576,10 +2562,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
|
|||||||
glUniform3f(projectiveTexture_cameraPositionIn,camPos[0],camPos[1],camPos[2]);
|
glUniform3f(projectiveTexture_cameraPositionIn,camPos[0],camPos[1],camPos[2]);
|
||||||
glUniform1f(projectiveTexture_materialShininessIn,gfxObj->m_materialShinyNess);
|
glUniform1f(projectiveTexture_materialShininessIn,gfxObj->m_materialShinyNess);
|
||||||
|
|
||||||
glUniformMatrix4fv(projectiveTexture_DepthBiasModelViewMatrix, 1, false, &textureMVP[0][0]);
|
glUniformMatrix4fv(projectiveTexture_TextureMVP, 1, false, &textureMVP[0]);
|
||||||
glActiveTexture(GL_TEXTURE1);
|
|
||||||
glBindTexture(GL_TEXTURE_2D, m_data->m_shadowTexture);
|
|
||||||
glUniform1i(projectiveTexture_shadowMap,1);
|
|
||||||
|
|
||||||
//sort transparent objects
|
//sort transparent objects
|
||||||
if ( gfxObj->m_flags&eGfxTransparency)
|
if ( gfxObj->m_flags&eGfxTransparency)
|
||||||
@@ -2600,8 +2583,6 @@ b3Assert(glGetError() ==GL_NO_ERROR);
|
|||||||
glDisable (GL_BLEND);
|
glDisable (GL_BLEND);
|
||||||
glDepthMask(true);
|
glDepthMask(true);
|
||||||
}
|
}
|
||||||
glActiveTexture(GL_TEXTURE1);
|
|
||||||
glBindTexture(GL_TEXTURE_2D,0);
|
|
||||||
|
|
||||||
glActiveTexture(GL_TEXTURE0);
|
glActiveTexture(GL_TEXTURE0);
|
||||||
glBindTexture(GL_TEXTURE_2D,0);
|
glBindTexture(GL_TEXTURE_2D,0);
|
||||||
|
|||||||
@@ -11,7 +11,7 @@ layout (location = 5) in vec4 instance_color;
|
|||||||
layout (location = 6) in vec3 instance_scale;
|
layout (location = 6) in vec3 instance_scale;
|
||||||
|
|
||||||
|
|
||||||
uniform mat4 DepthBiasModelViewProjectionMatrix;
|
uniform mat4 TextureMVP;
|
||||||
uniform mat4 MVP;
|
uniform mat4 MVP;
|
||||||
uniform vec3 lightPosIn;
|
uniform vec3 lightPosIn;
|
||||||
uniform vec3 cameraPositionIn;
|
uniform vec3 cameraPositionIn;
|
||||||
@@ -93,7 +93,7 @@ void main(void)
|
|||||||
gl_Position = vertexLoc;
|
gl_Position = vertexLoc;
|
||||||
|
|
||||||
fragment.color = instance_color;
|
fragment.color = instance_color;
|
||||||
vec4 projcoords = DepthBiasModelViewProjectionMatrix * vec4((instance_position+localcoord).xyz,1);
|
vec4 projcoords = TextureMVP * vec4((instance_position+localcoord).xyz,1);
|
||||||
vert.texcoord = projcoords.xy;
|
vert.texcoord = projcoords.xy/projcoords.z;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -9,7 +9,7 @@ static const char* projectiveTextureInstancingVertexShader= \
|
|||||||
"layout (location = 4) in vec3 vertexnormal;\n"
|
"layout (location = 4) in vec3 vertexnormal;\n"
|
||||||
"layout (location = 5) in vec4 instance_color;\n"
|
"layout (location = 5) in vec4 instance_color;\n"
|
||||||
"layout (location = 6) in vec3 instance_scale;\n"
|
"layout (location = 6) in vec3 instance_scale;\n"
|
||||||
"uniform mat4 DepthBiasModelViewProjectionMatrix;\n"
|
"uniform mat4 TextureMVP;\n"
|
||||||
"uniform mat4 MVP;\n"
|
"uniform mat4 MVP;\n"
|
||||||
"uniform vec3 lightPosIn;\n"
|
"uniform vec3 lightPosIn;\n"
|
||||||
"uniform vec3 cameraPositionIn;\n"
|
"uniform vec3 cameraPositionIn;\n"
|
||||||
@@ -80,7 +80,7 @@ static const char* projectiveTextureInstancingVertexShader= \
|
|||||||
" vec4 vertexLoc = MVP* vec4((instance_position+localcoord).xyz,1);\n"
|
" vec4 vertexLoc = MVP* vec4((instance_position+localcoord).xyz,1);\n"
|
||||||
" gl_Position = vertexLoc;\n"
|
" gl_Position = vertexLoc;\n"
|
||||||
" fragment.color = instance_color;\n"
|
" fragment.color = instance_color;\n"
|
||||||
" vec4 projcoords = DepthBiasModelViewProjectionMatrix * vec4((instance_position+localcoord).xyz,1);\n"
|
" vec4 projcoords = TextureMVP * vec4((instance_position+localcoord).xyz,1);\n"
|
||||||
" vert.texcoord = projcoords.xy;\n"
|
" vert.texcoord = projcoords.xy/projcoords.z;\n"
|
||||||
"}\n"
|
"}\n"
|
||||||
;
|
;
|
||||||
|
|||||||
@@ -6,34 +6,17 @@ import numpy as np
|
|||||||
|
|
||||||
physicsClient = p.connect(p.GUI)
|
physicsClient = p.connect(p.GUI)
|
||||||
|
|
||||||
p.setGravity(0,0,-10)
|
p.setGravity(0,0,0)
|
||||||
planeId = p.loadURDF("plane.urdf")
|
bearStartPos1 = [-3.3,0,0]
|
||||||
cubeStartPos = [0,0,1]
|
bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0])
|
||||||
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
|
bearId1 = p.loadURDF("teddy_large.urdf", bearStartPos1, bearStartOrientation1)
|
||||||
boxId = p.loadURDF("cube.urdf",cubeStartPos, cubeStartOrientation)
|
bearStartPos2 = [0,0,0]
|
||||||
cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
|
bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0])
|
||||||
textureId = p.loadTexture("checker_blue.png")
|
bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2)
|
||||||
|
textureId = p.loadTexture("checker_grid.jpg")
|
||||||
p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
|
p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
|
||||||
p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
|
p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
|
||||||
|
|
||||||
fov = 70
|
|
||||||
pixelWidth = 640
|
|
||||||
pixelHeight = 512
|
|
||||||
aspect = pixelWidth / pixelHeight;
|
|
||||||
nearPlane = 0.01
|
|
||||||
farPlane = 100
|
|
||||||
projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
|
|
||||||
viewMatrixLeft = p.computeViewMatrix(cameraEyePosition=[0.045, 3.0, 3.0], cameraTargetPosition=[0.045,0,0], cameraUpVector=[0,0,1])
|
|
||||||
viewMatrixRight = p.computeViewMatrix(cameraEyePosition=[-0.045, 3.0, 3.0], cameraTargetPosition=[-0.045,0,0], cameraUpVector=[0,0,1])
|
|
||||||
leftImage = p.getCameraImage(width=640,height=512,viewMatrix=viewMatrixLeft,projectionMatrix=projectionMatrix,renderer=p.ER_BULLET_HARDWARE_OPENGL)
|
|
||||||
rightImage = p.getCameraImage(width=640,height=512,viewMatrix=viewMatrixRight,projectionMatrix=projectionMatrix,renderer=p.ER_BULLET_HARDWARE_OPENGL)
|
|
||||||
|
|
||||||
left_img = np.reshape(np.asarray(leftImage[2], dtype=np.float32), (512, 640, 4))
|
|
||||||
left_img /= 255
|
|
||||||
plt.imsave("left_image.png", left_img)
|
|
||||||
right_img = np.reshape(np.asarray(rightImage[2], dtype=np.float32), (512, 640, 4))
|
|
||||||
right_img /= 255
|
|
||||||
plt.imsave("right_image.png", right_img)
|
|
||||||
|
|
||||||
useRealTimeSimulation = 1
|
useRealTimeSimulation = 1
|
||||||
|
|
||||||
@@ -42,7 +25,7 @@ if (useRealTimeSimulation):
|
|||||||
|
|
||||||
while 1:
|
while 1:
|
||||||
if (useRealTimeSimulation):
|
if (useRealTimeSimulation):
|
||||||
p.setGravity(0,0,-10)
|
p.setGravity(0,0,0)
|
||||||
sleep(0.01) # Time in seconds.
|
sleep(0.01) # Time in seconds.
|
||||||
else:
|
else:
|
||||||
p.stepSimulation()
|
p.stepSimulation()
|
||||||
|
|||||||
Reference in New Issue
Block a user