From db008ab3c215f4550cfe6914f331bd87626d1edf Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 23 May 2017 22:05:26 -0700 Subject: [PATCH] Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link. example: kuka = p.loadURDF("kuka_iiwa/model.urdf") p.getNumJoints(kuka) pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6) pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6) Also allow to render text using a given orientation (instead of pointing to the camera), example: pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6) Add drawTexturedTriangleMesh, for drawing 3d text. Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index. updateTexture: allow to not flip texels around up axis --- .../CommonGUIHelperInterface.h | 17 +- .../CommonGraphicsAppInterface.h | 23 +- .../CommonInterfaces/CommonRenderInterface.h | 18 +- examples/ExampleBrowser/OpenGLGuiHelper.cpp | 9 + examples/ExampleBrowser/OpenGLGuiHelper.h | 6 +- .../OpenGLWindow/GLInstancingRenderer.cpp | 461 ++++++++++++------ examples/OpenGLWindow/GLInstancingRenderer.h | 9 +- examples/OpenGLWindow/GLPrimitiveRenderer.cpp | 2 + examples/OpenGLWindow/SimpleOpenGL2App.cpp | 10 +- examples/OpenGLWindow/SimpleOpenGL2App.h | 5 +- .../OpenGLWindow/SimpleOpenGL2Renderer.cpp | 50 +- examples/OpenGLWindow/SimpleOpenGL2Renderer.h | 11 +- examples/OpenGLWindow/SimpleOpenGL3App.cpp | 300 ++++++++++-- examples/OpenGLWindow/SimpleOpenGL3App.h | 4 +- examples/OpenGLWindow/fontstash.cpp | 144 +++++- examples/OpenGLWindow/fontstash.h | 24 +- .../opengl_fontstashcallbacks.cpp | 12 +- .../OpenGLWindow/opengl_fontstashcallbacks.h | 4 + examples/SharedMemory/PhysicsClientC_API.cpp | 46 ++ examples/SharedMemory/PhysicsClientC_API.h | 6 + .../PhysicsClientSharedMemory.cpp | 10 +- .../PhysicsServerCommandProcessor.cpp | 62 ++- .../PhysicsServerCommandProcessor.h | 4 +- .../SharedMemory/PhysicsServerExample.cpp | 135 ++++- .../PhysicsServerSharedMemory.cpp | 7 +- .../SharedMemory/PhysicsServerSharedMemory.h | 1 + examples/SharedMemory/SharedMemoryCommands.h | 8 + examples/SharedMemory/SharedMemoryPublic.h | 10 +- examples/SimpleOpenGL3/main.cpp | 277 ++--------- examples/pybullet/pybullet.c | 37 +- 30 files changed, 1195 insertions(+), 517 deletions(-) diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index efc8ab295..a5121548e 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -84,12 +84,11 @@ struct GUIHelperInterface virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) =0; - virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)=0; - - + virtual void drawText3D( const char* txt, float posX, float posY, float posZ, float size)=0; + virtual void drawText3D( const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag)=0; - virtual int addUserDebugText3D( const char* txt, const double posisionXYZ[3], const double textColorRGB[3], double size, double lifeTime){return -1;}; - virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime ){return -1;}; + virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double orientation[4], const double textColorRGB[3], double size, double lifeTime, int trackingVisualShapeIndex, int optionFlags){return -1;} + virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex){return -1;}; virtual int addUserDebugParameter(const char* txt, double rangeMin, double rangeMax, double startValue){return -1;}; virtual int readUserDebugParameter(int itemUniqueId, double* value) { return 0;} @@ -174,12 +173,12 @@ struct DummyGUIHelper : public GUIHelperInterface virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size) { } - - virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double textColorRGB[3], double size, double lifeTime) + + virtual void drawText3D( const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag) { - return -1; } - virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime ) + + virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex) { return -1; } diff --git a/examples/CommonInterfaces/CommonGraphicsAppInterface.h b/examples/CommonInterfaces/CommonGraphicsAppInterface.h index 31f0f51ce..12f555a7a 100644 --- a/examples/CommonInterfaces/CommonGraphicsAppInterface.h +++ b/examples/CommonInterfaces/CommonGraphicsAppInterface.h @@ -37,6 +37,12 @@ enum EnumSphereLevelOfDetail }; struct CommonGraphicsApp { + enum drawText3DOption + { + eDrawText3D_OrtogonalFaceCamera=1, + eDrawText3D_TrueType=2, + eDrawText3D_TrackObject=4, + }; class CommonWindowInterface* m_window; struct CommonRenderInterface* m_renderer; struct CommonParameterInterface* m_parameterInterface; @@ -120,8 +126,21 @@ struct CommonGraphicsApp virtual int getUpAxis() const = 0; virtual void swapBuffer() = 0; - virtual void drawText( const char* txt, int posX, int posY, float size = 1.0f) = 0; - virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)=0; + virtual void drawText( const char* txt, int posX, int posY) + { + float size=1; + float colorRGBA[4]={1,1,1,1}; + drawText(txt,posX,posY, size, colorRGBA); + } + + virtual void drawText( const char* txt, int posX, int posY, float size) + { + float colorRGBA[4]={1,1,1,1}; + drawText(txt,posX,posY,size,colorRGBA); + } + virtual void drawText( const char* txt, int posX, int posY, float size, float colorRGBA[4]) = 0; + virtual void drawText3D( const char* txt, float posX, float posY, float posZ, float size)=0; + virtual void drawText3D( const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag)=0; virtual void drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA)=0; virtual int registerCubeShape(float halfExtentsX,float halfExtentsY, float halfExtentsZ, int textureIndex = -1, float textureScaling = 1)=0; virtual int registerGraphicsUnitSphereShape(EnumSphereLevelOfDetail lod, int textureId=-1) = 0; diff --git a/examples/CommonInterfaces/CommonRenderInterface.h b/examples/CommonInterfaces/CommonRenderInterface.h index 4af517782..986347061 100644 --- a/examples/CommonInterfaces/CommonRenderInterface.h +++ b/examples/CommonInterfaces/CommonRenderInterface.h @@ -17,6 +17,14 @@ enum B3_USE_SHADOWMAP_RENDERMODE, }; +struct CommonGfxVertex3D +{ + float x,y,z,w; + float nx,ny,nz; + float u,v; +}; + + struct CommonRenderInterface { virtual ~CommonRenderInterface() {} @@ -46,13 +54,17 @@ struct CommonRenderInterface virtual void drawLine(const double from[4], const double to[4], const double color[4], double lineWidth) = 0; virtual void drawPoint(const float* position, const float color[4], float pointDrawSize)=0; virtual void drawPoint(const double* position, const double color[4], double pointDrawSize)=0; + virtual void drawTexturedTriangleMesh(float worldPosition[3], float worldOrientation[4], const float* vertices, int numvertices, const unsigned int* indices, int numIndices, float color[4], int textureIndex=-1)=0; + virtual int registerShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType=B3_GL_TRIANGLES, int textureIndex=-1)=0; virtual void updateShape(int shapeIndex, const float* vertices)=0; - virtual int registerTexture(const unsigned char* texels, int width, int height)=0; - virtual void updateTexture(int textureIndex, const unsigned char* texels)=0; + virtual int registerTexture(const unsigned char* texels, int width, int height, bool flipPixelsY=true)=0; + virtual void updateTexture(int textureIndex, const unsigned char* texels, bool flipPixelsY=true)=0; virtual void activateTexture(int textureIndex)=0; - + + virtual bool readSingleInstanceTransformToCPU(float* position, float* orientation, int srcIndex)=0; + virtual void writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex)=0; virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex)=0; virtual void writeSingleInstanceColorToCPU(const float* color, int srcIndex)=0; diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index 634004d84..105ab64a6 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -1194,6 +1194,15 @@ void OpenGLGuiHelper::autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWor } } +void OpenGLGuiHelper::drawText3D( const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlags) +{ + B3_PROFILE("OpenGLGuiHelper::drawText3D"); + + btAssert(m_data->m_glApp); + m_data->m_glApp->drawText3D(txt,position, orientation, color,size, optionFlags); + +} + void OpenGLGuiHelper::drawText3D( const char* txt, float posX, float posY, float posZ, float size) { B3_PROFILE("OpenGLGuiHelper::drawText3D"); diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index 8a0dd67dc..182027daa 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -57,8 +57,10 @@ struct OpenGLGuiHelper : public GUIHelperInterface int destinationHeight, int* numPixelsCopied); virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) ; - - virtual void drawText3D( const char* txt, float posX, float posY, float posZ, float size); + + virtual void drawText3D( const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag); + + virtual void drawText3D( const char* txt, float posX, float posY, float posZ, float size); virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double textColorRGB[3], double size, double lifeTime) { diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index 8974cf24a..e60b94e9f 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -52,6 +52,7 @@ float shadowMapWorldSize=10; #include "Bullet3Common/b3Vector3.h" #include "Bullet3Common/b3Quaternion.h" +#include "Bullet3Common/b3Transform.h" #include "Bullet3Common/b3Matrix3x3.h" #include "Bullet3Common/b3ResizablePool.h" @@ -74,6 +75,36 @@ float shadowMapWorldSize=10; #include "GLRenderToTexture.h" +static const char* triangleVertexShaderText = +"#version 330\n" +"precision highp float;" +"uniform mat4 MVP;\n" +"uniform vec3 vCol;\n" +"attribute vec4 vPos;\n" +"attribute vec2 vUV;\n" +"out vec3 clr;\n" +"out vec2 uv0;\n" +"void main()\n" +"{\n" +" gl_Position = MVP * vPos;\n" +" clr = vCol;\n" +" uv0 = vUV;\n" +"}\n"; + + +static const char* triangleFragmentShader = +"#version 330\n" +"precision highp float;" +"in vec3 clr;\n" +"in vec2 uv0;" +"out vec4 color;" +"uniform sampler2D Diffuse;" +"void main()\n" +"{\n" +" vec4 texel = texture(Diffuse,uv0);\n" +" color = vec4(clr,texel.r)*texel;\n" +"}\n"; + //#include "../../opencl/gpu_rigidbody_pipeline/b3GpuNarrowphaseAndSolver.h"//for m_maxNumObjectCapacity @@ -213,6 +244,14 @@ struct GLInstanceRendererInternalData* GLInstancingRenderer::getInternalData() } +static GLuint triangleShaderProgram; +static GLint triangle_mvp_location=-1; +static GLint triangle_vpos_location=-1; +static GLint triangle_vUV_location=-1; +static GLint triangle_vcol_location=-1; +static GLuint triangleIndexVbo=0; + + static GLuint linesShader; // The line renderer static GLuint useShadowMapInstancingShader; // The shadow instancing renderer static GLuint createShadowMapInstancingShader; // The shadow instancing renderer @@ -333,7 +372,29 @@ GLInstancingRenderer::~GLInstancingRenderer() +bool GLInstancingRenderer::readSingleInstanceTransformToCPU(float* position, float* orientation, int shapeIndex) +{ + b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(shapeIndex); + if (pg) + { + int srcIndex = pg->m_internalInstanceIndex; + if ((srcIndexm_totalNumInstances) && (srcIndex>=0)) + { + position[0] = m_data->m_instance_positions_ptr[srcIndex*4+0]; + position[1] = m_data->m_instance_positions_ptr[srcIndex*4+1]; + position[2] = m_data->m_instance_positions_ptr[srcIndex*4+2]; + + orientation[0] = m_data->m_instance_quaternion_ptr[srcIndex*4+0]; + orientation[1] = m_data->m_instance_quaternion_ptr[srcIndex*4+1]; + orientation[2] = m_data->m_instance_quaternion_ptr[srcIndex*4+2]; + orientation[3] = m_data->m_instance_quaternion_ptr[srcIndex*4+3]; + return true; + } + } + + return false; +} void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int bodyUniqueId) { @@ -783,7 +844,7 @@ int GLInstancingRenderer::registerGraphicsInstance(int shapeIndex, const float* } -int GLInstancingRenderer::registerTexture(const unsigned char* texels, int width, int height) +int GLInstancingRenderer::registerTexture(const unsigned char* texels, int width, int height, bool flipPixelsY) { b3Assert(glGetError() ==GL_NO_ERROR); glActiveTexture(GL_TEXTURE0); @@ -801,12 +862,16 @@ int GLInstancingRenderer::registerTexture(const unsigned char* texels, int width h.m_height = height; m_data->m_textureHandles.push_back(h); - updateTexture(textureIndex, texels); + if (texels) + { + updateTexture(textureIndex, texels, flipPixelsY); + } return textureIndex; } -void GLInstancingRenderer::updateTexture(int textureIndex, const unsigned char* texels) + +void GLInstancingRenderer::updateTexture(int textureIndex, const unsigned char* texels, bool flipPixelsY) { if (textureIndex>=0) { @@ -816,25 +881,30 @@ void GLInstancingRenderer::updateTexture(int textureIndex, const unsigned cha glActiveTexture(GL_TEXTURE0); b3Assert(glGetError() ==GL_NO_ERROR); InternalTextureHandle& h = m_data->m_textureHandles[textureIndex]; - - //textures need to be flipped for OpenGL... - b3AlignedObjectArray flippedTexels; - flippedTexels.resize(h.m_width* h.m_height * 3); - for (int i = 0; i < h.m_width; i++) - { - for (int j = 0; j < h.m_height; j++) - { - flippedTexels[(i + j*h.m_width) * 3] = texels[(i + (h.m_height - 1 -j )*h.m_width) * 3]; - flippedTexels[(i + j*h.m_width) * 3+1] = texels[(i + (h.m_height - 1 - j)*h.m_width) * 3+1]; - flippedTexels[(i + j*h.m_width) * 3+2] = texels[(i + (h.m_height - 1 - j)*h.m_width) * 3+2]; - } - } - - - glBindTexture(GL_TEXTURE_2D,h.m_glTexture); + glBindTexture(GL_TEXTURE_2D,h.m_glTexture); b3Assert(glGetError() ==GL_NO_ERROR); - // const GLubyte* image= (const GLubyte*)texels; - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, h.m_width,h.m_height,0,GL_RGB,GL_UNSIGNED_BYTE,&flippedTexels[0]); + + if (flipPixelsY) + { + //textures need to be flipped for OpenGL... + b3AlignedObjectArray flippedTexels; + flippedTexels.resize(h.m_width* h.m_height * 3); + + for (int i = 0; i < h.m_width; i++) + { + for (int j = 0; j < h.m_height; j++) + { + flippedTexels[(i + j*h.m_width) * 3] = texels[(i + (h.m_height - 1 -j )*h.m_width) * 3]; + flippedTexels[(i + j*h.m_width) * 3+1] = texels[(i + (h.m_height - 1 - j)*h.m_width) * 3+1]; + flippedTexels[(i + j*h.m_width) * 3+2] = texels[(i + (h.m_height - 1 - j)*h.m_width) * 3+2]; + } + } + + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, h.m_width,h.m_height,0,GL_RGB,GL_UNSIGNED_BYTE,&flippedTexels[0]); + } else + { + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, h.m_width,h.m_height,0,GL_RGB,GL_UNSIGNED_BYTE,&texels[0]); + } b3Assert(glGetError() ==GL_NO_ERROR); glGenerateMipmap(GL_TEXTURE_2D); b3Assert(glGetError() ==GL_NO_ERROR); @@ -952,6 +1022,15 @@ void GLInstancingRenderer::InitShaders() int COLOR_BUFFER_SIZE = (m_data->m_maxNumObjectCapacity*sizeof(float)*4); int SCALE_BUFFER_SIZE = (m_data->m_maxNumObjectCapacity*sizeof(float)*3); + { + triangleShaderProgram = gltLoadShaderPair(triangleVertexShaderText,triangleFragmentShader); + triangle_mvp_location = glGetUniformLocation(triangleShaderProgram, "MVP"); + triangle_vpos_location = glGetAttribLocation(triangleShaderProgram, "vPos"); + triangle_vUV_location = glGetAttribLocation(triangleShaderProgram, "vUV"); + triangle_vcol_location = glGetUniformLocation(triangleShaderProgram, "vCol"); + glGenBuffers(1, &triangleIndexVbo); + } + linesShader = gltLoadShaderPair(linesVertexShader,linesFragmentShader); lines_ModelViewMatrix = glGetUniformLocation(linesShader, "ModelViewMatrix"); lines_ProjectionMatrix = glGetUniformLocation(linesShader, "ProjectionMatrix"); @@ -1294,6 +1373,216 @@ void GLInstancingRenderer::renderScene() } +struct PointerCaster +{ + union { + int m_baseIndex; + GLvoid* m_pointer; + }; + + PointerCaster() + :m_pointer(0) + { + } + + +}; + + + +#if 0 +static void b3CreateFrustum( + float left, + float right, + float bottom, + float top, + float nearVal, + float farVal, + float frustum[16]) +{ + + frustum[0*4+0] = (float(2) * nearVal) / (right - left); + frustum[0*4+1] = float(0); + frustum[0*4+2] = float(0); + frustum[0*4+3] = float(0); + + frustum[1*4+0] = float(0); + frustum[1*4+1] = (float(2) * nearVal) / (top - bottom); + frustum[1*4+2] = float(0); + frustum[1*4+3] = float(0); + + frustum[2*4+0] = (right + left) / (right - left); + frustum[2*4+1] = (top + bottom) / (top - bottom); + frustum[2*4+2] = -(farVal + nearVal) / (farVal - nearVal); + frustum[2*4+3] = float(-1); + + frustum[3*4+0] = float(0); + frustum[3*4+1] = float(0); + frustum[3*4+2] = -(float(2) * farVal * nearVal) / (farVal - nearVal); + frustum[3*4+3] = float(0); + +} +#endif + +static void b3Matrix4x4Mul(GLfloat aIn[4][4], GLfloat bIn[4][4], GLfloat result[4][4]) +{ + for (int j=0;j<4;j++) + for (int i=0;i<4;i++) + result[j][i] = aIn[0][i] * bIn[j][0] + aIn[1][i] * bIn[j][1] + aIn[2][i] * bIn[j][2] + aIn[3][i] * bIn[j][3]; +} + +static void b3Matrix4x4Mul16(GLfloat aIn[16], GLfloat bIn[16], GLfloat result[16]) +{ + for (int j=0;j<4;j++) + for (int i=0;i<4;i++) + result[j*4+i] = aIn[0*4+i] * bIn[j*4+0] + aIn[1*4+i] * bIn[j*4+1] + aIn[2*4+i] * bIn[j*4+2] + aIn[3*4+i] * bIn[j*4+3]; +} + + +static void b3CreateDiagonalMatrix(GLfloat value, GLfloat result[4][4]) +{ + for (int i=0;i<4;i++) + { + for (int j=0;j<4;j++) + { + if (i==j) + { + result[i][j] = value; + } else + { + result[i][j] = 0.f; + } + } + } +} + +static void b3CreateOrtho(GLfloat left, GLfloat right, GLfloat bottom, GLfloat top, GLfloat zNear, GLfloat zFar, GLfloat result[4][4]) +{ + b3CreateDiagonalMatrix(1.f,result); + + result[0][0] = 2.f / (right - left); + result[1][1] = 2.f / (top - bottom); + result[2][2] = - 2.f / (zFar - zNear); + result[3][0] = - (right + left) / (right - left); + result[3][1] = - (top + bottom) / (top - bottom); + result[3][2] = - (zFar + zNear) / (zFar - zNear); +} + +static void b3CreateLookAt(const b3Vector3& eye, const b3Vector3& center,const b3Vector3& up, GLfloat result[16]) +{ + b3Vector3 f = (center - eye).normalized(); + b3Vector3 u = up.normalized(); + b3Vector3 s = (f.cross(u)).normalized(); + u = s.cross(f); + + result[0*4+0] = s.x; + result[1*4+0] = s.y; + result[2*4+0] = s.z; + + result[0*4+1] = u.x; + result[1*4+1] = u.y; + result[2*4+1] = u.z; + + result[0*4+2] =-f.x; + result[1*4+2] =-f.y; + result[2*4+2] =-f.z; + + result[0*4+3] = 0.f; + result[1*4+3] = 0.f; + result[2*4+3] = 0.f; + + result[3*4+0] = -s.dot(eye); + result[3*4+1] = -u.dot(eye); + result[3*4+2] = f.dot(eye); + result[3*4+3] = 1.f; +} + + + + +typedef struct +{ + float position[4]; + float colour[4]; + float uv[2]; +} MyVertex; + + +void GLInstancingRenderer::drawTexturedTriangleMesh(float worldPosition[3], float worldOrientation[4], const float* vertices, int numvertices, const unsigned int* indices, int numIndices, float colorRGBA[4], int textureIndex) +{ + b3Quaternion orn(worldOrientation[0],worldOrientation[1],worldOrientation[2],worldOrientation[3]); + b3Vector3 pos = b3MakeVector3(worldPosition[0],worldPosition[1],worldPosition[2]); + + b3Transform worldTrans(orn,pos); + b3Scalar worldMatUnk[16]; + worldTrans.getOpenGLMatrix(worldMatUnk); + float modelMat[16]; + for (int i=0;i<16;i++) + { + modelMat[i] = worldMatUnk[i]; + } + glEnable(GL_TEXTURE_2D); + + activateTexture(textureIndex); + + + glUseProgram(triangleShaderProgram); + int err =GL_NO_ERROR; + err = glGetError(); + b3Assert(err ==GL_NO_ERROR); + GLuint vertex_buffer=0; + glGenBuffers(1, &vertex_buffer); + glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer); + glBufferData(GL_ARRAY_BUFFER, sizeof(MyVertex)*numvertices, vertices, GL_STATIC_DRAW); + + err = glGetError(); + b3Assert(err ==GL_NO_ERROR); + //glUniformMatrix4fv(useShadow_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]); + //glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]); + float modelView[16]; + + b3Matrix4x4Mul16(m_data->m_projectionMatrix,m_data->m_viewMatrix,modelView); + float MVP[16]; + b3Matrix4x4Mul16(modelView,modelMat,MVP); + + glUniformMatrix4fv(triangle_mvp_location, 1, GL_FALSE, (const GLfloat*) MVP); + err = glGetError(); + b3Assert(err ==GL_NO_ERROR); + + glUniform3f(triangle_vcol_location,colorRGBA[0],colorRGBA[1],colorRGBA[2]); + b3Assert(err ==GL_NO_ERROR); + + glEnableVertexAttribArray(triangle_vpos_location); + glVertexAttribPointer(triangle_vpos_location, 4, GL_FLOAT, GL_FALSE, + sizeof(MyVertex), (void*) 0); + err = glGetError(); + b3Assert(err ==GL_NO_ERROR); + glEnableVertexAttribArray(triangle_vUV_location); + err = glGetError(); + b3Assert(err ==GL_NO_ERROR); + glVertexAttribPointer(triangle_vUV_location, 2, GL_FLOAT, GL_FALSE, + sizeof(MyVertex), (void*) (sizeof(float) * 8)); + + err = glGetError(); + b3Assert(err ==GL_NO_ERROR); + + + b3Assert(glGetError() ==GL_NO_ERROR); + + b3Assert(glGetError() ==GL_NO_ERROR); + glDrawElements(GL_TRIANGLES, numIndices, GL_UNSIGNED_INT,indices); + b3Assert(glGetError() ==GL_NO_ERROR); + glDeleteBuffers(1,&vertex_buffer); + b3Assert(glGetError() ==GL_NO_ERROR); + + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D,0); + glUseProgram(0); + +} +//virtual void drawTexturedTriangleMesh(const double* vertices, int numvertices, const int* indices, int numIndices, int primitiveType=B3_GL_TRIANGLES, int textureIndex=-1); + + void GLInstancingRenderer::drawPoint(const double* position, const double color[4], double pointDrawSize) { float pos[4]={(float)position[0],(float)position[1],(float)position[2],0}; @@ -1491,131 +1780,6 @@ void GLInstancingRenderer::drawLine(const float from[4], const float to[4], cons glUseProgram(0); } -struct PointerCaster -{ - union { - int m_baseIndex; - GLvoid* m_pointer; - }; - - PointerCaster() - :m_pointer(0) - { - } - - -}; - - - -#if 0 -static void b3CreateFrustum( - float left, - float right, - float bottom, - float top, - float nearVal, - float farVal, - float frustum[16]) -{ - - frustum[0*4+0] = (float(2) * nearVal) / (right - left); - frustum[0*4+1] = float(0); - frustum[0*4+2] = float(0); - frustum[0*4+3] = float(0); - - frustum[1*4+0] = float(0); - frustum[1*4+1] = (float(2) * nearVal) / (top - bottom); - frustum[1*4+2] = float(0); - frustum[1*4+3] = float(0); - - frustum[2*4+0] = (right + left) / (right - left); - frustum[2*4+1] = (top + bottom) / (top - bottom); - frustum[2*4+2] = -(farVal + nearVal) / (farVal - nearVal); - frustum[2*4+3] = float(-1); - - frustum[3*4+0] = float(0); - frustum[3*4+1] = float(0); - frustum[3*4+2] = -(float(2) * farVal * nearVal) / (farVal - nearVal); - frustum[3*4+3] = float(0); - -} -#endif - -static void b3Matrix4x4Mul(GLfloat aIn[4][4], GLfloat bIn[4][4], GLfloat result[4][4]) -{ - for (int j=0;j<4;j++) - for (int i=0;i<4;i++) - result[j][i] = aIn[0][i] * bIn[j][0] + aIn[1][i] * bIn[j][1] + aIn[2][i] * bIn[j][2] + aIn[3][i] * bIn[j][3]; -} - -static void b3Matrix4x4Mul16(GLfloat aIn[16], GLfloat bIn[16], GLfloat result[16]) -{ - for (int j=0;j<4;j++) - for (int i=0;i<4;i++) - result[j*4+i] = aIn[0*4+i] * bIn[j*4+0] + aIn[1*4+i] * bIn[j*4+1] + aIn[2*4+i] * bIn[j*4+2] + aIn[3*4+i] * bIn[j*4+3]; -} - - -static void b3CreateDiagonalMatrix(GLfloat value, GLfloat result[4][4]) -{ - for (int i=0;i<4;i++) - { - for (int j=0;j<4;j++) - { - if (i==j) - { - result[i][j] = value; - } else - { - result[i][j] = 0.f; - } - } - } -} - -static void b3CreateOrtho(GLfloat left, GLfloat right, GLfloat bottom, GLfloat top, GLfloat zNear, GLfloat zFar, GLfloat result[4][4]) -{ - b3CreateDiagonalMatrix(1.f,result); - - result[0][0] = 2.f / (right - left); - result[1][1] = 2.f / (top - bottom); - result[2][2] = - 2.f / (zFar - zNear); - result[3][0] = - (right + left) / (right - left); - result[3][1] = - (top + bottom) / (top - bottom); - result[3][2] = - (zFar + zNear) / (zFar - zNear); -} - -static void b3CreateLookAt(const b3Vector3& eye, const b3Vector3& center,const b3Vector3& up, GLfloat result[16]) -{ - b3Vector3 f = (center - eye).normalized(); - b3Vector3 u = up.normalized(); - b3Vector3 s = (f.cross(u)).normalized(); - u = s.cross(f); - - result[0*4+0] = s.x; - result[1*4+0] = s.y; - result[2*4+0] = s.z; - - result[0*4+1] = u.x; - result[1*4+1] = u.y; - result[2*4+1] = u.z; - - result[0*4+2] =-f.x; - result[1*4+2] =-f.y; - result[2*4+2] =-f.z; - - result[0*4+3] = 0.f; - result[1*4+3] = 0.f; - result[2*4+3] = 0.f; - - result[3*4+0] = -s.dot(eye); - result[3*4+1] = -u.dot(eye); - result[3*4+2] = f.dot(eye); - result[3*4+3] = 1.f; -} - - void GLInstancingRenderer::renderSceneInternal(int renderMode) @@ -1960,7 +2124,10 @@ b3Assert(glGetError() ==GL_NO_ERROR); { glDisable (GL_BLEND); } - glActiveTexture(GL_TEXTURE0); + glActiveTexture(GL_TEXTURE1); + glBindTexture(GL_TEXTURE_2D,0); + + glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D,0); break; } diff --git a/examples/OpenGLWindow/GLInstancingRenderer.h b/examples/OpenGLWindow/GLInstancingRenderer.h index be063e0ab..b443f5e9e 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.h +++ b/examples/OpenGLWindow/GLInstancingRenderer.h @@ -64,8 +64,8 @@ public: ///vertices must be in the format x,y,z, nx,ny,nz, u,v virtual int registerShape(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType=B3_GL_TRIANGLES, int textureIndex=-1); - virtual int registerTexture(const unsigned char* texels, int width, int height); - virtual void updateTexture(int textureIndex, const unsigned char* texels); + virtual int registerTexture(const unsigned char* texels, int width, int height, bool flipPixelsY=true); + virtual void updateTexture(int textureIndex, const unsigned char* texels, bool flipPixelsY=true); virtual void activateTexture(int textureIndex); @@ -76,7 +76,8 @@ public: void writeTransforms(); - + virtual bool readSingleInstanceTransformToCPU(float* position, float* orientation, int srcIndex); + virtual void writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex); virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex) { @@ -113,6 +114,8 @@ public: virtual void drawPoints(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, float pointDrawSize); virtual void drawPoint(const float* position, const float color[4], float pointSize=1); virtual void drawPoint(const double* position, const double color[4], double pointDrawSize=1); + virtual void drawTexturedTriangleMesh(float worldPosition[3], float worldOrientation[4], const float* vertices, int numvertices, const unsigned int* indices, int numIndices, float color[4], int textureIndex=-1); + virtual void updateCamera(int upAxis=1); virtual const CommonCameraInterface* getActiveCamera() const; diff --git a/examples/OpenGLWindow/GLPrimitiveRenderer.cpp b/examples/OpenGLWindow/GLPrimitiveRenderer.cpp index 151d8ed67..b134a7ff9 100644 --- a/examples/OpenGLWindow/GLPrimitiveRenderer.cpp +++ b/examples/OpenGLWindow/GLPrimitiveRenderer.cpp @@ -43,6 +43,8 @@ static const char* fragmentShader3D= \ " texcolor = vec4(1,1,1,texcolor.x);\n" " }\n" " fragColour = colourV*texcolor;\n" +" if (fragColour.w == 0.f)\n" +" discard;\n" "}\n"; diff --git a/examples/OpenGLWindow/SimpleOpenGL2App.cpp b/examples/OpenGLWindow/SimpleOpenGL2App.cpp index 3122bd120..198a8bf67 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL2App.cpp @@ -297,13 +297,14 @@ void SimpleOpenGL2App::swapBuffer() m_window->startRendering(); } -void SimpleOpenGL2App::drawText( const char* txt, int posX, int posY, float size) + +void SimpleOpenGL2App::drawText( const char* txt, int posXi, int posYi, float size, float colorRGBA[4]) { } - static void restoreOpenGLState() +static void restoreOpenGLState() { @@ -339,6 +340,11 @@ void SimpleOpenGL2App::drawText( const char* txt, int posX, int posY, float size } +void SimpleOpenGL2App::drawText3D( const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag) +{ + +} + void SimpleOpenGL2App::drawText3D( const char* txt, float worldPosX, float worldPosY, float worldPosZ, float size1) { saveOpenGLState(gApp2->m_renderer->getScreenWidth(),gApp2->m_renderer->getScreenHeight()); diff --git a/examples/OpenGLWindow/SimpleOpenGL2App.h b/examples/OpenGLWindow/SimpleOpenGL2App.h index 0a08c8770..889df0657 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2App.h +++ b/examples/OpenGLWindow/SimpleOpenGL2App.h @@ -17,13 +17,16 @@ public: virtual int getUpAxis() const; virtual void swapBuffer(); - virtual void drawText( const char* txt, int posX, int posY, float size); + virtual void drawText( const char* txt, int posX, int posY, float size, float colorRGBA[4]); + virtual void drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA){}; virtual void setBackgroundColor(float red, float green, float blue); virtual int registerCubeShape(float halfExtentsX,float halfExtentsY, float halfExtentsZ, int textureIndex = -1, float textureScaling = 1); virtual int registerGraphicsUnitSphereShape(EnumSphereLevelOfDetail lod, int textureId=-1); virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size); + virtual void drawText3D( const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag); + virtual void registerGrid(int xres, int yres, float color0[4], float color1[4]); diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp index 25a3c2cfa..715f58721 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp @@ -139,6 +139,11 @@ void SimpleOpenGL2Renderer::removeGraphicsInstance(int instanceUid) m_data->m_graphicsInstancesPool.freeHandle(instanceUid); } +bool SimpleOpenGL2Renderer::readSingleInstanceTransformToCPU(float* position, float* orientation, int srcIndex) +{ + return false; +} + void SimpleOpenGL2Renderer::writeSingleInstanceColorToCPU(const float* color, int srcIndex) { } @@ -388,7 +393,7 @@ void SimpleOpenGL2Renderer::renderScene() } -int SimpleOpenGL2Renderer::registerTexture(const unsigned char* texels, int width, int height) +int SimpleOpenGL2Renderer::registerTexture(const unsigned char* texels, int width, int height, bool flipTexelsY) { b3Assert(glGetError() ==GL_NO_ERROR); glActiveTexture(GL_TEXTURE0); @@ -406,12 +411,12 @@ int SimpleOpenGL2Renderer::registerTexture(const unsigned char* texels, int widt h.m_height = height; m_data->m_textureHandles.push_back(h); - updateTexture(textureIndex, texels); + updateTexture(textureIndex, texels,flipTexelsY); return textureIndex; } -void SimpleOpenGL2Renderer::updateTexture(int textureIndex, const unsigned char* texels) +void SimpleOpenGL2Renderer::updateTexture(int textureIndex, const unsigned char* texels, bool flipTexelsY) { if (textureIndex>=0) { @@ -420,27 +425,36 @@ void SimpleOpenGL2Renderer::updateTexture(int textureIndex, const unsigned ch glActiveTexture(GL_TEXTURE0); b3Assert(glGetError() ==GL_NO_ERROR); - InternalTextureHandle2& h = m_data->m_textureHandles[textureIndex]; + InternalTextureHandle2& h = m_data->m_textureHandles[textureIndex]; + glBindTexture(GL_TEXTURE_2D,h.m_glTexture); + b3Assert(glGetError() ==GL_NO_ERROR); - //textures need to be flipped for OpenGL... - b3AlignedObjectArray flippedTexels; - flippedTexels.resize(h.m_width* h.m_height * 3); - for (int i = 0; i < h.m_width; i++) + + if (flipTexelsY) { - for (int j = 0; j < h.m_height; j++) + //textures need to be flipped for OpenGL... + b3AlignedObjectArray flippedTexels; + flippedTexels.resize(h.m_width* h.m_height * 3); + for (int i = 0; i < h.m_width; i++) { - flippedTexels[(i + j*h.m_width) * 3] = texels[(i + (h.m_height - 1 -j )*h.m_width) * 3]; - flippedTexels[(i + j*h.m_width) * 3+1] = texels[(i + (h.m_height - 1 - j)*h.m_width) * 3+1]; - flippedTexels[(i + j*h.m_width) * 3+2] = texels[(i + (h.m_height - 1 - j)*h.m_width) * 3+2]; + for (int j = 0; j < h.m_height; j++) + { + flippedTexels[(i + j*h.m_width) * 3] = texels[(i + (h.m_height - 1 -j )*h.m_width) * 3]; + flippedTexels[(i + j*h.m_width) * 3+1] = texels[(i + (h.m_height - 1 - j)*h.m_width) * 3+1]; + flippedTexels[(i + j*h.m_width) * 3+2] = texels[(i + (h.m_height - 1 - j)*h.m_width) * 3+2]; + } } + // const GLubyte* image= (const GLubyte*)texels; + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, h.m_width,h.m_height,0,GL_RGB,GL_UNSIGNED_BYTE,&flippedTexels[0]); + + } else + { + // const GLubyte* image= (const GLubyte*)texels; + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, h.m_width,h.m_height,0,GL_RGB,GL_UNSIGNED_BYTE,&texels[0]); + } - - glBindTexture(GL_TEXTURE_2D,h.m_glTexture); - b3Assert(glGetError() ==GL_NO_ERROR); - // const GLubyte* image= (const GLubyte*)texels; - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, h.m_width,h.m_height,0,GL_RGB,GL_UNSIGNED_BYTE,&flippedTexels[0]); - b3Assert(glGetError() ==GL_NO_ERROR); + b3Assert(glGetError() ==GL_NO_ERROR); glGenerateMipmap(GL_TEXTURE_2D); b3Assert(glGetError() ==GL_NO_ERROR); } diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h b/examples/OpenGLWindow/SimpleOpenGL2Renderer.h index 7eb9ab809..d89419528 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h +++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.h @@ -34,21 +34,24 @@ public: virtual void removeAllInstances(); virtual void removeGraphicsInstance(int instanceUid); + virtual bool readSingleInstanceTransformToCPU(float* position, float* orientation, int srcIndex); virtual void writeSingleInstanceColorToCPU(const float* color, int srcIndex); virtual void writeSingleInstanceColorToCPU(const double* color, int srcIndex); virtual void writeSingleInstanceScaleToCPU(const float* scale, int srcIndex); virtual void writeSingleInstanceScaleToCPU(const double* scale, int srcIndex); virtual void getCameraViewMatrix(float viewMat[16]) const; virtual void getCameraProjectionMatrix(float projMat[16]) const; - + virtual void drawTexturedTriangleMesh(float worldPosition[3], float worldOrientation[4], const float* vertices, int numvertices, const unsigned int* indices, int numIndices, float color[4], int textureIndex=-1) + { + } virtual void renderScene(); virtual int getScreenWidth(); virtual int getScreenHeight(); - virtual int registerTexture(const unsigned char* texels, int width, int height); - virtual void updateTexture(int textureIndex, const unsigned char* texels); - virtual void activateTexture(int textureIndex); + virtual int registerTexture(const unsigned char* texels, int width, int height, bool flipTexelsY); + virtual void updateTexture(int textureIndex, const unsigned char* texels, bool flipTexelsY); + virtual void activateTexture(int textureIndex); virtual int registerGraphicsInstance(int shapeIndex, const double* position, const double* quaternion, const double* color, const double* scaling); diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index 86aa9c031..b4781f749 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -33,7 +33,7 @@ #include #include "GLRenderToTexture.h" #include "Bullet3Common/b3Quaternion.h" - +#include //memset #ifdef _WIN32 #define popen _popen #define pclose _pclose @@ -44,8 +44,14 @@ struct SimpleInternalData GLuint m_fontTextureId; GLuint m_largeFontTextureId; struct sth_stash* m_fontStash; - OpenGL2RenderCallbacks* m_renderCallbacks; + struct sth_stash* m_fontStash2; + + RenderCallbacks* m_renderCallbacks; + RenderCallbacks* m_renderCallbacks2; + int m_droidRegular; + int m_droidRegular2; + const char* m_frameDumpPngFileName; FILE* m_ffmpegFile; GLRenderToTexture* m_renderTexture; @@ -117,8 +123,161 @@ static GLuint BindFont(const CTexFont *_Font) return TexID; } + + + +//static unsigned int s_indexData[INDEX_COUNT]; +//static GLuint s_indexArrayObject, s_indexBuffer; +//static GLuint s_vertexArrayObject,s_vertexBuffer; + + extern unsigned char OpenSansData[]; +struct MyRenderCallbacks : public RenderCallbacks +{ + GLInstancingRenderer* m_instancingRenderer; + + b3AlignedObjectArray m_rgbaTexture; + float m_color[4]; + float m_worldPosition[3]; + float m_worldOrientation[4]; + + int m_textureIndex; + + MyRenderCallbacks(GLInstancingRenderer* instancingRenderer) + :m_instancingRenderer(instancingRenderer), + m_textureIndex(-1) + { + for (int i=0;i<4;i++) + { + m_color[i] = 1; + m_worldOrientation[i]=0; + } + m_worldPosition[0]=0; + m_worldPosition[1]=0; + m_worldPosition[2]=0; + + m_worldOrientation[0] = 0; + m_worldOrientation[1] = 0; + m_worldOrientation[2] = 0; + m_worldOrientation[3] = 1; + } + virtual ~MyRenderCallbacks() + { + m_rgbaTexture.clear(); + } + + virtual void setWorldPosition(float pos[3]) + { + for (int i=0;i<3;i++) + { + m_worldPosition[i] = pos[i]; + } + } + + virtual void setWorldOrientation(float orn[4]) + { + for (int i=0;i<4;i++) + { + m_worldOrientation[i] = orn[i]; + } + } + + + virtual void setColorRGBA(float color[4]) + { + for (int i=0;i<4;i++) + { + m_color[i] = color[i]; + } + } + virtual void updateTexture(sth_texture* texture, sth_glyph* glyph, int textureWidth, int textureHeight) + { + if (glyph) + { + m_rgbaTexture.resize(textureWidth*textureHeight*3); + for (int i=0;im_texels[i]; + m_rgbaTexture[i*3+1] = texture->m_texels[i]; + m_rgbaTexture[i*3+2] = texture->m_texels[i]; + } + bool flipPixelsY = false; + m_instancingRenderer->updateTexture(m_textureIndex, &m_rgbaTexture[0], flipPixelsY); + } else + { + if (textureWidth && textureHeight) + { + texture->m_texels = (unsigned char*)malloc(textureWidth*textureHeight); + memset(texture->m_texels,0,textureWidth*textureHeight); + if (m_textureIndex<0) + { + m_rgbaTexture.resize(textureWidth*textureHeight*3); + bool flipPixelsY = false; + m_textureIndex = m_instancingRenderer->registerTexture(&m_rgbaTexture[0],textureWidth,textureHeight,flipPixelsY); + + int strideInBytes = 9*sizeof(float); + int numVertices = sizeof(cube_vertices_textured)/strideInBytes; + int numIndices = sizeof(cube_indices)/sizeof(int); + + float halfExtentsX=1; + float halfExtentsY=1; + float halfExtentsZ=1; + float textureScaling = 4; + + b3AlignedObjectArray verts; + verts.resize(numVertices); + for (int i=0;iregisterShape(&verts[0].x,numVertices,cube_indices,numIndices,B3_GL_TRIANGLES,m_textureIndex); + b3Vector3 pos = b3MakeVector3(0, 0, 0); + b3Quaternion orn(0, 0, 0, 1); + b3Vector4 color = b3MakeVector4(1, 1, 1,1); + b3Vector3 scaling = b3MakeVector3 (.1, .1, .1); + //m_instancingRenderer->registerGraphicsInstance(shapeId, pos, orn, color, scaling); + m_instancingRenderer->writeTransforms(); + + } else + { + b3Assert(0); + } + } else + { + delete texture->m_texels; + texture->m_texels = 0; + //there is no m_instancingRenderer->freeTexture (yet), all textures are released at reset/deletion of the renderer + } + } + + } + virtual void render(sth_texture* texture) + { + int index=0; + + float width = 1; + b3AlignedObjectArray< unsigned int> indices; + indices.resize(texture->nverts); + for (int i=0;idrawTexturedTriangleMesh(m_worldPosition,m_worldOrientation, &texture->newverts[0].position.p[0],texture->nverts,&indices[0],indices.size(),m_color,m_textureIndex); + } +}; + SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height, bool allowRetina) { gApp = this; @@ -207,9 +366,11 @@ SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height, boo { - m_data->m_renderCallbacks = new OpenGL2RenderCallbacks(m_primRenderer); + m_data->m_renderCallbacks2 = new MyRenderCallbacks(m_instancingRenderer); + m_data->m_fontStash2 = sth_create(512,512, m_data->m_renderCallbacks2); m_data->m_fontStash = sth_create(512,512,m_data->m_renderCallbacks);//256,256);//,1024);//512,512); + b3Assert(glGetError() ==GL_NO_ERROR); if (!m_data->m_fontStash) @@ -218,6 +379,10 @@ SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height, boo //fprintf(stderr, "Could not create stash.\n"); } + if (!m_data->m_fontStash2) + { + b3Warning("Could not create fontStash2"); + } unsigned char* data2 = OpenSansData; unsigned char* data = (unsigned char*) data2; @@ -225,7 +390,12 @@ SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height, boo { b3Warning("error!\n"); } - b3Assert(glGetError() ==GL_NO_ERROR); + if (!(m_data->m_droidRegular2 = sth_add_font_from_memory(m_data->m_fontStash2, data))) + { + b3Warning("error!\n"); + } + + b3Assert(glGetError() ==GL_NO_ERROR); } } @@ -235,7 +405,7 @@ struct sth_stash* SimpleOpenGL3App::getFontStash() return m_data->m_fontStash; } -void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float worldPosY, float worldPosZ, float size1) +void SimpleOpenGL3App::drawText3D( const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag) { B3_PROFILE("SimpleOpenGL3App::drawText3D"); float viewMat[16]; @@ -263,29 +433,54 @@ void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float world glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); int viewport[4]={0,0,m_instancingRenderer->getScreenWidth(),m_instancingRenderer->getScreenHeight()}; - float posX = 450.f; - float posY = 100.f; + float posX = position[0]; + float posY = position[1]; + float posZ = position[2]; float winx,winy, winz; - if (!projectWorldCoordToScreen(worldPosX, worldPosY, worldPosZ,viewMat,projMat,viewport,&winx, &winy, &winz)) - { - return; - } - posX = winx; - posY = m_instancingRenderer->getScreenHeight()/2+(m_instancingRenderer->getScreenHeight()/2)-winy; - - if (0)//m_useTrueTypeFont) + if (optionFlag&CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera) + { + if (!projectWorldCoordToScreen(position[0], position[1], position[2],viewMat,projMat,viewport,&winx, &winy, &winz)) + { + return; + } + posX = winx; + posY = m_instancingRenderer->getScreenHeight()/2+(m_instancingRenderer->getScreenHeight()/2)-winy; + posZ = 0.f; + } + + if (optionFlag&CommonGraphicsApp::eDrawText3D_TrueType) { bool measureOnly = false; - float fontSize= 32;//64;//512;//128; + float fontSize= 64;//512;//128; - sth_draw_text(m_data->m_fontStash, - m_data->m_droidRegular,fontSize,posX,posY, - txt,&dx, this->m_instancingRenderer->getScreenWidth(),this->m_instancingRenderer->getScreenHeight(),measureOnly,m_window->getRetinaScale()); - sth_end_draw(m_data->m_fontStash); - sth_flush_draw(m_data->m_fontStash); + if (optionFlag&CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera) + { + sth_draw_text(m_data->m_fontStash, + m_data->m_droidRegular,fontSize,posX,posY, + txt,&dx, this->m_instancingRenderer->getScreenWidth(),this->m_instancingRenderer->getScreenHeight(),measureOnly,m_window->getRetinaScale(),color); + sth_end_draw(m_data->m_fontStash); + sth_flush_draw(m_data->m_fontStash); + } else + { + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + + m_data->m_renderCallbacks2->setColorRGBA(color); + + m_data->m_renderCallbacks2->setWorldPosition(position); + m_data->m_renderCallbacks2->setWorldOrientation(orientation); + + sth_draw_text3D(m_data->m_fontStash2, + m_data->m_droidRegular2,fontSize,0,0,0, + txt,&dx,size,color,0); + sth_end_draw(m_data->m_fontStash2); + sth_flush_draw(m_data->m_fontStash2); + glDisable(GL_BLEND); + } } else { //float width = 0.f; @@ -298,26 +493,38 @@ void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float world //float extraSpacing = 0.; float startX = posX; - float startY = posY-g_DefaultLargeFont->m_CharHeight*size1; - + float startY = posY+g_DefaultLargeFont->m_CharHeight*size; + float z = position[2];//2.f*winz-1.f;//*(far + + if (optionFlag&CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera) + { + posX = winx; + posY = m_instancingRenderer->getScreenHeight()/2+(m_instancingRenderer->getScreenHeight()/2)-winy; + z = 2.f*winz-1.f; + startY = posY-g_DefaultLargeFont->m_CharHeight*size; + } while (txt[pos]) { int c = txt[pos]; //r.h = g_DefaultNormalFont->m_CharHeight; //r.w = g_DefaultNormalFont->m_CharWidth[c]+extraSpacing; - float endX = startX+g_DefaultLargeFont->m_CharWidth[c]*size1; + float endX = startX+g_DefaultLargeFont->m_CharWidth[c]*size; + if (optionFlag&CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera) + { + endX = startX+g_DefaultLargeFont->m_CharWidth[c]*size; + } float endY = posY; - float currentColor[]={1.f,0.2,0.2f,1.f}; + //float currentColor[]={1.f,1.,0.2f,1.f}; // m_primRenderer->drawTexturedRect(startX, startY, endX, endY, currentColor,g_DefaultLargeFont->m_CharU0[c],g_DefaultLargeFont->m_CharV0[c],g_DefaultLargeFont->m_CharU1[c],g_DefaultLargeFont->m_CharV1[c]); float u0 = g_DefaultLargeFont->m_CharU0[c]; float u1 = g_DefaultLargeFont->m_CharU1[c]; float v0 = g_DefaultLargeFont->m_CharV0[c]; float v1 = g_DefaultLargeFont->m_CharV1[c]; - float color[4] = {currentColor[0],currentColor[1],currentColor[2],currentColor[3]}; + //float color[4] = {currentColor[0],currentColor[1],currentColor[2],currentColor[3]}; float x0 = startX; float x1 = endX; float y0 = startY; @@ -325,20 +532,31 @@ void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float world int screenWidth = m_instancingRenderer->getScreenWidth(); int screenHeight = m_instancingRenderer->getScreenHeight(); - float z = 2.f*winz-1.f;//*(far float identity[16]={1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1}; + if (optionFlag&CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera) + { PrimVertex vertexData[4] = { PrimVertex(PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y0/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)), PrimVertex(PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y1/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)), PrimVertex(PrimVec4( -1.f+2.f*x1/float(screenWidth), 1.f-2.f*y1/float(screenHeight), z, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)), PrimVertex(PrimVec4( -1.f+2.f*x1/float(screenWidth), 1.f-2.f*y0/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)) }; - m_primRenderer->drawTexturedRect3D(vertexData[0],vertexData[1],vertexData[2],vertexData[3],identity,identity,false); + } else + { + PrimVertex vertexData[4] = { + PrimVertex(PrimVec4(x0, y0, z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)), + PrimVertex(PrimVec4(x0, y1, z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)), + PrimVertex(PrimVec4( x1, y1, z, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)), + PrimVertex(PrimVec4( x1, y0, z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)) + }; + + m_primRenderer->drawTexturedRect3D(vertexData[0],vertexData[1],vertexData[2],vertexData[3],viewMat,projMat,false); + } //DrawTexturedRect(0,r,g_DefaultNormalFont->m_CharU0[c],g_DefaultNormalFont->m_CharV0[c],g_DefaultNormalFont->m_CharU1[c],g_DefaultNormalFont->m_CharV1[c]); // DrawFilledRect(r); @@ -353,12 +571,22 @@ void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float world glDisable(GL_BLEND); +} +void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float worldPosY, float worldPosZ, float size1) +{ + + float position[3] = {worldPosX,worldPosY,worldPosZ}; + float orientation[4] = {0,0,0,1}; + float color[4] = {0,0,0,1}; + int optionFlags = CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera; + drawText3D(txt,position,orientation,color,size1,optionFlags); } -void SimpleOpenGL3App::drawText( const char* txt, int posXi, int posYi, float size) + +void SimpleOpenGL3App::drawText( const char* txt, int posXi, int posYi, float size, float colorRGBA[4]) { float posX = (float)posXi; @@ -386,7 +614,7 @@ void SimpleOpenGL3App::drawText( const char* txt, int posXi, int posYi, float si txt,&dx, this->m_instancingRenderer->getScreenWidth(), this->m_instancingRenderer->getScreenHeight(), measureOnly, - m_window->getRetinaScale()); + m_window->getRetinaScale(),colorRGBA); sth_end_draw(m_data->m_fontStash); sth_flush_draw(m_data->m_fontStash); @@ -445,12 +673,6 @@ void SimpleOpenGL3App::drawTexturedRect(float x0, float y0, float x1, float y1, -struct GfxVertex - { - float x,y,z,w; - float nx,ny,nz; - float u,v; - }; int SimpleOpenGL3App::registerCubeShape(float halfExtentsX,float halfExtentsY, float halfExtentsZ, int textureIndex, float textureScaling) { @@ -460,7 +682,7 @@ int SimpleOpenGL3App::registerCubeShape(float halfExtentsX,float halfExtentsY, f int numVertices = sizeof(cube_vertices_textured)/strideInBytes; int numIndices = sizeof(cube_indices)/sizeof(int); - b3AlignedObjectArray verts; + b3AlignedObjectArray verts; verts.resize(numVertices); for (int i=0;im_fontStash); delete m_data->m_renderCallbacks; + + sth_delete(m_data->m_fontStash2); + delete m_data->m_renderCallbacks2; + TwDeleteDefaultFonts(); m_window->closeWindow(); diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.h b/examples/OpenGLWindow/SimpleOpenGL3App.h index 3d0f69dd4..9a1621350 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.h +++ b/examples/OpenGLWindow/SimpleOpenGL3App.h @@ -31,8 +31,10 @@ struct SimpleOpenGL3App : public CommonGraphicsApp virtual int getUpAxis() const; virtual void swapBuffer(); - virtual void drawText( const char* txt, int posX, int posY, float size=1.0f); + virtual void drawText( const char* txt, int posX, int posY, float size, float colorRGBA[4]); virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size); + virtual void drawText3D( const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag); + virtual void drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA); struct sth_stash* getFontStash(); diff --git a/examples/OpenGLWindow/fontstash.cpp b/examples/OpenGLWindow/fontstash.cpp index 4f144095b..2289aec38 100644 --- a/examples/OpenGLWindow/fontstash.cpp +++ b/examples/OpenGLWindow/fontstash.cpp @@ -487,6 +487,9 @@ error: return 0; } + + + static int get_quad(struct sth_stash* stash, struct sth_font* fnt, struct sth_glyph* glyph, short isize, float* x, float* y, struct sth_quad* q) { float rx,ry; @@ -514,7 +517,36 @@ static int get_quad(struct sth_stash* stash, struct sth_font* fnt, struct sth_gl return 1; } -static Vertex* setv(Vertex* v, float x, float y, float s, float t, float width, float height) + +static int get_quad3D(struct sth_stash* stash, struct sth_font* fnt, struct sth_glyph* glyph, short isize2, float* x, float* y, struct sth_quad* q, float fontSize, float textScale) +{ + short isize=1; + float rx,ry; + float scale = textScale/fontSize;//0.1;//1.0f; + + if (fnt->type == BMFONT) + scale = isize/(glyph->size); + + rx = (*x + scale * float(glyph->xoff)); + ry = (scale * float(glyph->yoff)); + + q->x0 = rx; + q->y0 = *y -(ry); + + q->x1 = rx + scale * float(glyph->x1 - glyph->x0_); + q->y1 = *y-(ry + scale * float(glyph->y1 - glyph->y0)); + + q->s0 = float(glyph->x0_) * stash->itw; + q->t0 = float(glyph->y0) * stash->ith; + q->s1 = float(glyph->x1) * stash->itw; + q->t1 = float(glyph->y1) * stash->ith; + + *x += scale * glyph->xadv; + + return 1; +} + +static Vertex* setv(Vertex* v, float x, float y, float s, float t, float width, float height, float colorRGBA[4]) { bool scale=true; if (scale) @@ -532,15 +564,34 @@ static Vertex* setv(Vertex* v, float x, float y, float s, float t, float width, v->uv.p[0] = s; v->uv.p[1] = t; - v->colour.p[0] = 0.1f;//1.f; - v->colour.p[1] = 0.1f; - v->colour.p[2] = 0.1f; - v->colour.p[3] = 1.f; + v->colour.p[0] = colorRGBA[0]; + v->colour.p[1] = colorRGBA[1]; + v->colour.p[2] = colorRGBA[2]; + v->colour.p[3] = colorRGBA[3]; return v+1; } +static Vertex* setv3D(Vertex* v, float x, float y, float z, float s, float t, float colorRGBA[4]) +{ + v->position.p[0] = x; + v->position.p[1] = y; + v->position.p[2] = z; + v->position.p[3] = 1.f; + + v->uv.p[0] = s; + v->uv.p[1] = t; + + v->colour.p[0] = colorRGBA[0]; + v->colour.p[1] = colorRGBA[1]; + v->colour.p[2] = colorRGBA[2]; + v->colour.p[3] = colorRGBA[3]; + return v+1; +} + + + static void flush_draw(struct sth_stash* stash) @@ -598,7 +649,7 @@ void sth_draw_texture(struct sth_stash* stash, int idx, float size, float x, float y, int screenwidth, int screenheight, - const char* s, float* dx) + const char* s, float* dx, float colorRGBA[4]) { int width = stash->tw; int height=stash->th; @@ -642,13 +693,13 @@ void sth_draw_texture(struct sth_stash* stash, q.x1 = q.x0+width; q.y1 = q.y0+height; - v = setv(v, q.x0, q.y0, 0,0,(float)screenwidth,(float)screenheight); - v = setv(v, q.x1, q.y0, 1,0,(float)screenwidth,(float)screenheight); - v = setv(v, q.x1, q.y1, 1,1,(float)screenwidth,(float)screenheight); + v = setv(v, q.x0, q.y0, 0,0,(float)screenwidth,(float)screenheight,colorRGBA); + v = setv(v, q.x1, q.y0, 1,0,(float)screenwidth,(float)screenheight,colorRGBA); + v = setv(v, q.x1, q.y1, 1,1,(float)screenwidth,(float)screenheight,colorRGBA); - v = setv(v, q.x0, q.y0, 0,0,(float)screenwidth,(float)screenheight); - v = setv(v, q.x1, q.y1, 1,1,(float)screenwidth,(float)screenheight); - v = setv(v, q.x0, q.y1, 0,1,(float)screenwidth,(float)screenheight); + v = setv(v, q.x0, q.y0, 0,0,(float)screenwidth,(float)screenheight,colorRGBA); + v = setv(v, q.x1, q.y1, 1,1,(float)screenwidth,(float)screenheight,colorRGBA); + v = setv(v, q.x0, q.y1, 0,1,(float)screenwidth,(float)screenheight,colorRGBA); texture->nverts += 6; } @@ -667,7 +718,7 @@ void sth_flush_draw(struct sth_stash* stash) void sth_draw_text(struct sth_stash* stash, int idx, float size, float x, float y, - const char* s, float* dx, int screenwidth, int screenheight, int measureOnly, float retinaScale) + const char* s, float* dx, int screenwidth, int screenheight, int measureOnly, float retinaScale, float colorRGBA[4]) { unsigned int codepoint; @@ -708,13 +759,68 @@ void sth_draw_text(struct sth_stash* stash, { v = &texture->newverts[texture->nverts]; - v = setv(v, q.x0, q.y0, q.s0, q.t0,(float)screenwidth,(float)screenheight); - v = setv(v, q.x1, q.y0, q.s1, q.t0,(float)screenwidth,(float)screenheight); - v = setv(v, q.x1, q.y1, q.s1, q.t1,(float)screenwidth,(float)screenheight); + v = setv(v, q.x0, q.y0, q.s0, q.t0,(float)screenwidth,(float)screenheight,colorRGBA); + v = setv(v, q.x1, q.y0, q.s1, q.t0,(float)screenwidth,(float)screenheight,colorRGBA); + v = setv(v, q.x1, q.y1, q.s1, q.t1,(float)screenwidth,(float)screenheight,colorRGBA); - v = setv(v, q.x0, q.y0, q.s0, q.t0,(float)screenwidth,(float)screenheight); - v = setv(v, q.x1, q.y1, q.s1, q.t1,(float)screenwidth,(float)screenheight); - v = setv(v, q.x0, q.y1, q.s0, q.t1,(float)screenwidth,(float)screenheight); + v = setv(v, q.x0, q.y0, q.s0, q.t0,(float)screenwidth,(float)screenheight,colorRGBA); + v = setv(v, q.x1, q.y1, q.s1, q.t1,(float)screenwidth,(float)screenheight,colorRGBA); + v = setv(v, q.x0, q.y1, q.s0, q.t1,(float)screenwidth,(float)screenheight,colorRGBA); + + texture->nverts += 6; + } + } + + if (dx) *dx = x; +} + +void sth_draw_text3D(struct sth_stash* stash, + int idx, float fontSize, + float x, float y, float z, + const char* s, float* dx, float textScale, float colorRGBA[4], int unused) +{ + + unsigned int codepoint; + struct sth_glyph* glyph = NULL; + struct sth_texture* texture = NULL; + unsigned int state = 0; + struct sth_quad q; + short isize = (short)(fontSize*10.0f); + Vertex* v; + struct sth_font* fnt = NULL; + + s_retinaScale = 1; + if (stash == NULL) return; + + if (!stash->textures) return; + fnt = stash->fonts; + while(fnt != NULL && fnt->idx != idx) fnt = fnt->next; + if (fnt == NULL) return; + if (fnt->type != BMFONT && !fnt->data) return; + + for (; *s; ++s) + { + if (decutf8(&state, &codepoint, *(unsigned char*)s)) + continue; + glyph = get_glyph(stash, fnt, codepoint, isize); + if (!glyph) continue; + texture = glyph->texture; + + if (texture->nverts+6 >= VERT_COUNT) + flush_draw(stash); + + if (!get_quad3D(stash, fnt, glyph, isize, &x, &y, &q, fontSize, textScale)) continue; + + { + v = &texture->newverts[texture->nverts]; + + v = setv3D(v, q.x0, q.y0, z,q.s0, q.t0,colorRGBA); + v = setv3D(v, q.x1, q.y0, z,q.s1, q.t0,colorRGBA); + v = setv3D(v, q.x1, q.y1, z,q.s1, q.t1,colorRGBA); + + v = setv3D(v, q.x0, q.y0, z,q.s0, q.t0,colorRGBA); + v = setv3D(v, q.x1, q.y1, z,q.s1, q.t1,colorRGBA); + v = setv3D(v, q.x0, q.y1, z,q.s0, q.t1,colorRGBA); texture->nverts += 6; } diff --git a/examples/OpenGLWindow/fontstash.h b/examples/OpenGLWindow/fontstash.h index 940bb7856..ef6610f38 100644 --- a/examples/OpenGLWindow/fontstash.h +++ b/examples/OpenGLWindow/fontstash.h @@ -22,7 +22,7 @@ #define MAX_ROWS 128 -#define VERT_COUNT (6*128) +#define VERT_COUNT (16*128) #define INDEX_COUNT (VERT_COUNT*2) @@ -102,6 +102,9 @@ struct sth_texture struct RenderCallbacks { virtual ~RenderCallbacks() {} + virtual void setColorRGBA(float color[4])=0; + virtual void setWorldPosition(float pos[3])=0; + virtual void setWorldOrientation(float orn[4])=0; virtual void updateTexture(sth_texture* texture, sth_glyph* glyph, int textureWidth, int textureHeight)=0; virtual void render(sth_texture* texture)=0; }; @@ -124,13 +127,28 @@ void sth_draw_texture(struct sth_stash* stash, int idx, float size, float x, float y, int screenwidth, int screenheight, - const char* s, float* dx); + const char* s, float* dx, float colorRGBA[4]); void sth_flush_draw(struct sth_stash* stash); + +void sth_draw_text3D(struct sth_stash* stash, + int idx, float fontSize, + float x, float y, float z, + const char* s, float* dx, float textScale, float colorRGBA[4], int bla); + void sth_draw_text(struct sth_stash* stash, int idx, float size, - float x, float y, const char* string, float* dx, int screenwidth, int screenheight, int measureOnly=0, float retinaScale=1); + float x, float y, const char* string, float* dx, int screenwidth, int screenheight, int measureOnly, float retinaScale, float colorRGBA[4]); + +inline void sth_draw_text(struct sth_stash* stash, + int idx, float size, + float x, float y, const char* string, float* dx, int screenwidth, int screenheight, int measureOnly=false, float retinaScale=1.) +{ + float colorRGBA[4]={1,1,1,1}; + sth_draw_text(stash,idx,size,x,y,string,dx,screenwidth,screenheight,measureOnly, retinaScale, colorRGBA); + +} void sth_dim_text(struct sth_stash* stash, int idx, float size, const char* string, float* minx, float* miny, float* maxx, float* maxy); diff --git a/examples/OpenGLWindow/opengl_fontstashcallbacks.cpp b/examples/OpenGLWindow/opengl_fontstashcallbacks.cpp index 3b9f9fe40..90e4bebca 100644 --- a/examples/OpenGLWindow/opengl_fontstashcallbacks.cpp +++ b/examples/OpenGLWindow/opengl_fontstashcallbacks.cpp @@ -14,8 +14,8 @@ static unsigned int s_indexData[INDEX_COUNT]; -GLuint s_indexArrayObject, s_indexBuffer; -GLuint s_vertexArrayObject,s_vertexBuffer; +static GLuint s_indexArrayObject, s_indexBuffer; +static GLuint s_vertexArrayObject,s_vertexBuffer; OpenGL2RenderCallbacks::OpenGL2RenderCallbacks(GLPrimitiveRenderer* primRender) :m_primRender2(primRender) @@ -54,7 +54,13 @@ void InternalOpenGL2RenderCallbacks::display2() assert(glGetError()==GL_NO_ERROR); - + float identity[16]={1,0,0,0, + 0,1,0,0, + 0,0,1,0, + 0,0,0,1}; + glUniformMatrix4fv(data->m_viewmatUniform, 1, false, identity); + glUniformMatrix4fv(data->m_projMatUniform, 1, false, identity); + vec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) ); glUniform2fv(data->m_positionUniform, 1, (const GLfloat *)&p); diff --git a/examples/OpenGLWindow/opengl_fontstashcallbacks.h b/examples/OpenGLWindow/opengl_fontstashcallbacks.h index 285a3df97..e4db0010f 100644 --- a/examples/OpenGLWindow/opengl_fontstashcallbacks.h +++ b/examples/OpenGLWindow/opengl_fontstashcallbacks.h @@ -43,6 +43,10 @@ struct OpenGL2RenderCallbacks : public InternalOpenGL2RenderCallbacks GLPrimitiveRenderer* m_primRender2; virtual PrimInternalData* getData(); + virtual void setWorldPosition(float pos[3]){} + virtual void setWorldOrientation(float orn[4]){} + virtual void setColorRGBA(float color[4]){} + OpenGL2RenderCallbacks(GLPrimitiveRenderer* primRender); virtual ~OpenGL2RenderCallbacks(); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 6a4023c58..90e340890 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1612,6 +1612,9 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle p command->m_userDebugDrawArgs.m_lineWidth = lineWidth; command->m_userDebugDrawArgs.m_lifeTime = lifeTime; + command->m_userDebugDrawArgs.m_trackingObjectUniqueId = -1; + command->m_userDebugDrawArgs.m_trackingLinkIndex = -1; + command->m_userDebugDrawArgs.m_optionFlags = 0; return (b3SharedMemoryCommandHandle) command; } @@ -1646,10 +1649,51 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle p command->m_userDebugDrawArgs.m_textSize = textSize; command->m_userDebugDrawArgs.m_lifeTime = lifeTime; + command->m_userDebugDrawArgs.m_trackingObjectUniqueId = -1; + command->m_userDebugDrawArgs.m_trackingLinkIndex = -1; + + command->m_userDebugDrawArgs.m_optionFlags = 0; return (b3SharedMemoryCommandHandle) command; } +void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_DEBUG_DRAW); + b3Assert(command->m_updateFlags & USER_DEBUG_HAS_TEXT); + command->m_userDebugDrawArgs.m_optionFlags = optionFlags; + command->m_updateFlags |= USER_DEBUG_HAS_OPTION_FLAGS; +} + +void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, double orientation[4]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_DEBUG_DRAW); + b3Assert(command->m_updateFlags & USER_DEBUG_HAS_TEXT); + command->m_userDebugDrawArgs.m_textOrientation[0] = orientation[0]; + command->m_userDebugDrawArgs.m_textOrientation[1] = orientation[1]; + command->m_userDebugDrawArgs.m_textOrientation[2] = orientation[2]; + command->m_userDebugDrawArgs.m_textOrientation[3] = orientation[3]; + command->m_updateFlags |= USER_DEBUG_HAS_TEXT_ORIENTATION; + +} + + +void b3UserDebugItemSetTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_DEBUG_DRAW); + + command->m_updateFlags |= USER_DEBUG_HAS_TRACKING_OBJECT; + command->m_userDebugDrawArgs.m_trackingObjectUniqueId = objectUniqueId; + command->m_userDebugDrawArgs.m_trackingLinkIndex = linkIndex; +} + + b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -1670,6 +1714,8 @@ b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle ph command->m_userDebugDrawArgs.m_rangeMin = rangeMin; command->m_userDebugDrawArgs.m_rangeMax = rangeMax; command->m_userDebugDrawArgs.m_startValue = startValue; + command->m_userDebugDrawArgs.m_trackingObjectUniqueId = -1; + command->m_userDebugDrawArgs.m_optionFlags = 0; return (b3SharedMemoryCommandHandle)command; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index cee3a9b41..f36560508 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -122,7 +122,13 @@ int b3GetStatusOpenGLVisualizerCamera(b3SharedMemoryStatusHandle statusHandle, s /// Add/remove user-specific debug lines and debug text messages b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime); + b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime); +void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags); +void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, double orientation[4]); + +void b3UserDebugItemSetTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex); + b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue); b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId); int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 2c1936e75..4872a151a 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -280,7 +280,15 @@ bool PhysicsClientSharedMemory::connect() { if (m_data->m_testBlock1) { if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER) { - b3Error("Error: please start server before client\n"); + //there is no chance people are still using this software 100 years from now ;-) + if ((m_data->m_testBlock1->m_magicId < 211705023) && + (m_data->m_testBlock1->m_magicId >=201705023)) + { + b3Error("Error: physics server version mismatch (expected %d got %d)\n",SHARED_MEMORY_MAGIC_NUMBER, m_data->m_testBlock1->m_magicId); + } else + { + b3Error("Error connecting to shared memory: please start server before client\n"); + } m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); m_data->m_testBlock1 = 0; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6f3cd63dd..e7e0c93f5 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5816,6 +5816,37 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED; hasStatus = true; + + + int trackingVisualShapeIndex = -1; + + if (clientCmd.m_userDebugDrawArgs.m_trackingObjectUniqueId>=0) + { + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_userDebugDrawArgs.m_trackingObjectUniqueId); + if (bodyHandle && bodyHandle->m_multiBody) + { + int linkIndex = clientCmd.m_userDebugDrawArgs.m_trackingLinkIndex; + if (linkIndex ==-1) + { + if (bodyHandle->m_multiBody->getBaseCollider()) + { + trackingVisualShapeIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); + } + } else + { + if (linkIndex >=0 && linkIndex < bodyHandle->m_multiBody->getNumLinks()) + { + if (bodyHandle->m_multiBody->getLink(linkIndex).m_collider) + { + trackingVisualShapeIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex(); + } + } + + } + + } + } + if (clientCmd.m_updateFlags & USER_DEBUG_ADD_PARAMETER) { int uid = m_data->m_guiHelper->addUserDebugParameter( @@ -5888,11 +5919,27 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT) { + //addUserDebugText3D( const double orientation[4], const double textColorRGB[3], double size, double lifeTime, int trackingObjectUniqueId, int optionFlags){return -1;} + + int optionFlags = clientCmd.m_userDebugDrawArgs.m_optionFlags; + + if (clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT_ORIENTATION) + { + optionFlags |= DEB_DEBUG_TEXT_USE_ORIENTATION; + } + + + + + int uid = m_data->m_guiHelper->addUserDebugText3D(clientCmd.m_userDebugDrawArgs.m_text, clientCmd.m_userDebugDrawArgs.m_textPositionXYZ, + clientCmd.m_userDebugDrawArgs.m_textOrientation, clientCmd.m_userDebugDrawArgs.m_textColorRGB, clientCmd.m_userDebugDrawArgs.m_textSize, - clientCmd.m_userDebugDrawArgs.m_lifeTime); + clientCmd.m_userDebugDrawArgs.m_lifeTime, + trackingVisualShapeIndex, + optionFlags); if (uid>=0) { @@ -5908,7 +5955,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_userDebugDrawArgs.m_debugLineToXYZ, clientCmd.m_userDebugDrawArgs.m_debugLineColorRGB, clientCmd.m_userDebugDrawArgs.m_lineWidth, - clientCmd.m_userDebugDrawArgs.m_lifeTime); + clientCmd.m_userDebugDrawArgs.m_lifeTime, + trackingVisualShapeIndex); if (uid>=0) { @@ -5953,14 +6001,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm //static int skip=1; +void PhysicsServerCommandProcessor::syncPhysicsToGraphics() +{ + m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld); +} + + void PhysicsServerCommandProcessor::renderScene(int renderFlags) { if (m_data->m_guiHelper) { - if (0==(renderFlags&COV_DISABLE_SYNC_RENDERING)) - { - m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld); - } m_data->m_guiHelper->render(m_data->m_dynamicsWorld); } #ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index f30d35f2c..6b0f7a58f 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -81,7 +81,9 @@ public: virtual void renderScene(int renderFlags); virtual void physicsDebugDraw(int debugDrawFlags); virtual void setGuiHelper(struct GUIHelperInterface* guiHelper); - + virtual void syncPhysicsToGraphics(); + + //@todo(erwincoumans) Should we have shared memory commands for picking objects? ///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index cf5f0c275..772dbd6d4 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -541,6 +541,7 @@ struct UserDebugDrawLine double m_lifeTime; int m_itemUniqueId; + int m_trackingVisualShapeIndex; }; struct UserDebugParameter @@ -555,12 +556,16 @@ struct UserDebugParameter struct UserDebugText { char m_text[1024]; - double m_textPositionXYZ[3]; + double m_textPositionXYZ1[3]; double m_textColorRGB[3]; double textSize; double m_lifeTime; int m_itemUniqueId; + double m_textOrientation[4]; + int m_trackingVisualShapeIndex; + int m_optionFlags; + }; @@ -783,7 +788,7 @@ public: m_texels(0), m_textureId(-1) { - m_childGuiHelper = guiHelper;; + m_childGuiHelper = guiHelper; } @@ -1078,12 +1083,16 @@ public: { } + virtual void drawText3D( const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag) + { + } + btAlignedObjectArray m_userDebugText; UserDebugText m_tmpText; - virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double textColorRGB[3], double size, double lifeTime) + virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double orientation[4], const double textColorRGB[3], double size, double lifeTime, int trackingVisualShapeIndex, int optionFlags) { m_tmpText.m_itemUniqueId = m_uidGenerator++; @@ -1091,13 +1100,27 @@ public: m_tmpText.textSize = size; //int len = strlen(txt); strcpy(m_tmpText.m_text,txt); - m_tmpText.m_textPositionXYZ[0] = positionXYZ[0]; - m_tmpText.m_textPositionXYZ[1] = positionXYZ[1]; - m_tmpText.m_textPositionXYZ[2] = positionXYZ[2]; + m_tmpText.m_textPositionXYZ1[0] = positionXYZ[0]; + m_tmpText.m_textPositionXYZ1[1] = positionXYZ[1]; + m_tmpText.m_textPositionXYZ1[2] = positionXYZ[2]; + + m_tmpText.m_textOrientation[0] = orientation[0]; + m_tmpText.m_textOrientation[1] = orientation[1]; + m_tmpText.m_textOrientation[2] = orientation[2]; + m_tmpText.m_textOrientation[3] = orientation[3]; + m_tmpText.m_textColorRGB[0] = textColorRGB[0]; m_tmpText.m_textColorRGB[1] = textColorRGB[1]; m_tmpText.m_textColorRGB[2] = textColorRGB[2]; + m_tmpText.m_trackingVisualShapeIndex = trackingVisualShapeIndex; + + m_tmpText.m_optionFlags = optionFlags; + m_tmpText.m_textOrientation[0] = orientation[0]; + m_tmpText.m_textOrientation[1] = orientation[1]; + m_tmpText.m_textOrientation[2] = orientation[2]; + m_tmpText.m_textOrientation[3] = orientation[3]; + m_cs->lock(); m_cs->setSharedParam(1, eGUIUserDebugAddText); workerThreadWait(); @@ -1140,7 +1163,7 @@ public: btAlignedObjectArray m_userDebugLines; UserDebugDrawLine m_tmpLine; - virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime ) + virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex) { m_tmpLine.m_lifeTime = lifeTime; m_tmpLine.m_lineWidth = lineWidth; @@ -1156,7 +1179,7 @@ public: m_tmpLine.m_debugLineColorRGB[0] = debugLineColorRGB[0]; m_tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1]; m_tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2]; - + m_tmpLine.m_trackingVisualShapeIndex = trackingVisualShapeIndex; m_cs->lock(); m_cs->setSharedParam(1, eGUIUserDebugAddLine); workerThreadWait(); @@ -2004,6 +2027,11 @@ void PhysicsServerExample::drawUserDebugLines() for (int i = 0; im_userDebugLines.size(); i++) { + + + + + btVector3 from; from.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[0], m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[1], @@ -2013,6 +2041,26 @@ void PhysicsServerExample::drawUserDebugLines() m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[1], m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[2]); + int graphicsIndex = m_multiThreadedHelper->m_userDebugLines[i].m_trackingVisualShapeIndex; + if (graphicsIndex>=0) + { + CommonRenderInterface* renderer = m_guiHelper->getRenderInterface(); + if (renderer) + { + float parentPos[3]; + float parentOrn[4]; + + if (renderer->readSingleInstanceTransformToCPU(parentPos,parentOrn,graphicsIndex)) + { + btTransform parentTrans; + parentTrans.setOrigin(btVector3(parentPos[0],parentPos[1],parentPos[2])); + parentTrans.setRotation(btQuaternion(parentOrn[0],parentOrn[1],parentOrn[2],parentOrn[3])); + from = parentTrans*from; + toX = parentTrans*toX; + } + } + } + btVector3 color; color.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0], m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1], @@ -2071,11 +2119,77 @@ void PhysicsServerExample::drawUserDebugLines() for (int i = 0; im_userDebugText.size(); i++) { + +// int optionFlag = CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera|CommonGraphicsApp::eDrawText3D_TrueType; + //int optionFlag = CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera; + int optionFlag = 0; + float orientation[4] = {0,0,0,1}; + if (m_multiThreadedHelper->m_userDebugText[i].m_optionFlags&CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera) + { + orientation[0] = m_multiThreadedHelper->m_userDebugText[i].m_textOrientation[0]; + orientation[1] = m_multiThreadedHelper->m_userDebugText[i].m_textOrientation[1]; + orientation[2] = m_multiThreadedHelper->m_userDebugText[i].m_textOrientation[2]; + orientation[3] = m_multiThreadedHelper->m_userDebugText[i].m_textOrientation[3]; + optionFlag |= CommonGraphicsApp::eDrawText3D_TrueType; + } else + { + optionFlag |= CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera; + } + + float colorRGBA[4] = { + m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[0], + m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[1], + m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[2], + 1.}; + + float pos[3] = {m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[0], + m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[1], + m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[2]}; + + int graphicsIndex = m_multiThreadedHelper->m_userDebugText[i].m_trackingVisualShapeIndex; + if (graphicsIndex>=0) + { + CommonRenderInterface* renderer = m_guiHelper->getRenderInterface(); + if (renderer) + { + float parentPos[3]; + float parentOrn[4]; + + if (renderer->readSingleInstanceTransformToCPU(parentPos,parentOrn,graphicsIndex)) + { + btTransform parentTrans; + parentTrans.setOrigin(btVector3(parentPos[0],parentPos[1],parentPos[2])); + parentTrans.setRotation(btQuaternion(parentOrn[0],parentOrn[1],parentOrn[2],parentOrn[3])); + btTransform childTr; + childTr.setOrigin(btVector3(pos[0],pos[1],pos[2])); + childTr.setRotation(btQuaternion(orientation[0],orientation[1],orientation[2],orientation[3])); + + btTransform siteTr = parentTrans*childTr; + pos[0] = siteTr.getOrigin()[0]; + pos[1] = siteTr.getOrigin()[1]; + pos[2] = siteTr.getOrigin()[2]; + btQuaternion siteOrn = siteTr.getRotation(); + orientation[0] = siteOrn[0]; + orientation[1] = siteOrn[1]; + orientation[2] = siteOrn[2]; + orientation[3] = siteOrn[3]; + } + } + } + + + m_guiHelper->getAppInterface()->drawText3D(m_multiThreadedHelper->m_userDebugText[i].m_text, + pos,orientation,colorRGBA, + m_multiThreadedHelper->m_userDebugText[i].textSize,optionFlag); + + + /*m_guiHelper->getAppInterface()->drawText3D(m_multiThreadedHelper->m_userDebugText[i].m_text, m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[0], m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[1], m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[2], m_multiThreadedHelper->m_userDebugText[i].textSize); + */ } } @@ -2260,6 +2374,11 @@ void PhysicsServerExample::renderScene() void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags) { + if (gEnableSyncPhysicsRendering) + { + m_physicsServer.syncPhysicsToGraphics(); + } + drawUserDebugLines(); if (gEnableRendering) diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.cpp b/examples/SharedMemory/PhysicsServerSharedMemory.cpp index b145db6c6..225076910 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsServerSharedMemory.cpp @@ -278,8 +278,11 @@ void PhysicsServerSharedMemory::renderScene(int renderFlags) { m_data->m_commandProcessor->renderScene(renderFlags); - - +} + +void PhysicsServerSharedMemory::syncPhysicsToGraphics() +{ + m_data->m_commandProcessor->syncPhysicsToGraphics(); } void PhysicsServerSharedMemory::physicsDebugDraw(int debugDrawFlags) diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.h b/examples/SharedMemory/PhysicsServerSharedMemory.h index 3e1270c51..e3dc7facf 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.h +++ b/examples/SharedMemory/PhysicsServerSharedMemory.h @@ -43,6 +43,7 @@ public: //to a physics client, over shared memory void physicsDebugDraw(int debugDrawFlags); void renderScene(int renderFlags); + void syncPhysicsToGraphics(); void enableCommandLogging(bool enable, const char* fileName); void replayFromLogFile(const char* fileName); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index c9f1099a5..6a584f86d 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -625,6 +625,9 @@ enum EnumUserDebugDrawFlags USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR = 32, USER_DEBUG_ADD_PARAMETER=64, USER_DEBUG_READ_PARAMETER=128, + USER_DEBUG_HAS_OPTION_FLAGS=256, + USER_DEBUG_HAS_TEXT_ORIENTATION = 512, + USER_DEBUG_HAS_TRACKING_OBJECT=1024, }; @@ -640,8 +643,13 @@ struct UserDebugDrawArgs char m_text[MAX_FILENAME_LENGTH]; double m_textPositionXYZ[3]; + double m_textOrientation[4]; + int m_trackingObjectUniqueId; + int m_trackingLinkIndex; double m_textColorRGB[3]; double m_textSize; + int m_optionFlags; + double m_rangeMin; double m_rangeMax; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 0f4bb31cd..97e462825 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -4,7 +4,8 @@ #define SHARED_MEMORY_KEY 12347 ///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures ///my convention is year/month/day/rev -#define SHARED_MEMORY_MAGIC_NUMBER 201703024 +#define SHARED_MEMORY_MAGIC_NUMBER 201705023 +//#define SHARED_MEMORY_MAGIC_NUMBER 201703024 enum EnumSharedMemoryClientCommand { @@ -507,6 +508,13 @@ enum b3ConfigureDebugVisualizerEnum COV_ENABLE_SYNC_RENDERING_INTERNAL, }; +enum b3AddUserDebugItemEnum +{ + DEB_DEBUG_TEXT_USE_ORIENTATION=1, + DEB_DEBUG_TEXT_USE_TRUE_TYPE_FONTS=2, + DEB_DEBUG_TEXT_HAS_TRACKING_OBJECT=4, +}; + enum eCONNECT_METHOD { eCONNECT_GUI = 1, eCONNECT_DIRECT = 2, diff --git a/examples/SimpleOpenGL3/main.cpp b/examples/SimpleOpenGL3/main.cpp index 998412c1a..c51be2b55 100644 --- a/examples/SimpleOpenGL3/main.cpp +++ b/examples/SimpleOpenGL3/main.cpp @@ -1,23 +1,12 @@ -//#define USE_OPENGL2 -#ifdef USE_OPENGL2 -#include "OpenGLWindow/SimpleOpenGL2App.h" -typedef SimpleOpenGL2App SimpleOpenGLApp ; -#else #include "OpenGLWindow/SimpleOpenGL3App.h" -typedef SimpleOpenGL3App SimpleOpenGLApp ; - -#endif //USE_OPENGL2 - #include "Bullet3Common/b3Quaternion.h" #include "Bullet3Common/b3CommandLineArgs.h" #include "assert.h" #include - - char* gVideoFileName = 0; char* gPngFileName = 0; @@ -30,131 +19,9 @@ static b3KeyboardCallback sOldKeyboardCB = 0; float gWidth = 1024; float gHeight = 768; -SimpleOpenGLApp* app = 0; -float gMouseX = 0; -float gMouseY = 0; -float g_MouseWheel = 0.0f; -int g_MousePressed[3] = {0}; -int g_MousePressed2[3] = {0}; - -//#define B3_USE_IMGUI -#ifdef B3_USE_IMGUI -#include "OpenGLWindow/OpenGLInclude.h" -#include "ThirdPartyLibs/imgui/imgui.h" -static GLuint g_FontTexture = 0; - - - -void ImGui_ImplBullet_CreateDeviceObjects() -{ - ImGuiIO& io = ImGui::GetIO(); - - unsigned char* pixels; - int width, height; - io.Fonts->GetTexDataAsRGBA32(&pixels, &width, &height); // Load as RGBA 32-bits (75% of the memory is wasted, but default font is so small) because it is more likely to be compatible with user's existing shaders. If your ImTextureId represent a higher-level concept than just a GL texture id, consider calling GetTexDataAsAlpha8() instead to save on GPU memory. - - // Upload texture to graphics system - GLint last_texture; - glGetIntegerv(GL_TEXTURE_BINDING_2D, &last_texture); - glGenTextures(1, &g_FontTexture); - glBindTexture(GL_TEXTURE_2D, g_FontTexture); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, pixels); - - // Store our identifier - io.Fonts->TexID = (void *)(intptr_t)g_FontTexture; - - // Restore state - glBindTexture(GL_TEXTURE_2D, last_texture); - -} - - -void ImGui_ImplBullet_RenderDrawLists(ImDrawData* draw_data) -{ - // Avoid rendering when minimized, scale coordinates for retina displays (screen coordinates != framebuffer coordinates) - ImGuiIO& io = ImGui::GetIO(); - int fb_width = (int)(io.DisplaySize.x * io.DisplayFramebufferScale.x); - int fb_height = (int)(io.DisplaySize.y * io.DisplayFramebufferScale.y); - if (fb_width == 0 || fb_height == 0) - return; - draw_data->ScaleClipRects(io.DisplayFramebufferScale); - - // We are using the OpenGL fixed pipeline to make the example code simpler to read! - // Setup render state: alpha-blending enabled, no face culling, no depth testing, scissor enabled, vertex/texcoord/color pointers. - GLint last_texture; glGetIntegerv(GL_TEXTURE_BINDING_2D, &last_texture); - GLint last_viewport[4]; glGetIntegerv(GL_VIEWPORT, last_viewport); - GLint last_scissor_box[4]; glGetIntegerv(GL_SCISSOR_BOX, last_scissor_box); - glPushAttrib(GL_ENABLE_BIT | GL_COLOR_BUFFER_BIT | GL_TRANSFORM_BIT); - glEnable(GL_BLEND); - glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - glDisable(GL_CULL_FACE); - glDisable(GL_DEPTH_TEST); - glEnable(GL_SCISSOR_TEST); - glEnableClientState(GL_VERTEX_ARRAY); - glEnableClientState(GL_TEXTURE_COORD_ARRAY); - glEnableClientState(GL_COLOR_ARRAY); - glEnable(GL_TEXTURE_2D); - //glUseProgram(0); // You may want this if using this code in an OpenGL 3+ context - - // Setup viewport, orthographic projection matrix - glViewport(0, 0, (GLsizei)fb_width, (GLsizei)fb_height); - glMatrixMode(GL_PROJECTION); - glPushMatrix(); - glLoadIdentity(); - glOrtho(0.0f, io.DisplaySize.x, io.DisplaySize.y, 0.0f, -1.0f, +1.0f); - glMatrixMode(GL_MODELVIEW); - glPushMatrix(); - glLoadIdentity(); - - // Render command lists - #define OFFSETOF(TYPE, ELEMENT) ((size_t)&(((TYPE *)0)->ELEMENT)) - for (int n = 0; n < draw_data->CmdListsCount; n++) - { - const ImDrawList* cmd_list = draw_data->CmdLists[n]; - const ImDrawVert* vtx_buffer = cmd_list->VtxBuffer.Data; - const ImDrawIdx* idx_buffer = cmd_list->IdxBuffer.Data; - glVertexPointer(2, GL_FLOAT, sizeof(ImDrawVert), (const GLvoid*)((const char*)vtx_buffer + OFFSETOF(ImDrawVert, pos))); - glTexCoordPointer(2, GL_FLOAT, sizeof(ImDrawVert), (const GLvoid*)((const char*)vtx_buffer + OFFSETOF(ImDrawVert, uv))); - glColorPointer(4, GL_UNSIGNED_BYTE, sizeof(ImDrawVert), (const GLvoid*)((const char*)vtx_buffer + OFFSETOF(ImDrawVert, col))); - - for (int cmd_i = 0; cmd_i < cmd_list->CmdBuffer.Size; cmd_i++) - { - const ImDrawCmd* pcmd = &cmd_list->CmdBuffer[cmd_i]; - if (pcmd->UserCallback) - { - pcmd->UserCallback(cmd_list, pcmd); - } - else - { - glBindTexture(GL_TEXTURE_2D, (GLuint)(intptr_t)pcmd->TextureId); - glScissor((int)pcmd->ClipRect.x, (int)(fb_height - pcmd->ClipRect.w), (int)(pcmd->ClipRect.z - pcmd->ClipRect.x), (int)(pcmd->ClipRect.w - pcmd->ClipRect.y)); - glDrawElements(GL_TRIANGLES, (GLsizei)pcmd->ElemCount, sizeof(ImDrawIdx) == 2 ? GL_UNSIGNED_SHORT : GL_UNSIGNED_INT, idx_buffer); - } - idx_buffer += pcmd->ElemCount; - } - } - #undef OFFSETOF - - // Restore modified state - glDisableClientState(GL_COLOR_ARRAY); - glDisableClientState(GL_TEXTURE_COORD_ARRAY); - glDisableClientState(GL_VERTEX_ARRAY); - glBindTexture(GL_TEXTURE_2D, (GLuint)last_texture); - glMatrixMode(GL_MODELVIEW); - glPopMatrix(); - glMatrixMode(GL_PROJECTION); - glPopMatrix(); - glPopAttrib(); - glViewport(last_viewport[0], last_viewport[1], (GLsizei)last_viewport[2], (GLsizei)last_viewport[3]); - glScissor(last_scissor_box[0], last_scissor_box[1], (GLsizei)last_scissor_box[2], (GLsizei)last_scissor_box[3]); -} -#endif //B3_USE_IMGUI void MyWheelCallback(float deltax, float deltay) { - g_MouseWheel += deltax+deltay; if (sOldWheelCB) sOldWheelCB(deltax,deltay); } @@ -169,29 +36,12 @@ void MyResizeCallback( float width, float height) void MyMouseMoveCallback( float x, float y) { printf("Mouse Move: %f, %f\n", x,y); - gMouseX = x; - gMouseY = y; if (sOldMouseMoveCB) sOldMouseMoveCB(x,y); } void MyMouseButtonCallback(int button, int state, float x, float y) { - gMouseX = x; - gMouseY = y; - { - if (button>=0 && button<3) - { - if (state) - { - g_MousePressed[button] = state; - } - g_MousePressed2[button] = state; - - } - - } - if (sOldMouseButtonCB) sOldMouseButtonCB(button,state,x,y); } @@ -209,56 +59,17 @@ void MyKeyboardCallback(int keycode, int state) } -bool ImGui_ImplGlfw_Init() -{ - - #if 0 - ImGuiIO& io = ImGui::GetIO(); - io.KeyMap[ImGuiKey_Tab] = GLFW_KEY_TAB; // Keyboard mapping. ImGui will use those indices to peek into the io.KeyDown[] array. - io.KeyMap[ImGuiKey_LeftArrow] = GLFW_KEY_LEFT; - io.KeyMap[ImGuiKey_RightArrow] = GLFW_KEY_RIGHT; - io.KeyMap[ImGuiKey_UpArrow] = GLFW_KEY_UP; - io.KeyMap[ImGuiKey_DownArrow] = GLFW_KEY_DOWN; - io.KeyMap[ImGuiKey_PageUp] = GLFW_KEY_PAGE_UP; - io.KeyMap[ImGuiKey_PageDown] = GLFW_KEY_PAGE_DOWN; - io.KeyMap[ImGuiKey_Home] = GLFW_KEY_HOME; - io.KeyMap[ImGuiKey_End] = GLFW_KEY_END; - io.KeyMap[ImGuiKey_Delete] = GLFW_KEY_DELETE; - io.KeyMap[ImGuiKey_Backspace] = GLFW_KEY_BACKSPACE; - io.KeyMap[ImGuiKey_Enter] = GLFW_KEY_ENTER; - io.KeyMap[ImGuiKey_Escape] = GLFW_KEY_ESCAPE; - io.KeyMap[ImGuiKey_A] = GLFW_KEY_A; - io.KeyMap[ImGuiKey_C] = GLFW_KEY_C; - io.KeyMap[ImGuiKey_V] = GLFW_KEY_V; - io.KeyMap[ImGuiKey_X] = GLFW_KEY_X; - io.KeyMap[ImGuiKey_Y] = GLFW_KEY_Y; - io.KeyMap[ImGuiKey_Z] = GLFW_KEY_Z; - - io.RenderDrawListsFn = ImGui_ImplGlfw_RenderDrawLists; // Alternatively you can set this to NULL and call ImGui::GetDrawData() after ImGui::Render() to get the same ImDrawData pointer. - io.SetClipboardTextFn = ImGui_ImplGlfw_SetClipboardText; - io.GetClipboardTextFn = ImGui_ImplGlfw_GetClipboardText; - io.ClipboardUserData = g_Window; -#ifdef _WIN32 - io.ImeWindowHandle = glfwGetWin32Window(g_Window); -#endif - -#endif - - return true; -} - int main(int argc, char* argv[]) { { b3CommandLineArgs myArgs(argc, argv); - app = new SimpleOpenGLApp("SimpleOpenGLApp", gWidth, gHeight); - - app->m_renderer->getActiveCamera()->setCameraDistance(13); - app->m_renderer->getActiveCamera()->setCameraPitch(0); - app->m_renderer->getActiveCamera()->setCameraTargetPosition(0, 0, 0); + SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App", gWidth, gHeight, true); + app->m_instancingRenderer->getActiveCamera()->setCameraDistance(13); + app->m_instancingRenderer->getActiveCamera()->setCameraPitch(0); + app->m_instancingRenderer->getActiveCamera()->setCameraTargetPosition(0, 0, 0); sOldKeyboardCB = app->m_window->getKeyboardCallback(); app->m_window->setKeyboardCallback(MyKeyboardCallback); sOldMouseMoveCB = app->m_window->getMouseMoveCallback(); @@ -290,9 +101,9 @@ int main(int argc, char* argv[]) b3Vector3 pos = b3MakeVector3(0, 0, 0); b3Quaternion orn(0, 0, 0, 1); - b3Vector3 color = b3MakeVector3(1, 0, 0); + b3Vector4 color = b3MakeVector4(1, 0, 0,1); b3Vector3 scaling = b3MakeVector3 (1, 1, 1); - app->m_renderer->registerGraphicsInstance(cubeIndex, pos, orn, color, scaling); + //app->m_renderer->registerGraphicsInstance(cubeIndex, pos, orn, color, scaling); app->m_renderer->writeTransforms(); do @@ -307,8 +118,6 @@ int main(int argc, char* argv[]) app->dumpNextFrameToPng(fileName); } - - //update the texels of the texture using a simple pattern, animated using frame index @@ -328,62 +137,50 @@ int main(int argc, char* argv[]) app->m_renderer->activateTexture(textureHandle); app->m_renderer->updateTexture(textureHandle, image); - //float color[4] = { 255, 1, 1, 1 }; - //app->m_primRenderer->drawTexturedRect(100, 200, gWidth / 2 - 50, gHeight / 2 - 50, color, 0, 0, 1, 1, true); + float color[4] = { 1, 0, 0, 1 }; + app->m_primRenderer->drawTexturedRect(100, 200, gWidth / 2 - 50, gHeight / 2 - 50, color, 0, 0, 1, 1, true); - app->m_renderer->init(); - app->m_renderer->updateCamera(1); + app->m_instancingRenderer->init(); + app->m_instancingRenderer->updateCamera(); app->m_renderer->renderScene(); app->drawGrid(); char bla[1024]; - sprintf(bla, "Simple test frame %d", frameCount); + sprintf(bla, "2d text:%d", frameCount); - //app->drawText(bla, 10, 10); + float yellow[4] = {1,1,0,1}; + app->drawText(bla, 10, 10, 1, yellow); + int optionFlag = 0; + float position[3] = {1,1,1}; + float position2[3] = {0,0,5}; -#ifdef B3_USE_IMGUI - { - bool show_test_window = true; - bool show_another_window = false; - ImVec4 clear_color = ImColor(114, 144, 154); + float orientation[4] = {0,0,0,1}; + + //app->drawText3D(bla,0,0,1,1); - // Start the frame - ImGuiIO& io = ImGui::GetIO(); - if (!g_FontTexture) - ImGui_ImplBullet_CreateDeviceObjects(); + sprintf(bla, "3d bitmap camera facing text:%d", frameCount); + app->drawText3D(bla,position2,orientation,color,1,CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera); + + sprintf(bla, "3d bitmap text:%d", frameCount); + app->drawText3D(bla,position,orientation,color,0.001,0); - io.DisplaySize = ImVec2((float)gWidth, (float)gHeight); - io.DisplayFramebufferScale = ImVec2(gWidth > 0 ? ((float)1.) : 0, gHeight > 0 ? ((float)1.) : 0); - io.DeltaTime = (float)(1.0f/60.0f); - io.MousePos = ImVec2((float)gMouseX, (float)gMouseY); - io.RenderDrawListsFn = ImGui_ImplBullet_RenderDrawLists; + float green[4] = {0,1,0,1}; + float blue[4] = {0,0,1,1}; + sprintf(bla, "3d ttf camera facing text:%d", frameCount); + app->drawText3D(bla,position2,orientation,green,1,CommonGraphicsApp::eDrawText3D_TrueType|CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera); - for (int i=0;i<3;i++) - { - io.MouseDown[i] = g_MousePressed[i]|g_MousePressed2[i]; - g_MousePressed[i] = false; - } + app->drawText3D(bla,position2,orientation,green,1,CommonGraphicsApp::eDrawText3D_TrueType|CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera); + sprintf(bla, "3d ttf text:%d", frameCount); + b3Quaternion orn; + orn.setEulerZYX(B3_HALF_PI/2.,0,B3_HALF_PI/2.); + app->drawText3D(bla,position2,orn,blue,1,CommonGraphicsApp::eDrawText3D_TrueType); - io.MouseWheel = g_MouseWheel; + + //app->drawText3D(bla,position, orientation,color,0.1,optionFlag); + - ImGui::NewFrame(); - - ImGui::ShowTestWindow(); - ImGui::ShowMetricsWindow(); - #if 0 - static float f = 0.0f; - ImGui::Text("Hello, world!"); - ImGui::SliderFloat("float", &f, 0.0f, 1.0f); - ImGui::ColorEdit3("clear color", (float*)&clear_color); - if (ImGui::Button("Test Window")) show_test_window ^= 1; - if (ImGui::Button("Another Window")) show_another_window ^= 1; - ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate); - #endif - ImGui::Render(); - } -#endif //B3_USE_IMGUI app->swapBuffer(); } while (!app->m_window->requestedExit()); @@ -394,4 +191,4 @@ int main(int argc, char* argv[]) delete[] image; } return 0; -} +} \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 16d4ca70a..43d0a1992 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -3077,13 +3077,18 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj PyObject* textPositionObj = 0; PyObject* textColorRGBObj = 0; + PyObject* textOrientationObj = 0; + double textOrientation[4]; + int trackingObjectUniqueId=-1; + int trackingLinkIndex=-1; + double textSize = 1.f; double lifeTime = 0.f; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char* kwlist[] = {"text", "textPosition", "textColorRGB", "textSize", "lifeTime", "physicsClientId", NULL}; + static char* kwlist[] = {"text", "textPosition", "textColorRGB", "textSize", "lifeTime", "textOrientation", "trackObjectUniqueId", "trackLinkIndex", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "sO|Oddi", kwlist, &text, &textPositionObj, &textColorRGBObj, &textSize, &lifeTime, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "sO|OddOiii", kwlist, &text, &textPositionObj, &textColorRGBObj, &textSize, &lifeTime, &textOrientationObj, &trackingObjectUniqueId, &trackingLinkIndex, &physicsClientId)) { return NULL; } @@ -3113,6 +3118,23 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj commandHandle = b3InitUserDebugDrawAddText3D(sm, text, posXYZ, colorRGB, textSize, lifeTime); + if (trackingObjectUniqueId>=0) + { + b3UserDebugItemSetTrackingObject(commandHandle, trackingObjectUniqueId,trackingLinkIndex); + } + if (textOrientationObj) + { + res = pybullet_internalSetVector4d(textOrientationObj, textOrientation); + if (!res) + { + PyErr_SetString(SpamError, "Error converting textOrientation[4]"); + return NULL; + } else + { + b3UserDebugTextSetOrientation(commandHandle,textOrientation); + } + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) @@ -3136,6 +3158,8 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj double fromXYZ[3]; double toXYZ[3]; double colorRGB[3] = {1, 1, 1}; + int trackingObjectUniqueId=-1; + int trackingLinkIndex=-1; PyObject* lineFromObj = 0; PyObject* lineToObj = 0; @@ -3144,9 +3168,9 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj double lifeTime = 0.f; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char* kwlist[] = {"lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", "physicsClientId", NULL}; + static char* kwlist[] = {"lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", "trackObjectUniqueId", "trackLinkIndex", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Oddi", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj, &lineWidth, &lifeTime, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Oddiii", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj, &lineWidth, &lifeTime, &trackingObjectUniqueId, &trackingLinkIndex, &physicsClientId)) { return NULL; } @@ -3177,6 +3201,11 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj commandHandle = b3InitUserDebugDrawAddLine3D(sm, fromXYZ, toXYZ, colorRGB, lineWidth, lifeTime); + if (trackingObjectUniqueId>=0) + { + b3UserDebugItemSetTrackingObject(commandHandle, trackingObjectUniqueId,trackingLinkIndex); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED)