From 2f2f070ab52a66d06267801f1ae437ca16081336 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sun, 8 Oct 2017 11:34:42 -0700 Subject: [PATCH] allow changeVisualShape (rgbaColor, specularColor and texture) for maximal coordinates rigid body (btRigidBody) Make the examples\pybullet\examples\createVisualShape.py a bit more interesting --- examples/ExampleBrowser/OpenGLGuiHelper.cpp | 8 ++-- .../PhysicsServerCommandProcessor.cpp | 22 ++++++++- .../SharedMemory/PhysicsServerExample.cpp | 2 +- .../pybullet/examples/createVisualShape.py | 47 +++++++++++++++++-- 4 files changed, 68 insertions(+), 11 deletions(-) diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index a5ccc8654..2559d3d3c 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -1019,12 +1019,10 @@ bool OpenGLGuiHelper::getCameraInfo(int* width, int* height, float viewMatrix[16 getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix); getRenderInterface()->getActiveCamera()->getCameraUpVector(camUp); getRenderInterface()->getActiveCamera()->getCameraForwardVector(camForward); - float frustumNearPlane = getRenderInterface()->getActiveCamera()->getCameraFrustumNear(); - float frustumFarPlane = getRenderInterface()->getActiveCamera()->getCameraFrustumFar(); - + float top = 1.f; float bottom = -1.f; - float tanFov = (top-bottom)*0.5f / frustumNearPlane; + float tanFov = (top-bottom)*0.5f / 1; float fov = btScalar(2.0) * btAtan(tanFov); btVector3 camPos,camTarget; getRenderInterface()->getActiveCamera()->getCameraPosition(camPos); @@ -1046,7 +1044,7 @@ bool OpenGLGuiHelper::getCameraInfo(int* width, int* height, float viewMatrix[16 float tanfov = tanf(0.5f*fov); hori *= 2.f * farPlane * tanfov; vertical *= 2.f * farPlane * tanfov; - btScalar aspect = *width / *height; + btScalar aspect = float(*width) / float(*height); hori*=aspect; //compute 'hor' and 'vert' vectors, useful to generate raytracer rays hor[0] = hori[0]; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index a557b5102..258e1e54d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -7890,7 +7890,27 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } else { - //todo: change color for rigid body + if (bodyHandle->m_rigidBody) + { + int graphicsIndex = bodyHandle->m_rigidBody->getUserIndex(); + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) + { + if (texHandle) + { + int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex); + m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId); + } + } + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) + { + m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + } + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) + { + m_data->m_guiHelper->changeSpecularColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_specularColor); + } + } } } } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 290192139..3ab943e3c 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -2712,7 +2712,7 @@ btVector3 PhysicsServerExample::getRayTo(int x,int y) btAssert(0); return btVector3(0,0,0); } - + float top = 1.f; float bottom = -1.f; float nearPlane = 1.f; diff --git a/examples/pybullet/examples/createVisualShape.py b/examples/pybullet/examples/createVisualShape.py index 75c89ece7..3dd212832 100644 --- a/examples/pybullet/examples/createVisualShape.py +++ b/examples/pybullet/examples/createVisualShape.py @@ -1,5 +1,25 @@ import pybullet as p import time +import math + +def getRayFromTo(mouseX,mouseY): + width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera() + camPos = [camTarget[0] - dist*camForward[0],camTarget[1] - dist*camForward[1],camTarget[2] - dist*camForward[2]] + farPlane = 10000 + rayForward = [(camTarget[0]-camPos[0]),(camTarget[1]-camPos[1]),(camTarget[2]-camPos[2])] + invLen = farPlane*1./(math.sqrt(rayForward[0]*rayForward[0]+rayForward[1]*rayForward[1]+rayForward[2]*rayForward[2])) + rayForward = [invLen*rayForward[0],invLen*rayForward[1],invLen*rayForward[2]] + rayFrom = camPos + oneOverWidth = float(1)/float(width) + oneOverHeight = float(1)/float(height) + dHor = [horizon[0] * oneOverWidth,horizon[1] * oneOverWidth,horizon[2] * oneOverWidth] + dVer = [vertical[0] * oneOverHeight,vertical[1] * oneOverHeight,vertical[2] * oneOverHeight] + rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]] + rayTo = [rayFrom[0]+rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0], + rayFrom[1]+rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1], + rayFrom[2]+rayForward[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]] + return rayFrom,rayTo + p.connect(p.GUI) p.setPhysicsEngineParameter(numSolverIterations=10) p.setTimeStep(1./120.) @@ -18,8 +38,8 @@ meshScale=[0.1,0.1,0.1] visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="duck.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale) collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale) -rangex = 40 -rangey = 40 +rangex = 32 +rangey = 32 for i in range (rangex): for j in range (rangey ): p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*2,1], useMaximalCoordinates=True) @@ -27,6 +47,25 @@ p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) p.stopStateLogging(logId) p.setGravity(0,0,-10) p.setRealTimeSimulation(1) + +colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,1]] +currentColor = 0 + while (1): - p.setGravity(0,0,-10) - time.sleep(1) + mouseEvents = p.getMouseEvents() + for e in mouseEvents: + if ((e[0] == 2) and (e[3]==0) and (e[4]& p.KEY_WAS_TRIGGERED)): + mouseX = e[1] + mouseY = e[2] + rayFrom,rayTo=getRayFromTo(mouseX,mouseY) + rayInfo = p.rayTest(rayFrom,rayTo) + #p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3) + for l in range(len(rayInfo)): + hit = rayInfo[l] + objectUid = hit[0] + if (objectUid>=0): + #p.removeBody(objectUid) + p.changeVisualShape(objectUid,-1,rgbaColor=colors[currentColor]) + currentColor+=1 + if (currentColor>=len(colors)): + currentColor=0