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..a1eb298e0 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) { @@ -2218,8 +2245,9 @@ b3Assert(glGetError() ==GL_NO_ERROR); case B3_USE_SHADOWMAP_RENDERMODE: { - if (m_enableBlend) + if ( gfxObj->m_flags&eGfxTransparency) { + glDepthMask(false); glEnable (GL_BLEND); glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); } @@ -2249,10 +2277,55 @@ b3Assert(glGetError() ==GL_NO_ERROR); 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) + + //sort transparent objects + + //gfxObj->m_instanceOffset + + if ( gfxObj->m_flags&eGfxTransparency) + { + b3AlignedObjectArray transparentInstances; + transparentInstances.reserve(gfxObj->m_numGraphicsInstances); + + for (int i=0;im_numGraphicsInstances;i++) + { + SortableTransparentInstance inst; + + inst.m_shapeIndex = -1; + + 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); + + } + TransparentDistanceSortPredicate sorter; + float fwd[3]; + m_data->m_activeCamera->getCameraForwardVector(fwd); + sorter.m_camForwardVec.setValue(fwd[0],fwd[1],fwd[2]); + transparentInstances.quickSort(sorter); + + + for (int i=0;im_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); 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/Tutorial/Tutorial.cpp b/examples/Tutorial/Tutorial.cpp index f59209a7c..18a9f34eb 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[]={"./","../","../../","../../../","../../../../"}; @@ -427,10 +426,16 @@ 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); + + 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; @@ -467,7 +472,6 @@ public: m_timeSeriesCanvas0 = 0; m_timeSeriesCanvas1 = 0; - m_app->m_renderer->enableBlend(false); } 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)