diff --git a/data/racecar/racecar_differential.urdf b/data/racecar/racecar_differential.urdf index 8f5f37519..c4f6ee42e 100644 --- a/data/racecar/racecar_differential.urdf +++ b/data/racecar/racecar_differential.urdf @@ -20,12 +20,12 @@ - + - + @@ -33,14 +33,14 @@ - + - + @@ -66,14 +66,14 @@ - + - + @@ -163,14 +163,14 @@ - + - + @@ -207,14 +207,14 @@ - + - + @@ -533,7 +533,7 @@ - + @@ -573,7 +573,7 @@ - + @@ -612,7 +612,7 @@ - + @@ -650,7 +650,7 @@ - + @@ -686,7 +686,7 @@ - + diff --git a/data/sphere_transparent.urdf b/data/sphere_transparent.urdf new file mode 100644 index 000000000..d856b5a26 --- /dev/null +++ b/data/sphere_transparent.urdf @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/Collision/CollisionTutorialBullet2.cpp b/examples/Collision/CollisionTutorialBullet2.cpp index 8159f89ee..6e65ac0cd 100644 --- a/examples/Collision/CollisionTutorialBullet2.cpp +++ b/examples/Collision/CollisionTutorialBullet2.cpp @@ -79,7 +79,6 @@ public: gTotalPoints = 0; m_app->setUpAxis(1); - m_app->m_renderer->enableBlend(true); switch (m_tutorialIndex) { @@ -250,7 +249,6 @@ public: m_timeSeriesCanvas0 = 0; - m_app->m_renderer->enableBlend(false); } diff --git a/examples/CommonInterfaces/CommonRenderInterface.h b/examples/CommonInterfaces/CommonRenderInterface.h index a01533358..851db8d5f 100644 --- a/examples/CommonInterfaces/CommonRenderInterface.h +++ b/examples/CommonInterfaces/CommonRenderInterface.h @@ -86,7 +86,7 @@ struct CommonRenderInterface virtual int getTotalNumInstances() const = 0; virtual void writeTransforms()=0; - virtual void enableBlend(bool blend)=0; + virtual void clearZBuffer()=0; //This is internal access to OpenGL3+ features, mainly used for OpenCL-OpenGL interop diff --git a/examples/InverseKinematics/InverseKinematicsExample.cpp b/examples/InverseKinematics/InverseKinematicsExample.cpp index 9f2eae900..b224b8b51 100644 --- a/examples/InverseKinematics/InverseKinematicsExample.cpp +++ b/examples/InverseKinematics/InverseKinematicsExample.cpp @@ -198,7 +198,6 @@ public: } virtual ~InverseKinematicsExample() { - m_app->m_renderer->enableBlend(false); } diff --git a/examples/MultiThreading/MultiThreadingExample.cpp b/examples/MultiThreading/MultiThreadingExample.cpp index 92dea68c5..d0ad1ba51 100644 --- a/examples/MultiThreading/MultiThreadingExample.cpp +++ b/examples/MultiThreading/MultiThreadingExample.cpp @@ -176,14 +176,12 @@ public: //int numBodies = 1; m_app->setUpAxis(1); - m_app->m_renderer->enableBlend(true); } virtual ~MultiThreadingExample() { - m_app->m_renderer->enableBlend(false); } diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index c7fffa36b..3796bf371 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -115,6 +115,10 @@ static InternalDataRenderer* sData2; GLint lineWidthRange[2]={1,1}; +enum +{ + eGfxTransparency=1 +}; struct b3GraphicsInstance { GLuint m_cube_vao; @@ -124,6 +128,7 @@ struct b3GraphicsInstance int m_numIndices; int m_numVertices; + int m_numGraphicsInstances; b3AlignedObjectArray m_tempObjectUids; int m_instanceOffset; @@ -131,6 +136,7 @@ struct b3GraphicsInstance int m_primitiveType; float m_materialShinyNess; b3Vector3 m_materialSpecularColor; + int m_flags;//transparency etc b3GraphicsInstance() :m_cube_vao(-1), @@ -143,7 +149,8 @@ struct b3GraphicsInstance m_vertexArrayOffset(0), m_primitiveType(B3_GL_TRIANGLES), m_materialShinyNess(41), - m_materialSpecularColor(b3MakeVector3(.5,.5,.5)) + m_materialSpecularColor(b3MakeVector3(.5,.5,.5)), + m_flags(0) { } @@ -324,8 +331,7 @@ GLInstancingRenderer::GLInstancingRenderer(int maxNumObjectCapacity, int maxShap m_textureinitialized(false), m_screenWidth(0), m_screenHeight(0), - m_upAxis(1), - m_enableBlend(false) + m_upAxis(1) { m_data = new InternalDataRenderer; @@ -865,6 +871,10 @@ int GLInstancingRenderer::registerGraphicsInstanceInternal(int newUid, const flo m_data->m_instance_scale_ptr[index*3+1] = scaling[1]; m_data->m_instance_scale_ptr[index*3+2] = scaling[2]; + if (color[3]<1 && color[3]>0) + { + gfxObj->m_flags |= eGfxTransparency; + } gfxObj->m_numGraphicsInstances++; m_data->m_totalNumInstances++; } else @@ -1898,7 +1908,24 @@ void GLInstancingRenderer::drawLine(const float from[4], const float to[4], cons glUseProgram(0); } +struct SortableTransparentInstance +{ + int m_shapeIndex; + int m_instanceId; + b3Vector3 m_centerPosition; +}; +struct TransparentDistanceSortPredicate +{ + b3Vector3 m_camForwardVec; + + inline bool operator() (const SortableTransparentInstance& a, const SortableTransparentInstance& b) const + { + b3Scalar projA = a.m_centerPosition.dot(m_camForwardVec); + b3Scalar projB = b.m_centerPosition.dot(m_camForwardVec); + return (projA > projB); + } +}; void GLInstancingRenderer::renderSceneInternal(int renderMode) { @@ -2019,9 +2046,9 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode) b3Vector3 center = b3MakeVector3(0,0,0); //float upf[3]; //m_data->m_activeCamera->getCameraUpVector(upf); - b3Vector3 up, fwd; + b3Vector3 up, lightFwd; b3Vector3 lightDir = m_data->m_lightPos.normalized(); - b3PlaneSpace1(lightDir,up,fwd); + b3PlaneSpace1(lightDir,up,lightFwd); // b3Vector3 up = b3MakeVector3(upf[0],upf[1],upf[2]); b3CreateLookAt(m_data->m_lightPos,center,up,&depthViewMatrix[0][0]); //b3CreateLookAt(lightPos,m_data->m_cameraTargetPosition,b3Vector3(0,1,0),(float*)depthModelViewMatrix2); @@ -2081,198 +2108,299 @@ b3Assert(glGetError() ==GL_NO_ERROR); { totalNumInstances+=m_graphicsInstances[i]->m_numGraphicsInstances; } - - int curOffset = 0; - //GLuint lastBindTexture = 0; - - for (int i=0;i transparentInstances; + { + int curOffset = 0; + //GLuint lastBindTexture = 0; - b3GraphicsInstance* gfxObj = m_graphicsInstances[i]; - if (gfxObj->m_numGraphicsInstances) + transparentInstances.reserve(totalNumInstances); + + for (int obj=0;objm_texturehandle) - curBindTexture = gfxObj->m_texturehandle; - else - curBindTexture = m_data->m_defaultTexturehandle; - -//disable lazy evaluation, it just leads to bugs - //if (lastBindTexture != curBindTexture) + b3GraphicsInstance* gfxObj = m_graphicsInstances[obj]; + if (gfxObj->m_numGraphicsInstances) { - glBindTexture(GL_TEXTURE_2D,curBindTexture); - } - //lastBindTexture = curBindTexture; - -b3Assert(glGetError() ==GL_NO_ERROR); - // int myOffset = gfxObj->m_instanceOffset*4*sizeof(float); - - int POSITION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4); - int ORIENTATION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4); - int COLOR_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4); -// int SCALE_BUFFER_SIZE = (totalNumInstances*sizeof(float)*3); - - glBindVertexArray(gfxObj->m_cube_vao); - - - int vertexStride = 9*sizeof(float); - PointerCaster vertex; - vertex.m_baseIndex = gfxObj->m_vertexArrayOffset*vertexStride; - - - glVertexAttribPointer(0, 4, GL_FLOAT, GL_FALSE, 9*sizeof(float), vertex.m_pointer); - glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(curOffset*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes)); - glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(curOffset*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE)); - - PointerCaster uv; - uv.m_baseIndex = 7*sizeof(float)+vertex.m_baseIndex; - - PointerCaster normal; - normal.m_baseIndex = 4*sizeof(float)+vertex.m_baseIndex; - - glVertexAttribPointer(3, 2, GL_FLOAT, GL_FALSE, 9*sizeof(float), uv.m_pointer); - glVertexAttribPointer(4, 3, GL_FLOAT, GL_FALSE, 9*sizeof(float), normal.m_pointer); - glVertexAttribPointer(5, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(curOffset*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE)); - glVertexAttribPointer(6, 3, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(curOffset*3*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE)); - - glEnableVertexAttribArray(0); - glEnableVertexAttribArray(1); - glEnableVertexAttribArray(2); - glEnableVertexAttribArray(3); - glEnableVertexAttribArray(4); - glEnableVertexAttribArray(5); - glEnableVertexAttribArray(6); - glVertexAttribDivisor(0, 0); - glVertexAttribDivisor(1, 1); - glVertexAttribDivisor(2, 1); - glVertexAttribDivisor(3, 0); - glVertexAttribDivisor(4, 0); - glVertexAttribDivisor(5, 1); - glVertexAttribDivisor(6, 1); - - - - - - - int indexCount = gfxObj->m_numIndices; - GLvoid* indexOffset = 0; - - glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, gfxObj->m_index_vbo); - { - B3_PROFILE("glDrawElementsInstanced"); - - if (gfxObj->m_primitiveType==B3_GL_POINTS) + SortableTransparentInstance inst; + + inst.m_shapeIndex = obj; + + + if ((gfxObj->m_flags&eGfxTransparency)==0) { - glUseProgram(instancingShaderPointSprite); - glUniformMatrix4fv(ProjectionMatrixPointSprite, 1, false, &m_data->m_projectionMatrix[0]); - glUniformMatrix4fv(ModelViewMatrixPointSprite, 1, false, &m_data->m_viewMatrix[0]); - glUniform1f(screenWidthPointSprite,float(m_screenWidth)); - - //glUniform1i(uniform_texture_diffusePointSprite, 0); - b3Assert(glGetError() ==GL_NO_ERROR); - glPointSize(20); - -#ifndef __APPLE__ - glEnable(GL_POINT_SPRITE_ARB); -// glTexEnvi(GL_POINT_SPRITE_ARB, GL_COORD_REPLACE_ARB, GL_TRUE); -#endif - - glEnable(GL_VERTEX_PROGRAM_POINT_SIZE); - glDrawElementsInstanced(GL_POINTS, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); + inst.m_instanceId = curOffset; + inst.m_centerPosition.setValue(m_data->m_instance_positions_ptr[inst.m_instanceId*4+0], + m_data->m_instance_positions_ptr[inst.m_instanceId*4+1], + m_data->m_instance_positions_ptr[inst.m_instanceId*4+2]); + inst.m_centerPosition *= -1;//reverse sort opaque instances + transparentInstances.push_back(inst); } else { - switch (renderMode) + for (int i=0;im_numGraphicsInstances;i++) { - - case B3_DEFAULT_RENDERMODE: - { - glUseProgram(instancingShader); - glUniformMatrix4fv(ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]); - glUniformMatrix4fv(ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); - - b3Vector3 gLightDir = m_data->m_lightPos; - gLightDir.normalize(); - glUniform3f(regularLightDirIn,gLightDir[0],gLightDir[1],gLightDir[2]); - - glUniform1i(uniform_texture_diffuse, 0); - glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); - break; - } - case B3_CREATE_SHADOWMAP_RENDERMODE: - { - /*printf("createShadowMapInstancingShader=%d\n",createShadowMapInstancingShader); - printf("createShadow_depthMVP=%d\n",createShadow_depthMVP); - printf("indexOffset=%d\n",indexOffset); - printf("gfxObj->m_numGraphicsInstances=%d\n",gfxObj->m_numGraphicsInstances); - printf("indexCount=%d\n",indexCount); - */ - - glUseProgram(createShadowMapInstancingShader); - glUniformMatrix4fv(createShadow_depthMVP, 1, false, &depthMVP[0][0]); - glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); - break; - } - - case B3_USE_SHADOWMAP_RENDERMODE: - { - if (m_enableBlend) - { - glEnable (GL_BLEND); - glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - } - - glUseProgram(useShadowMapInstancingShader); - glUniformMatrix4fv(useShadow_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]); - glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); - glUniformMatrix4fv(useShadow_ViewMatrixInverse, 1, false, &m_data->m_viewMatrixInverse[0]); - glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); - - glUniform3f(useShadow_lightSpecularIntensity, m_data->m_lightSpecularIntensity[0],m_data->m_lightSpecularIntensity[1],m_data->m_lightSpecularIntensity[2]); - glUniform3f(useShadow_materialSpecularColor, gfxObj->m_materialSpecularColor[0],gfxObj->m_materialSpecularColor[1],gfxObj->m_materialSpecularColor[2]); - - - - float MVP[16]; - b3Matrix4x4Mul16(m_data->m_projectionMatrix,m_data->m_viewMatrix,MVP); - glUniformMatrix4fv(useShadow_MVP, 1, false, &MVP[0]); - //gLightDir.normalize(); - glUniform3f(useShadow_lightPosIn,m_data->m_lightPos[0],m_data->m_lightPos[1],m_data->m_lightPos[2]); - float camPos[3]; - m_data->m_activeCamera->getCameraPosition(camPos); - glUniform3f(useShadow_cameraPositionIn,camPos[0],camPos[1],camPos[2]); - glUniform1f(useShadow_materialShininessIn,gfxObj->m_materialShinyNess); - - glUniformMatrix4fv(useShadow_DepthBiasModelViewMatrix, 1, false, &depthBiasMVP[0][0]); - glActiveTexture(GL_TEXTURE1); - glBindTexture(GL_TEXTURE_2D, m_data->m_shadowTexture); - glUniform1i(useShadow_shadowMap,1); - glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); - if (m_enableBlend) - { - glDisable (GL_BLEND); - } - glActiveTexture(GL_TEXTURE1); - glBindTexture(GL_TEXTURE_2D,0); - - glActiveTexture(GL_TEXTURE0); - glBindTexture(GL_TEXTURE_2D,0); - break; - } - default: - { - // b3Assert(0); - } - }; + inst.m_instanceId = curOffset+i; + inst.m_centerPosition.setValue(m_data->m_instance_positions_ptr[inst.m_instanceId*4+0], + m_data->m_instance_positions_ptr[inst.m_instanceId*4+1], + m_data->m_instance_positions_ptr[inst.m_instanceId*4+2]); + transparentInstances.push_back(inst); + } + } + curOffset+=gfxObj->m_numGraphicsInstances; + } + } + TransparentDistanceSortPredicate sorter; + float fwd[3]; + m_data->m_activeCamera->getCameraForwardVector(fwd); + sorter.m_camForwardVec.setValue(fwd[0],fwd[1],fwd[2]); + transparentInstances.quickSort(sorter); + } + + + //two passes: first for opaque instances, second for transparent ones. + for (int pass = 0; pass<2;pass++) + { + for (int i=0;im_flags&eGfxTransparency)==0); + + //transparent objects don't cast shadows (to simplify things) + if (gfxObj->m_flags&eGfxTransparency) + { + if (renderMode==B3_CREATE_SHADOWMAP_RENDERMODE) + drawThisPass = 0; + } + + if (drawThisPass && gfxObj->m_numGraphicsInstances) + { + glActiveTexture(GL_TEXTURE0); + GLuint curBindTexture = 0; + if (gfxObj->m_texturehandle) + curBindTexture = gfxObj->m_texturehandle; + else + curBindTexture = m_data->m_defaultTexturehandle; + + //disable lazy evaluation, it just leads to bugs + //if (lastBindTexture != curBindTexture) + { + glBindTexture(GL_TEXTURE_2D,curBindTexture); + } + //lastBindTexture = curBindTexture; + + b3Assert(glGetError() ==GL_NO_ERROR); + // int myOffset = gfxObj->m_instanceOffset*4*sizeof(float); + + int POSITION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4); + int ORIENTATION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4); + int COLOR_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4); + // int SCALE_BUFFER_SIZE = (totalNumInstances*sizeof(float)*3); + + glBindVertexArray(gfxObj->m_cube_vao); + + + int vertexStride = 9*sizeof(float); + PointerCaster vertex; + vertex.m_baseIndex = gfxObj->m_vertexArrayOffset*vertexStride; + + + glVertexAttribPointer(0, 4, GL_FLOAT, GL_FALSE, 9*sizeof(float), vertex.m_pointer); + glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(transparentInstances[i].m_instanceId*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes)); + glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(transparentInstances[i].m_instanceId*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE)); + + PointerCaster uv; + uv.m_baseIndex = 7*sizeof(float)+vertex.m_baseIndex; + + PointerCaster normal; + normal.m_baseIndex = 4*sizeof(float)+vertex.m_baseIndex; + + glVertexAttribPointer(3, 2, GL_FLOAT, GL_FALSE, 9*sizeof(float), uv.m_pointer); + glVertexAttribPointer(4, 3, GL_FLOAT, GL_FALSE, 9*sizeof(float), normal.m_pointer); + glVertexAttribPointer(5, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(transparentInstances[i].m_instanceId*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE)); + glVertexAttribPointer(6, 3, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(transparentInstances[i].m_instanceId*3*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE)); + + glEnableVertexAttribArray(0); + glEnableVertexAttribArray(1); + glEnableVertexAttribArray(2); + glEnableVertexAttribArray(3); + glEnableVertexAttribArray(4); + glEnableVertexAttribArray(5); + glEnableVertexAttribArray(6); + glVertexAttribDivisor(0, 0); + glVertexAttribDivisor(1, 1); + glVertexAttribDivisor(2, 1); + glVertexAttribDivisor(3, 0); + glVertexAttribDivisor(4, 0); + glVertexAttribDivisor(5, 1); + glVertexAttribDivisor(6, 1); + + + + + + + int indexCount = gfxObj->m_numIndices; + GLvoid* indexOffset = 0; + + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, gfxObj->m_index_vbo); + { + B3_PROFILE("glDrawElementsInstanced"); + + if (gfxObj->m_primitiveType==B3_GL_POINTS) + { + glUseProgram(instancingShaderPointSprite); + glUniformMatrix4fv(ProjectionMatrixPointSprite, 1, false, &m_data->m_projectionMatrix[0]); + glUniformMatrix4fv(ModelViewMatrixPointSprite, 1, false, &m_data->m_viewMatrix[0]); + glUniform1f(screenWidthPointSprite,float(m_screenWidth)); + + //glUniform1i(uniform_texture_diffusePointSprite, 0); + b3Assert(glGetError() ==GL_NO_ERROR); + glPointSize(20); + + #ifndef __APPLE__ + glEnable(GL_POINT_SPRITE_ARB); + // glTexEnvi(GL_POINT_SPRITE_ARB, GL_COORD_REPLACE_ARB, GL_TRUE); + #endif + + glEnable(GL_VERTEX_PROGRAM_POINT_SIZE); + glDrawElementsInstanced(GL_POINTS, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); + } else + { + switch (renderMode) + { + + case B3_DEFAULT_RENDERMODE: + { + if ( gfxObj->m_flags&eGfxTransparency) + { + glDepthMask(false); + glEnable (GL_BLEND); + glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + } + + glUseProgram(instancingShader); + glUniformMatrix4fv(ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]); + glUniformMatrix4fv(ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); + + b3Vector3 gLightDir = m_data->m_lightPos; + gLightDir.normalize(); + glUniform3f(regularLightDirIn,gLightDir[0],gLightDir[1],gLightDir[2]); + + glUniform1i(uniform_texture_diffuse, 0); + + if ( gfxObj->m_flags&eGfxTransparency) + { + + int instanceId = transparentInstances[i].m_instanceId; + glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes)); + glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE)); + glVertexAttribPointer(5, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE)); + glVertexAttribPointer(6, 3, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*3*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE)); + + glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0); + + } else + { + glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); + } + + if ( gfxObj->m_flags&eGfxTransparency) + { + glDisable (GL_BLEND); + glDepthMask(true); + } + + break; + } + case B3_CREATE_SHADOWMAP_RENDERMODE: + { + glUseProgram(createShadowMapInstancingShader); + glUniformMatrix4fv(createShadow_depthMVP, 1, false, &depthMVP[0][0]); + glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); + break; + } + + case B3_USE_SHADOWMAP_RENDERMODE: + { + if ( gfxObj->m_flags&eGfxTransparency) + { + glDepthMask(false); + glEnable (GL_BLEND); + glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + } + + glUseProgram(useShadowMapInstancingShader); + glUniformMatrix4fv(useShadow_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]); + glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); + glUniformMatrix4fv(useShadow_ViewMatrixInverse, 1, false, &m_data->m_viewMatrixInverse[0]); + glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); + + glUniform3f(useShadow_lightSpecularIntensity, m_data->m_lightSpecularIntensity[0],m_data->m_lightSpecularIntensity[1],m_data->m_lightSpecularIntensity[2]); + glUniform3f(useShadow_materialSpecularColor, gfxObj->m_materialSpecularColor[0],gfxObj->m_materialSpecularColor[1],gfxObj->m_materialSpecularColor[2]); + + + + float MVP[16]; + b3Matrix4x4Mul16(m_data->m_projectionMatrix,m_data->m_viewMatrix,MVP); + glUniformMatrix4fv(useShadow_MVP, 1, false, &MVP[0]); + //gLightDir.normalize(); + glUniform3f(useShadow_lightPosIn,m_data->m_lightPos[0],m_data->m_lightPos[1],m_data->m_lightPos[2]); + float camPos[3]; + m_data->m_activeCamera->getCameraPosition(camPos); + glUniform3f(useShadow_cameraPositionIn,camPos[0],camPos[1],camPos[2]); + glUniform1f(useShadow_materialShininessIn,gfxObj->m_materialShinyNess); + + glUniformMatrix4fv(useShadow_DepthBiasModelViewMatrix, 1, false, &depthBiasMVP[0][0]); + glActiveTexture(GL_TEXTURE1); + glBindTexture(GL_TEXTURE_2D, m_data->m_shadowTexture); + glUniform1i(useShadow_shadowMap,1); + + //sort transparent objects + + //gfxObj->m_instanceOffset + + if ( gfxObj->m_flags&eGfxTransparency) + { + int instanceId = transparentInstances[i].m_instanceId; + glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes)); + glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE)); + glVertexAttribPointer(5, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE)); + glVertexAttribPointer(6, 3, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*3*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE)); + glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0); + } else + { + glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); + } + + if ( gfxObj->m_flags&eGfxTransparency) + { + glDisable (GL_BLEND); + glDepthMask(true); + } + glActiveTexture(GL_TEXTURE1); + glBindTexture(GL_TEXTURE_2D,0); + + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D,0); + break; + } + default: + { + // b3Assert(0); + } + }; + } + + + //glDrawElementsInstanced(GL_LINE_LOOP, indexCount, GL_UNSIGNED_INT, (void*)indexOffset, gfxObj->m_numGraphicsInstances); } - - - //glDrawElementsInstanced(GL_LINE_LOOP, indexCount, GL_UNSIGNED_INT, (void*)indexOffset, gfxObj->m_numGraphicsInstances); } } - curOffset+= gfxObj->m_numGraphicsInstances; } { diff --git a/examples/OpenGLWindow/GLInstancingRenderer.h b/examples/OpenGLWindow/GLInstancingRenderer.h index 3716b32a6..a066197b5 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.h +++ b/examples/OpenGLWindow/GLInstancingRenderer.h @@ -39,7 +39,7 @@ class GLInstancingRenderer : public CommonRenderInterface int m_screenHeight; int m_upAxis; - bool m_enableBlend; + int registerGraphicsInstanceInternal(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling); void rebuildGraphicsInstances(); @@ -147,10 +147,7 @@ public: virtual int getTotalNumInstances() const; virtual void enableShadowMap(); - virtual void enableBlend(bool blend) - { - m_enableBlend = blend; - } + virtual void clearZBuffer(); virtual void setRenderFrameBuffer(unsigned int renderFrameBuffer); diff --git a/examples/OpenGLWindow/SimpleCamera.cpp b/examples/OpenGLWindow/SimpleCamera.cpp index 14c2fea5d..76992ef90 100644 --- a/examples/OpenGLWindow/SimpleCamera.cpp +++ b/examples/OpenGLWindow/SimpleCamera.cpp @@ -234,8 +234,15 @@ void SimpleCamera::update() b3Vector3 eyePos = b3MakeVector3(0,0,0); eyePos[forwardAxis] = -m_data->m_cameraDistance; + eyePos = b3Matrix3x3(eyeRot)*eyePos; - m_data->m_cameraForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]); + m_data->m_cameraPosition = eyePos; + + + + m_data->m_cameraPosition+= m_data->m_cameraTargetPosition; + + m_data->m_cameraForward = m_data->m_cameraTargetPosition-m_data->m_cameraPosition; if (m_data->m_cameraForward.length2() < B3_EPSILON) { m_data->m_cameraForward.setValue(1.f,0.f,0.f); @@ -243,12 +250,6 @@ void SimpleCamera::update() { m_data->m_cameraForward.normalize(); } - - eyePos = b3Matrix3x3(eyeRot)*eyePos; - - m_data->m_cameraPosition = eyePos; - m_data->m_cameraPosition+= m_data->m_cameraTargetPosition; - } void SimpleCamera::getCameraProjectionMatrix(float projectionMatrix[16]) const diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp index 715f58721..6431789b2 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp @@ -676,9 +676,6 @@ void SimpleOpenGL2Renderer::updateShape(int shapeIndex, const float* vertices) } } -void SimpleOpenGL2Renderer::enableBlend(bool blend) -{ -} void SimpleOpenGL2Renderer::clearZBuffer() { diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h b/examples/OpenGLWindow/SimpleOpenGL2Renderer.h index a438f4cd5..f57c98a03 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h +++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.h @@ -83,8 +83,7 @@ public: virtual void updateShape(int shapeIndex, const float* vertices); - virtual void enableBlend(bool blend); - + virtual void clearZBuffer(); diff --git a/examples/RenderingExamples/CoordinateSystemDemo.cpp b/examples/RenderingExamples/CoordinateSystemDemo.cpp index 148db5674..7176298ff 100644 --- a/examples/RenderingExamples/CoordinateSystemDemo.cpp +++ b/examples/RenderingExamples/CoordinateSystemDemo.cpp @@ -37,7 +37,6 @@ public: } virtual ~CoordinateSystemDemo() { - m_app->m_renderer->enableBlend(false); } diff --git a/examples/RenderingExamples/DynamicTexturedCubeDemo.cpp b/examples/RenderingExamples/DynamicTexturedCubeDemo.cpp index 44d005f18..5b2b2b6d9 100644 --- a/examples/RenderingExamples/DynamicTexturedCubeDemo.cpp +++ b/examples/RenderingExamples/DynamicTexturedCubeDemo.cpp @@ -66,7 +66,6 @@ public: virtual ~DynamicTexturedCubeDemo() { delete m_tinyVrGUI; - m_app->m_renderer->enableBlend(false); } diff --git a/examples/RenderingExamples/RenderInstancingDemo.cpp b/examples/RenderingExamples/RenderInstancingDemo.cpp index 069ac46bb..c85824dc9 100644 --- a/examples/RenderingExamples/RenderInstancingDemo.cpp +++ b/examples/RenderingExamples/RenderInstancingDemo.cpp @@ -68,7 +68,6 @@ public: } virtual ~RenderInstancingDemo() { - m_app->m_renderer->enableBlend(false); } diff --git a/examples/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp index 80584c9a8..a896552ac 100644 --- a/examples/RenderingExamples/TinyRendererSetup.cpp +++ b/examples/RenderingExamples/TinyRendererSetup.cpp @@ -154,7 +154,6 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui) m_app = gui->getAppInterface(); m_internalData = new TinyRendererSetupInternalData(gui->getAppInterface()->m_window->getWidth(),gui->getAppInterface()->m_window->getHeight()); - m_app->m_renderer->enableBlend(true); const char* fileName = "textured_sphere_smooth.obj"; fileName = "cube.obj"; @@ -225,7 +224,6 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui) TinyRendererSetup::~TinyRendererSetup() { - m_app->m_renderer->enableBlend(false); delete m_internalData; } diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 075185d61..5b9d5e437 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -45,7 +45,6 @@ public: } virtual ~GripperGraspExample() { - m_app->m_renderer->enableBlend(false); } diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 1bf00785d..99f24f7de 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -54,7 +54,6 @@ public: } virtual ~KukaGraspExample() { - m_app->m_renderer->enableBlend(false); } diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp index d5896ab4e..abe4e6ff5 100644 --- a/examples/RoboticsLearning/R2D2GraspExample.cpp +++ b/examples/RoboticsLearning/R2D2GraspExample.cpp @@ -41,7 +41,6 @@ public: } virtual ~R2D2GraspExample() { - m_app->m_renderer->enableBlend(false); } diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index de1e2e2d9..cb3e69de4 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -733,6 +733,7 @@ int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,do if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_SPHERE; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_sphereRadius = radius; command->m_createCollisionShapeArgs.m_numCollisionShapes++; @@ -753,6 +754,7 @@ int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,doubl if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_BOX; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[0] = halfExtents[0]; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[1] = halfExtents[1]; @@ -775,6 +777,7 @@ int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,d if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CAPSULE; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height; @@ -796,6 +799,7 @@ int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle, if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CYLINDER; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height; @@ -818,6 +822,7 @@ int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, do if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_PLANE; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeNormal[0] = planeNormal[0]; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeNormal[1] = planeNormal[1]; @@ -841,6 +846,7 @@ int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,cons if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; strcpy(command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshFileName,fileName); command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0]; @@ -856,6 +862,7 @@ int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,cons void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags) { + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); @@ -1942,6 +1949,20 @@ int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHa return 0; } +int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_CONSTRAINT); + b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT); + + command->m_updateFlags |=USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK; + command->m_userConstraintArguments.m_gearAuxLink = gearAuxLink; + + return 0; +} + + b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index efa45d495..da7bdac42 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -107,7 +107,7 @@ int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHan int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]); int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce); int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio); - +int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink); b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index cbddd1ddc..1c07479cc 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -14,7 +14,7 @@ #include "../Importers/ImportURDFDemo/UrdfParser.h" #include "../Utils/b3ResourcePath.h" #include "Bullet3Common/b3FileUtils.h" - +#include "../OpenGLWindow/GLInstanceGraphicsShape.h" #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" @@ -3704,50 +3704,102 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (out_type==UrdfGeometry::FILE_OBJ) { - std::vector shapes; - std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str()); //create a convex hull for each shape, and store it in a btCompoundShape - //shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); - //static btCollisionShape* createConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale) - B3_PROFILE("createConvexHullFromShapes"); - if (compound==0) + if (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH) { - compound = worldImporter->createCompoundShape(); - } - compound->setMargin(defaultCollisionMargin); - - for (int s = 0; s<(int)shapes.size(); s++) - { - btConvexHullShape* convexHull = worldImporter->createConvexHullShape(); - convexHull->setMargin(defaultCollisionMargin); - tinyobj::shape_t& shape = shapes[s]; - int faceCount = shape.mesh.indices.size(); - - for (int f = 0; fm_numvertices<=0) { + b3Warning("%s: cannot extract mesh from '%s'\n", pathPrefix, relativeFileName); + delete glmesh; + break; + } + btAlignedObjectArray convertedVerts; + convertedVerts.reserve(glmesh->m_numvertices); + btVector3 meshScale(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[0], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[1], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[2]); - btVector3 pt; - pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]); - - convexHull->addPoint(pt*meshScale,false); - - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); - convexHull->addPoint(pt*meshScale, false); - - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); - convexHull->addPoint(pt*meshScale, false); + for (int i=0; im_numvertices; i++) + { + convertedVerts.push_back(btVector3( + glmesh->m_vertices->at(i).xyzw[0]*meshScale[0], + glmesh->m_vertices->at(i).xyzw[1]*meshScale[1], + glmesh->m_vertices->at(i).xyzw[2]*meshScale[2])); } - convexHull->recalcLocalAabb(); - convexHull->optimizeConvexHull(); - compound->addChildShape(childTransform,convexHull); + BT_PROFILE("convert trimesh"); + btTriangleMesh* meshInterface = new btTriangleMesh(); + { + BT_PROFILE("convert vertices"); + + for (int i=0; im_numIndices/3; i++) + { + 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); + } + } + { + BT_PROFILE("create btBvhTriangleMeshShape"); + btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); + //trimesh->setLocalScaling(collision->m_geometry.m_meshScale); + shape = trimesh; + if (compound) + { + compound->addChildShape(childTransform,shape); + } + } + } else + { + + std::vector shapes; + std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str()); + + //shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); + //static btCollisionShape* createConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale) + B3_PROFILE("createConvexHullFromShapes"); + if (compound==0) + { + compound = worldImporter->createCompoundShape(); + } + compound->setMargin(defaultCollisionMargin); + + for (int s = 0; s<(int)shapes.size(); s++) + { + btConvexHullShape* convexHull = worldImporter->createConvexHullShape(); + convexHull->setMargin(defaultCollisionMargin); + tinyobj::shape_t& shape = shapes[s]; + int faceCount = shape.mesh.indices.size(); + + for (int f = 0; faddPoint(pt*meshScale,false); + + pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], + shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], + shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); + convexHull->addPoint(pt*meshScale, false); + + pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], + shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], + shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); + convexHull->addPoint(pt*meshScale, false); + } + + convexHull->recalcLocalAabb(); + convexHull->optimizeConvexHull(); + compound->addChildShape(childTransform,convexHull); + } } } } @@ -6398,6 +6450,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio); } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK) + { + userConstraintPtr->m_mbConstraint->setGearAuxLink(clientCmd.m_userConstraintArguments.m_gearAuxLink); + } } if (userConstraintPtr->m_rbConstraint) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 9658d6e6c..103dd5f96 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -645,6 +645,8 @@ enum EnumUserConstraintFlags USER_CONSTRAINT_CHANGE_MAX_FORCE=32, USER_CONSTRAINT_REQUEST_INFO=64, USER_CONSTRAINT_CHANGE_GEAR_RATIO=128, + USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK=256, + }; enum EnumBodyChangeFlags diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 882db7a24..eaf3de675 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -235,6 +235,7 @@ struct b3UserConstraint double m_maxAppliedForce; int m_userConstraintUniqueId; double m_gearRatio; + int m_gearAuxLink; }; diff --git a/examples/Tutorial/Tutorial.cpp b/examples/Tutorial/Tutorial.cpp index f59209a7c..8d43daac2 100644 --- a/examples/Tutorial/Tutorial.cpp +++ b/examples/Tutorial/Tutorial.cpp @@ -300,7 +300,6 @@ public: int numBodies = 1; m_app->setUpAxis(1); - m_app->m_renderer->enableBlend(true); switch (m_tutorialIndex) { @@ -405,7 +404,7 @@ public: { int width,height,n; - const char* filename = "data/cube.png"; + const char* filename = "data/checker_huge.gif"; const unsigned char* image=0; const char* prefix[]={"./","../","../../","../../../","../../../../"}; @@ -426,15 +425,23 @@ public: } // int boxId = m_app->registerCubeShape(1,1,1,textureIndex); - int boxId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_HIGH, textureIndex); - b3Vector4 color = b3MakeVector4(1,1,1,0.8); + int sphereTransparent = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_HIGH, textureIndex); + int sphereOpaque= m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_HIGH, textureIndex); + b3Vector3 scaling = b3MakeVector3(SPHERE_RADIUS,SPHERE_RADIUS,SPHERE_RADIUS); for (int i=0;im_collisionShape.m_sphere.m_radius = SPHERE_RADIUS; m_bodies[i]->m_collisionShape.m_type = LW_SPHERE_TYPE; - m_bodies[i]->m_graphicsIndex = m_app->m_renderer->registerGraphicsInstance(boxId,m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation,color,scaling); + m_bodies[i]->m_graphicsIndex = m_app->m_renderer->registerGraphicsInstance(gfxShape,m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation,color,scaling); m_app->m_renderer->writeSingleInstanceTransformToCPU(m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation, m_bodies[i]->m_graphicsIndex); } } @@ -467,7 +474,6 @@ public: m_timeSeriesCanvas0 = 0; m_timeSeriesCanvas1 = 0; - m_app->m_renderer->enableBlend(false); } diff --git a/examples/pybullet/examples/createMultiBodyLinks.py b/examples/pybullet/examples/createMultiBodyLinks.py index 9c776cab6..bdb7f6739 100644 --- a/examples/pybullet/examples/createMultiBodyLinks.py +++ b/examples/pybullet/examples/createMultiBodyLinks.py @@ -49,5 +49,8 @@ for i in range (p.getNumJoints(sphereUid)): p.getJointInfo(sphereUid,i) while (1): + keys = p.getKeyboardEvents() + print(keys) + time.sleep(0.01) \ No newline at end of file diff --git a/examples/pybullet/examples/createSphereMultiBodies.py b/examples/pybullet/examples/createSphereMultiBodies.py index fb24febb1..f2b0fb59e 100644 --- a/examples/pybullet/examples/createSphereMultiBodies.py +++ b/examples/pybullet/examples/createSphereMultiBodies.py @@ -4,7 +4,11 @@ import time useMaximalCoordinates = 0 p.connect(p.GUI) -p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates) +#p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates) +monastryId = concaveEnv =p.createCollisionShape(p.GEOM_MESH,fileName="samurai_monastry.obj",flags=p.GEOM_FORCE_CONCAVE_TRIMESH) +orn = p.getQuaternionFromEuler([1.5707963,0,0]) +p.createMultiBody (0,monastryId, baseOrientation=orn) + sphereRadius = 0.05 colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius]) @@ -28,5 +32,7 @@ p.setGravity(0,0,-10) p.setRealTimeSimulation(1) while (1): + keys = p.getKeyboardEvents() + #print(keys) time.sleep(0.01) \ No newline at end of file diff --git a/examples/pybullet/examples/racecar_differential.py b/examples/pybullet/examples/racecar_differential.py index 251ce0141..9abab35df 100644 --- a/examples/pybullet/examples/racecar_differential.py +++ b/examples/pybullet/examples/racecar_differential.py @@ -7,16 +7,15 @@ if (cid<0): p.resetSimulation() p.setGravity(0,0,-10) -p.setPhysicsEngineParameter(numSolverIterations=1000) useRealTimeSim = 1 #for video recording (works best on Mac and Linux, not well on Windows) #p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4") p.setRealTimeSimulation(useRealTimeSim) # either this -#p.loadURDF("plane.urdf") -p.loadSDF("stadium.sdf") +p.loadURDF("plane.urdf") +#p.loadSDF("stadium.sdf") -car = p.loadURDF("racecar/racecar_differential.urdf")#, [0,0,2],useFixedBase=True) +car = p.loadURDF("racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True) for i in range (p.getNumJoints(car)): print (p.getJointInfo(car,i)) for wheel in range(p.getNumJoints(car)): @@ -27,7 +26,6 @@ wheels = [8,15] print("----------------") #p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10) - c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c,gearRatio=1, maxForce=10000) @@ -40,12 +38,17 @@ p.changeConstraint(c,gearRatio=-1, maxForce=10000) c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c,gearRatio=1, maxForce=10000) -c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) -p.changeConstraint(c,gearRatio=-1, maxForce=10000) c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c,gearRatio=-1, maxForce=10000) +c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, maxForce=10000) + +c = p.createConstraint(car,1,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15, maxForce=10000) +c = p.createConstraint(car,3,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15,maxForce=10000) steering = [0,2] diff --git a/examples/pybullet/examples/transparent.py b/examples/pybullet/examples/transparent.py new file mode 100644 index 000000000..5c6cc09c6 --- /dev/null +++ b/examples/pybullet/examples/transparent.py @@ -0,0 +1,18 @@ +import pybullet as p +import time +p.connect(p.GUI) +p.loadURDF("plane.urdf") +sphereUid = p.loadURDF("sphere_transparent.urdf",[0,0,2]) + +redSlider = p.addUserDebugParameter("red",0,1,1) +greenSlider = p.addUserDebugParameter("green",0,1,0) +blueSlider = p.addUserDebugParameter("blue",0,1,0) +alphaSlider = p.addUserDebugParameter("alpha",0,1,0.5) + +while (1): + red = p.readUserDebugParameter(redSlider) + green = p.readUserDebugParameter(greenSlider) + blue = p.readUserDebugParameter(blueSlider) + alpha = p.readUserDebugParameter(alphaSlider) + p.changeVisualShape(sphereUid,-1,rgbaColor=[red,green,blue,alpha]) + time.sleep(0.01) \ No newline at end of file diff --git a/examples/pybullet/examples/vr_racecar_differential.py b/examples/pybullet/examples/vr_racecar_differential.py new file mode 100644 index 000000000..5cdb8dc95 --- /dev/null +++ b/examples/pybullet/examples/vr_racecar_differential.py @@ -0,0 +1,93 @@ +import pybullet as p +import time +CONTROLLER_ID = 0 +POSITION=1 +ORIENTATION=2 +BUTTONS=6 + + +cid = p.connect(p.SHARED_MEMORY) +if (cid<0): + p.connect(p.GUI) + +p.resetSimulation() +p.setGravity(0,0,-10) +useRealTimeSim = 1 + +#for video recording (works best on Mac and Linux, not well on Windows) +#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4") +p.setRealTimeSimulation(useRealTimeSim) # either this +p.loadURDF("plane.urdf") +#p.loadSDF("stadium.sdf") + +car = p.loadURDF("racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True) +for i in range (p.getNumJoints(car)): + print (p.getJointInfo(car,i)) +for wheel in range(p.getNumJoints(car)): + p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0) + p.getJointInfo(car,wheel) + +wheels = [8,15] +print("----------------") + +#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10) +c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=1, maxForce=10000) + +c = p.createConstraint(car,10,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, maxForce=10000) + +c = p.createConstraint(car,9,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, maxForce=10000) + +c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=1, maxForce=10000) + + +c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, maxForce=10000) + +c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, maxForce=10000) + +c = p.createConstraint(car,1,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15, maxForce=10000) +c = p.createConstraint(car,3,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15,maxForce=10000) + + +steering = [0,2] + +targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-50,50,0) +maxForceSlider = p.addUserDebugParameter("maxForce",0,50,20) +steeringSlider = p.addUserDebugParameter("steering",-1,1,0) +activeController = -1 + +while (True): + + + maxForce = p.readUserDebugParameter(maxForceSlider) + targetVelocity = p.readUserDebugParameter(targetVelocitySlider) + steeringAngle = p.readUserDebugParameter(steeringSlider) + #print(targetVelocity) + events = p.getVREvents() + for e in events: + if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED): + activeController = e[CONTROLLER_ID] + if (activeController == e[CONTROLLER_ID]): + orn = e[2] + eul = p.getEulerFromQuaternion(orn) + steeringAngle=eul[0] + + targetVelocity = 20.0*e[3] + + for wheel in wheels: + p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=maxForce) + + for steer in steering: + p.setJointMotorControl2(car,steer,p.POSITION_CONTROL,targetPosition=-steeringAngle) + + steering + if (useRealTimeSim==0): + p.stepSimulation() + time.sleep(0.01) diff --git a/examples/pybullet/gym/envs/bullet/cartpole_bullet.py b/examples/pybullet/gym/envs/bullet/cartpole_bullet.py index f117e8f71..3e7f29daf 100644 --- a/examples/pybullet/gym/envs/bullet/cartpole_bullet.py +++ b/examples/pybullet/gym/envs/bullet/cartpole_bullet.py @@ -22,10 +22,14 @@ class CartPoleBulletEnv(gym.Env): 'video.frames_per_second' : 50 } - def __init__(self): + def __init__(self, renders=True): # start the bullet physics server - p.connect(p.GUI) - #p.connect(p.DIRECT) + self._renders = renders + if (renders): + p.connect(p.GUI) + else: + p.connect(p.DIRECT) + observation_high = np.array([ np.finfo(np.float32).max, np.finfo(np.float32).max, @@ -33,7 +37,7 @@ class CartPoleBulletEnv(gym.Env): np.finfo(np.float32).max]) action_high = np.array([0.1]) - self.action_space = spaces.Discrete(5) + self.action_space = spaces.Discrete(9) self.observation_space = spaces.Box(-observation_high, observation_high) self.theta_threshold_radians = 1 @@ -56,8 +60,8 @@ class CartPoleBulletEnv(gym.Env): self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] theta, theta_dot, x, x_dot = self.state - dv = 0.4 - deltav = [-2.*dv, -dv, 0, dv, 2.*dv][action] + dv = 0.1 + deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action] p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3])) diff --git a/examples/pybullet/gym/train_pybullet_cartpole.py b/examples/pybullet/gym/train_pybullet_cartpole.py index 353fa4a86..4b7f4839e 100644 --- a/examples/pybullet/gym/train_pybullet_cartpole.py +++ b/examples/pybullet/gym/train_pybullet_cartpole.py @@ -12,7 +12,7 @@ def callback(lcl, glb): def main(): - env = gym.make('CartPoleBulletEnv-v0') + env = CartPoleBulletEnv(renders=False) model = deepq.models.mlp([64]) act = deepq.learn( env, diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e7b83649c..882de968c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4804,11 +4804,12 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject* keywds) { - static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "physicsClientId", NULL}; + static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "gearAuxLink", "physicsClientId", NULL}; int userConstraintUniqueId = -1; b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; int statusType; + int gearAuxLink = -1; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; PyObject* jointChildPivotObj = 0; @@ -4817,7 +4818,7 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P double jointChildFrameOrn[4]; double maxForce = -1; double gearRatio = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddii", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &gearAuxLink, &physicsClientId)) { return NULL; } @@ -4847,6 +4848,10 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P { b3InitChangeUserConstraintSetGearRatio(commandHandle,gearRatio); } + if (gearAuxLink>=0) + { + b3InitChangeUserConstraintSetGearAuxLink(commandHandle,gearAuxLink); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); Py_INCREF(Py_None); @@ -5008,7 +5013,7 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P pybullet_internalSetVectord(planeNormalObj,planeNormal); shapeIndex = b3CreateCollisionShapeAddPlane(commandHandle, planeNormal, planeConstant); } - if (shapeIndex && flags) + if (shapeIndex>=0 && flags) { b3CreateCollisionSetFlag(commandHandle,shapeIndex,flags); } @@ -5180,14 +5185,16 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje double linkJointAxis[3]; double linkInertialFramePosition[3]; double linkInertialFrameOrientation[4]; - + int linkParentIndex; + int linkJointType; + pybullet_internalGetVector3FromSequence(seqLinkInertialFramePositions,i,linkInertialFramePosition); pybullet_internalGetVector4FromSequence(linkInertialFrameOrientationObj,i,linkInertialFrameOrientation); pybullet_internalGetVector3FromSequence(seqLinkPositions,i,linkPosition); pybullet_internalGetVector4FromSequence(seqLinkOrientations,i,linkOrientation); pybullet_internalGetVector3FromSequence(seqLinkJoinAxis,i,linkJointAxis); - int linkParentIndex = pybullet_internalGetIntFromSequence(seqLinkParentIndices,i); - int linkJointType = pybullet_internalGetIntFromSequence(seqLinkJointTypes,i); + linkParentIndex = pybullet_internalGetIntFromSequence(seqLinkParentIndices,i); + linkJointType = pybullet_internalGetIntFromSequence(seqLinkJointTypes,i); b3CreateMultiBodyLink(commandHandle, linkMass, @@ -7275,6 +7282,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "GEOM_PLANE", GEOM_PLANE); PyModule_AddIntConstant(m, "GEOM_CAPSULE", GEOM_CAPSULE); + PyModule_AddIntConstant(m, "GEOM_FORCE_CONCAVE_TRIMESH", GEOM_FORCE_CONCAVE_TRIMESH); + SpamError = PyErr_NewException("pybullet.error", NULL, NULL); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 27e1ccc05..8c28bbf4c 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -184,6 +184,9 @@ public: virtual void debugDraw(class btIDebugDraw* drawer)=0; virtual void setGearRatio(btScalar ratio) {} + virtual void setGearAuxLink(int gearAuxLink) {} + + }; #endif //BT_MULTIBODY_CONSTRAINT_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp index 2e242b1f7..3fdd51815 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp @@ -22,7 +22,8 @@ subject to the following restrictions: btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,1,false), - m_gearRatio(1) + m_gearRatio(1), + m_gearAuxLink(-1) { } @@ -121,11 +122,18 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& int dof = 0; btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; - + btScalar auxVel = 0; + + if (m_gearAuxLink>=0) + { + auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof]; + } + currentVelocity += auxVel; + //btScalar positionStabiliationTerm = erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; //btScalar velocityError = (m_desiredVelocity - currentVelocity); - btScalar desiredRelativeVelocity = 0; + btScalar desiredRelativeVelocity = auxVel; fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h index 947166ae7..711a73e46 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h @@ -31,10 +31,11 @@ protected: btMatrix3x3 m_frameInA; btMatrix3x3 m_frameInB; btScalar m_gearRatio; + int m_gearAuxLink; public: - btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); + //btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); virtual ~btMultiBodyGearConstraint(); @@ -97,6 +98,10 @@ public: { m_gearRatio = gearRatio; } + virtual void setGearAuxLink(int gearAuxLink) + { + m_gearAuxLink = gearAuxLink; + } };