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

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 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		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		addUserDebugParameter(const char* txt, double	rangeMin, double	rangeMax, double startValue){return -1;};
 	virtual int		readUserDebugParameter(int itemUniqueId, double* value) { return 0;}
 
@@ -175,11 +174,11 @@ struct DummyGUIHelper : public GUIHelperInterface
 	{
 	}
 	
-	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;
 	}

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 drawText( const char* txt, int posX, int posY)
-	virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)=0;
+	{
+		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;

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 int registerTexture(const unsigned char* texels, int width, int height, bool flipPixelsY=true)=0;
-    virtual void updateTexture(int textureIndex, const unsigned char* texels)=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;

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");

examples/ExampleBrowser/OpenGLGuiHelper.h

@@ -58,6 +58,8 @@ struct OpenGLGuiHelper : public GUIHelperInterface
 
 	virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) ;
 
+	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)

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 ((srcIndex<m_data->m_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,10 +881,15 @@ void    GLInstancingRenderer::updateTexture(int textureIndex, const unsigned cha
         glActiveTexture(GL_TEXTURE0);
         b3Assert(glGetError() ==GL_NO_ERROR);
         InternalTextureHandle& h = m_data->m_textureHandles[textureIndex];
+		glBindTexture(GL_TEXTURE_2D,h.m_glTexture);
+        b3Assert(glGetError() ==GL_NO_ERROR);
 
+		if (flipPixelsY)
+		{
 			//textures need to be flipped for OpenGL...
 			b3AlignedObjectArray<unsigned char> 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++)
@@ -830,11 +900,11 @@ void    GLInstancingRenderer::updateTexture(int textureIndex, const unsigned cha
 				}
 			}
 
-
-        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]);
+		} 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,6 +2124,9 @@ b3Assert(glGetError() ==GL_NO_ERROR);
                             {
                                 glDisable (GL_BLEND);
                             }
+							glActiveTexture(GL_TEXTURE1);
+							glBindTexture(GL_TEXTURE_2D,0);
+
 							glActiveTexture(GL_TEXTURE0);
 							glBindTexture(GL_TEXTURE_2D,0);
                             break;

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 int registerTexture(const unsigned char* texels, int width, int height, bool flipPixelsY=true);
-	virtual void    updateTexture(int textureIndex, const unsigned char* texels);
+    virtual void updateTexture(int textureIndex, const unsigned char* texels, bool flipPixelsY=true);
     virtual void activateTexture(int textureIndex);
 
 
@@ -76,6 +76,7 @@ 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;

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";
 
 

examples/OpenGLWindow/SimpleOpenGL2App.cpp

@@ -297,7 +297,8 @@ 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])
 {
 
 }
@@ -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());

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]);
 
     

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)
     {
@@ -421,7 +426,12 @@ void    SimpleOpenGL2Renderer::updateTexture(int textureIndex, const unsigned ch
         glActiveTexture(GL_TEXTURE0);
         b3Assert(glGetError() ==GL_NO_ERROR);
 		InternalTextureHandle2& h = m_data->m_textureHandles[textureIndex];
+		glBindTexture(GL_TEXTURE_2D,h.m_glTexture);
+        b3Assert(glGetError() ==GL_NO_ERROR);
 
+       
+		if (flipTexelsY)
+		{
 			//textures need to be flipped for OpenGL...
 			b3AlignedObjectArray<unsigned char> flippedTexels;
 			flippedTexels.resize(h.m_width* h.m_height * 3);
@@ -434,12 +444,16 @@ void    SimpleOpenGL2Renderer::updateTexture(int textureIndex, const unsigned ch
 					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);
-        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]);
+    
+		} 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]);
+    
+		}
+
 		b3Assert(glGetError() ==GL_NO_ERROR);
         glGenerateMipmap(GL_TEXTURE_2D);
         b3Assert(glGetError() ==GL_NO_ERROR);

examples/OpenGLWindow/SimpleOpenGL2Renderer.h

@@ -34,20 +34,23 @@ 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 int	registerTexture(const unsigned char* texels, int width, int height, bool flipTexelsY);
-	virtual void    updateTexture(int textureIndex, const unsigned char* texels);
+	virtual void	updateTexture(int textureIndex, const unsigned char* texels, bool flipTexelsY);
     virtual void	activateTexture(int textureIndex);
 
 

examples/OpenGLWindow/SimpleOpenGL3App.cpp

@@ -33,7 +33,7 @@
 #include <assert.h>
 #include "GLRenderToTexture.h"
 #include "Bullet3Common/b3Quaternion.h"
-
+#include <string>//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<unsigned char> 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;i<textureWidth*textureHeight;i++)
+				{
+					m_rgbaTexture[i*3+0] = texture->m_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<CommonGfxVertex3D> verts;
+					verts.resize(numVertices);
+					for (int i=0;i<numVertices;i++)
+					{
+						verts[i].x = halfExtentsX*cube_vertices_textured[i*9];
+						verts[i].y = halfExtentsY*cube_vertices_textured[i*9+1];
+						verts[i].z = halfExtentsZ*cube_vertices_textured[i*9+2];
+						verts[i].w = cube_vertices_textured[i*9+3];
+						verts[i].nx = cube_vertices_textured[i*9+4];
+						verts[i].ny = cube_vertices_textured[i*9+5];
+						verts[i].nz = cube_vertices_textured[i*9+6];
+						verts[i].u = cube_vertices_textured[i*9+7]*textureScaling;
+						verts[i].v = cube_vertices_textured[i*9+8]*textureScaling;
+					}
+	
+				
+					int shapeId = m_instancingRenderer->registerShape(&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;i<indices.size();i++)
+		{
+			indices[i] = i;
+		}
+
+		m_instancingRenderer->drawTexturedTriangleMesh(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,6 +390,11 @@ SimpleOpenGL3App::SimpleOpenGL3App(	const char* title, int width,int height, boo
 	{
 		b3Warning("error!\n");
 	}
+	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,30 +433,55 @@ 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 posX = position[0];
-	float posY = 100.f;
+	float posY = position[1];
+	float posZ = position[2];
 	float winx,winy, winz;
 
-	if (!projectWorldCoordToScreen(worldPosX, worldPosY, worldPosZ,viewMat,projMat,viewport,&winx, &winy, &winz))
+	
+	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)
-	if (0)//m_useTrueTypeFont)
 	{
 		bool measureOnly = false;
 
-		float fontSize= 32;//64;//512;//128;
+		float fontSize= 64;//512;//128;
 		
+		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());
+						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;
 		int pos=0;
@@ -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<GfxVertex> verts;
+	b3AlignedObjectArray<CommonGfxVertex3D> verts;
 	verts.resize(numVertices);
 	for (int i=0;i<numVertices;i++)
 	{
@@ -663,6 +885,10 @@ SimpleOpenGL3App::~SimpleOpenGL3App()
 	delete m_primRenderer ;
 	sth_delete(m_data->m_fontStash);
 	delete m_data->m_renderCallbacks;
+
+	sth_delete(m_data->m_fontStash2);
+	delete m_data->m_renderCallbacks2;
+
 	TwDeleteDefaultFonts();
 	m_window->closeWindow();
 	

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();
 

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[0] = colorRGBA[0];
-    v->colour.p[1] = 0.1f;
+    v->colour.p[1] = colorRGBA[1];
-    v->colour.p[2] = 0.1f;
+    v->colour.p[2] = colorRGBA[2];
-    v->colour.p[3] = 1.f;
+    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.x0, q.y0, 0,0,(float)screenwidth,(float)screenheight,colorRGBA);
-		v = setv(v, q.x1, q.y0, 1,0,(float)screenwidth,(float)screenheight);
+		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);
+		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.x0, q.y0, 0,0,(float)screenwidth,(float)screenheight,colorRGBA);
-		v = setv(v, q.x1, q.y1, 1,1,(float)screenwidth,(float)screenheight);
+		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);
+		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.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);
+            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);
+            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.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);
+            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);
+            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;
         }

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);

examples/OpenGLWindow/opengl_fontstashcallbacks.cpp

@@ -14,8 +14,8 @@
 
 
 static unsigned int s_indexData[INDEX_COUNT];
-GLuint s_indexArrayObject, s_indexBuffer;
+static GLuint s_indexArrayObject, s_indexBuffer;
-GLuint s_vertexArrayObject,s_vertexBuffer;
+static GLuint s_vertexArrayObject,s_vertexBuffer;
 
 OpenGL2RenderCallbacks::OpenGL2RenderCallbacks(GLPrimitiveRenderer* primRender)
 	:m_primRender2(primRender)
@@ -54,6 +54,12 @@ 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);

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();
 

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;
 }
 

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);

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;

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

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -81,6 +81,8 @@ 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

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<UserDebugText> 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_textPositionXYZ1[0] = positionXYZ[0];
-		m_tmpText.m_textPositionXYZ[1] = positionXYZ[1];
+		m_tmpText.m_textPositionXYZ1[1] = positionXYZ[1];
-		m_tmpText.m_textPositionXYZ[2] = positionXYZ[2];
+		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<UserDebugDrawLine> 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; i<m_multiThreadedHelper->m_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; i<m_multiThreadedHelper->m_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)

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)

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);

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;

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,

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 <stdio.h>
 
-
-
 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);
+		SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App", gWidth, gHeight, true);
-
-		app->m_renderer->getActiveCamera()->setCameraDistance(13);
-		app->m_renderer->getActiveCamera()->setCameraPitch(0);
-		app->m_renderer->getActiveCamera()->setCameraTargetPosition(0, 0, 0);
 
+		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
@@ -309,8 +120,6 @@ int main(int argc, char* argv[])
 
 
 
-
-
 			//update the texels of the texture using a simple pattern, animated using frame index
 			for (int y = 0; y < textureHeight; ++y)
 			{
@@ -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 };
+			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_primRenderer->drawTexturedRect(100, 200, gWidth / 2 - 50, gHeight / 2 - 50, color, 0, 0, 1, 1, true);
 
 
-			app->m_renderer->init();
+			app->m_instancingRenderer->init();
-			app->m_renderer->updateCamera(1);
+			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
+			float orientation[4] = {0,0,0,1};
-			{
-				bool show_test_window = true;
-				bool show_another_window = false;
-				ImVec4 clear_color = ImColor(114, 144, 154);
 			
-				 // Start the frame
+			//app->drawText3D(bla,0,0,1,1);
-				ImGuiIO& io = ImGui::GetIO();
-				if (!g_FontTexture)
-			        ImGui_ImplBullet_CreateDeviceObjects();
 
-				io.DisplaySize = ImVec2((float)gWidth, (float)gHeight);
+			sprintf(bla, "3d bitmap camera facing text:%d", frameCount);			
-				io.DisplayFramebufferScale = ImVec2(gWidth > 0 ? ((float)1.) : 0, gHeight > 0 ? ((float)1.) : 0);
+			app->drawText3D(bla,position2,orientation,color,1,CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera);
-				io.DeltaTime = (float)(1.0f/60.0f);
+			
-				io.MousePos = ImVec2((float)gMouseX, (float)gMouseY);   
+			sprintf(bla, "3d bitmap text:%d", frameCount);			
-				io.RenderDrawListsFn = ImGui_ImplBullet_RenderDrawLists;
+			app->drawText3D(bla,position,orientation,color,0.001,0);
+
+			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);
+
+			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);
 
 			
-				for (int i=0;i<3;i++)
+			//app->drawText3D(bla,position, orientation,color,0.1,optionFlag);
-				{
-					io.MouseDown[i] = g_MousePressed[i]|g_MousePressed2[i];
-					g_MousePressed[i] = false;
-				}
 		
-				io.MouseWheel = g_MouseWheel;
 
-				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());
 

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)