diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 019eb2868..f81bce7bf 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -746,16 +746,13 @@ upAxisMat.setIdentity(); btTriangleMesh* meshInterface = new btTriangleMesh(); for (int i=0; im_numIndices/3; i++) { - float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw; - float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw; - float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw; - meshInterface->addTriangle( - btVector3(v0[0],v0[1],v0[2]), - btVector3(v1[0],v1[1],v1[2]), - btVector3(v2[0],v2[1],v2[2])); + const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i*3)]; + const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i*3+1)]; + const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i*3+2)]; + meshInterface->addTriangle(v0,v1,v2); } btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); - trimesh->setLocalScaling(collision->m_geometry.m_meshScale); + //trimesh->setLocalScaling(collision->m_geometry.m_meshScale); shape = trimesh; } else @@ -765,7 +762,7 @@ upAxisMat.setIdentity(); convexHull->optimizeConvexHull(); //convexHull->initializePolyhedralFeatures(); convexHull->setMargin(gUrdfDefaultCollisionMargin); - convexHull->setLocalScaling(collision->m_geometry.m_meshScale); + //convexHull->setLocalScaling(collision->m_geometry.m_meshScale); shape = convexHull; } diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 7ee648c3c..d0090f6b5 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -89,10 +89,10 @@ protected: virtual void resetCamera() { - float dist = 4; - float pitch = 193; - float yaw = 25; - float targetPos[3]={0,0,0.5};//-3,2.8,-2.5}; + float dist = 3.45; + float pitch = 287; + float yaw = 16.2; + float targetPos[3]={2.05,0.02,0.53};//-3,2.8,-2.5}; m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); } @@ -256,7 +256,11 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) case CMD_LOAD_SDF: { - b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf");//kuka_iiwa/model.sdf"); +#ifdef BT_DEBUG + b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf"); +#else + b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kitchens/1.sdf");//two_cubes.sdf");//kitchens/1.sdf");//kuka_iiwa/model.sdf"); +#endif b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } @@ -834,13 +838,16 @@ void PhysicsClientExample::stepSimulation(float deltaTime) //todo: rescale the depthValue to [0..255] if (depthValue>-1e20) { - int rgb = (depthValue-minDepthValue)*(255. / (btFabs(maxDepthValue-minDepthValue))); - if (rgb<0 || rgb>255) - { - - printf("rgb=%d\n",rgb); - } - + int rgb = 0; + + if (maxDepthValue!=minDepthValue) + { + rgb = (depthValue-minDepthValue)*(255. / (btFabs(maxDepthValue-minDepthValue))); + if (rgb<0 || rgb>255) + { + //printf("rgb=%d\n",rgb); + } + } m_canvas->setPixel(m_canvasDepthIndex,i,j, rgb, rgb, diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 90c4871af..a8494fa90 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -377,42 +377,91 @@ TinyRenderObjectData::~TinyRenderObjectData() delete m_model; } -void TinyRenderer::renderObjectDepth(TinyRenderObjectData& renderData) +static bool equals(const Vec4f& vA, const Vec4f& vB) { - int width = renderData.m_rgbColorBuffer.get_width(); - int height = renderData.m_rgbColorBuffer.get_height(); - - Vec3f light_dir_local = Vec3f(renderData.m_lightDirWorld[0],renderData.m_lightDirWorld[1],renderData.m_lightDirWorld[2]); - float light_distance = renderData.m_lightDistance; - Model* model = renderData.m_model; - if (0==model) - return; - - renderData.m_viewportMatrix = viewport(0,0,width, height); - - float* shadowBufferPtr = (renderData.m_shadowBuffer && renderData.m_shadowBuffer->size())?&renderData.m_shadowBuffer->at(0):0; - int* segmentationMaskBufferPtr = 0; - - TGAImage depthFrame(width, height, TGAImage::RGB); - - { - // light target is set to be the origin, and the up direction is set to be vertical up. - Matrix lightViewMatrix = lookat(light_dir_local*light_distance, Vec3f(0.0,0.0,0.0), Vec3f(0.0,0.0,1.0)); - Matrix lightModelViewMatrix = lightViewMatrix*renderData.m_modelMatrix; - Matrix lightViewProjectionMatrix = renderData.m_projectionMatrix; - Vec3f localScaling(renderData.m_localScaling[0],renderData.m_localScaling[1],renderData.m_localScaling[2]); - - DepthShader shader(model, lightModelViewMatrix, lightViewProjectionMatrix,renderData.m_modelMatrix, localScaling, light_distance); - - for (int i=0; infaces(); i++) - { - for (int j=0; j<3; j++) { - shader.vertex(i, j); - } - triangle(shader.varying_tri, shader, depthFrame, shadowBufferPtr, segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex); - } - } - + return false; +} + +static void clipEdge(const mat<4,3,float>& triangleIn, int vertexIndexA, int vertexIndexB, b3AlignedObjectArray& vertices) +{ + Vec4f v0New = triangleIn.col(vertexIndexA); + Vec4f v1New = triangleIn.col(vertexIndexB); + + bool v0Inside = v0New[3] > 0.f && v0New[2] > -v0New[3]; + bool v1Inside= v1New[3] > 0.f && v1New[2] > -v1New[3]; + + if (v0Inside && v1Inside) + { + + } else if (v0Inside || v1Inside) + { + float d0 = v0New[2]+v0New[3]; + float d1 = v1New[2]+v1New[3]; + float factor = 1.0 / (d1-d0); + Vec4f newVertex =(v0New*d1-v1New*d0)*factor; + if (v0Inside) + { + v1New = newVertex; + } else + { + v0New = newVertex; + } + } else + { + return; + } + + if (vertices.size()==0 || !(equals(vertices[vertices.size()-1],v0New))) + { + vertices.push_back(v0New); + } + + vertices.push_back(v1New); +} + + + + +static bool clipTriangleAgainstNearplane(const mat<4,3,float>& triangleIn, b3AlignedObjectArray >& clippedTrianglesOut) +{ + //discard triangle if all vertices are behind near-plane + if (triangleIn[3][0]<0 && triangleIn[3][1] <0 && triangleIn[3][2] <0) + { + return true; + } + + //accept triangle if all vertices are in front of the near-plane + if (triangleIn[3][0]>=0 && triangleIn[3][1] >=0 && triangleIn[3][2] >=0) + { + clippedTrianglesOut.push_back(triangleIn); + return false; + } + + Vec4f vtxCache[5]; + + b3AlignedObjectArray vertices; + vertices.initializeFromBuffer(vtxCache,0,5); + clipEdge(triangleIn,0,1,vertices); + clipEdge(triangleIn,1,2,vertices); + clipEdge(triangleIn,2,0,vertices); + + if (vertices.size()<3) + return true; + + if (equals(vertices[0],vertices[vertices.size()-1])) + { + vertices.pop_back(); + } + + //create a fan of triangles + for (int i=1;i& vtx = clippedTrianglesOut.expand(); + vtx.set_col(0,vertices[0]); + vtx.set_col(1,vertices[i]); + vtx.set_col(2,vertices[i+1]); + } + return true; } void TinyRenderer::renderObject(TinyRenderObjectData& renderData) @@ -449,7 +498,82 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) for (int j=0; j<3; j++) { shader.vertex(i, j); } - triangle(shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex); + + mat<4,3,float> stackTris[3]; + + b3AlignedObjectArray< mat<4,3,float> > clippedTriangles; + clippedTriangles.initializeFromBuffer(stackTris,0,3); + + bool hasClipped = clipTriangleAgainstNearplane(shader.varying_tri,clippedTriangles); + + if (hasClipped) + { + for (int t=0;tsize())?&renderData.m_shadowBuffer->at(0):0; + int* segmentationMaskBufferPtr = 0; + + TGAImage depthFrame(width, height, TGAImage::RGB); + + { + // light target is set to be the origin, and the up direction is set to be vertical up. + Matrix lightViewMatrix = lookat(light_dir_local*light_distance, Vec3f(0.0,0.0,0.0), Vec3f(0.0,0.0,1.0)); + Matrix lightModelViewMatrix = lightViewMatrix*renderData.m_modelMatrix; + Matrix lightViewProjectionMatrix = renderData.m_projectionMatrix; + Vec3f localScaling(renderData.m_localScaling[0],renderData.m_localScaling[1],renderData.m_localScaling[2]); + + DepthShader shader(model, lightModelViewMatrix, lightViewProjectionMatrix,renderData.m_modelMatrix, localScaling, light_distance); + + for (int i=0; infaces(); i++) + { + for (int j=0; j<3; j++) { + shader.vertex(i, j); + } + + + mat<4,3,float> stackTris[3]; + + b3AlignedObjectArray< mat<4,3,float> > clippedTriangles; + clippedTriangles.initializeFromBuffer(stackTris,0,3); + + bool hasClipped = clipTriangleAgainstNearplane(shader.varying_tri,clippedTriangles); + + if (hasClipped) + { + for (int t=0;t &clipc, mat<4,3,float> &orgClipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix) +{ + triangleClipped(clipc, orgClipc,shader,image,zbuffer,0,viewPortMatrix,0); +} + +void triangleClipped(mat<4,3,float> &clipc, mat<4,3,float> &orgClipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex) +{ + + mat<3,4,float> screenSpacePts = (viewPortMatrix*clipc).transpose(); // transposed to ease access to each of the points + + mat<3,2,float> pts2; + for (int i=0; i<3; i++) + { + pts2[i] = proj<2>(screenSpacePts[i]/screenSpacePts[i][3]); + } + + Vec2f bboxmin( std::numeric_limits::max(), std::numeric_limits::max()); + Vec2f bboxmax(-std::numeric_limits::max(), -std::numeric_limits::max()); + Vec2f clamp(image.get_width()-1, image.get_height()-1); + + for (int i=0; i<3; i++) { + for (int j=0; j<2; j++) { + bboxmin[j] = b3Max(0.f, b3Min(bboxmin[j], pts2[i][j])); + bboxmax[j] = b3Min(clamp[j], b3Max(bboxmax[j], pts2[i][j])); + } + } + + Vec2i P; + TGAColor color; + + mat<3,4,float> orgScreenSpacePts = (viewPortMatrix*orgClipc).transpose(); // transposed to ease access to each of the points + + mat<3,2,float> orgPts2; + for (int i=0; i<3; i++) + { + orgPts2[i] = proj<2>(orgScreenSpacePts[i]/orgScreenSpacePts[i][3]); + } + + + for (P.x=bboxmin.x; P.x<=bboxmax.x; P.x++) { + for (P.y=bboxmin.y; P.y<=bboxmax.y; P.y++) + { + float frag_depth = 0; + { + Vec3f bc_screen = barycentric(pts2[0], pts2[1], pts2[2], P); + Vec3f bc_clip = Vec3f(bc_screen.x/screenSpacePts[0][3], bc_screen.y/screenSpacePts[1][3], bc_screen.z/screenSpacePts[2][3]); + bc_clip = bc_clip/(bc_clip.x+bc_clip.y+bc_clip.z); + frag_depth = -1*(clipc[2]*bc_clip); + + if (bc_screen.x<0 || bc_screen.y<0 || bc_screen.z<0 || + zbuffer[P.x+P.y*image.get_width()]>frag_depth) + continue; + } + + Vec3f bc_screen2 = barycentric(orgPts2[0], orgPts2[1], orgPts2[2], P); + Vec3f bc_clip2 = Vec3f(bc_screen2.x/orgScreenSpacePts[0][3], bc_screen2.y/orgScreenSpacePts[1][3], bc_screen2.z/orgScreenSpacePts[2][3]); + bc_clip2 = bc_clip2/(bc_clip2.x+bc_clip2.y+bc_clip2.z); + float frag_depth2 = -1*(orgClipc[2]*bc_clip2); + + bool discard = shader.fragment(bc_clip2, color); + + if (!discard) { + zbuffer[P.x+P.y*image.get_width()] = frag_depth; + if (segmentationMaskBuffer) + { + segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectIndex; + } + image.set(P.x, P.y, color); + } + } + } +} + + void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix) { triangle(clipc,shader,image,zbuffer,0,viewPortMatrix,0); @@ -78,9 +152,7 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex) { mat<3,4,float> pts = (viewPortMatrix*clipc).transpose(); // transposed to ease access to each of the points - //we don't clip triangles that cross the near plane, just discard them instead of showing artifacts - if (pts[0][3]<0 || pts[1][3] <0 || pts[2][3] <0) - return; + mat<3,2,float> pts2; for (int i=0; i<3; i++) pts2[i] = proj<2>(pts[i]/pts[i][3]); @@ -119,4 +191,3 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb } } } - diff --git a/examples/TinyRenderer/our_gl.h b/examples/TinyRenderer/our_gl.h index 378324cbd..17403764c 100644 --- a/examples/TinyRenderer/our_gl.h +++ b/examples/TinyRenderer/our_gl.h @@ -18,5 +18,8 @@ struct IShader { void triangle(mat<4,3,float> &pts, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix); void triangle(mat<4,3,float> &pts, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex); +void triangleClipped(mat<4,3,float> &clippedPts, mat<4,3,float> &pts, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix); +void triangleClipped(mat<4,3,float> &clippedPts, mat<4,3,float> &pts, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex); + #endif //__OUR_GL_H__