Implementation of virtual void CommonRenderInterface::removeGraphicsInstance(int instanceUid)

for GLInstancingRenderer (OpenGL3+) and SimpleOpenGL2Renderer (OpenGL2)
Refactored the add/remove object pool in Bullet3Common/b3ResizablePool.h
Added CommonRigidBodyBase::deleteRigidBody, also removing its graphics instance.

examples/CommonInterfaces/CommonGUIHelperInterface.h

@@ -37,6 +37,7 @@ struct GUIHelperInterface
 	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) = 0;
 	virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) =0;
     virtual void removeAllGraphicsInstances()=0;
+	virtual void removeGraphicsInstance(int graphicsUid)=0;
 
 	virtual Common2dCanvasInterface* get2dCanvasInterface()=0;
 	
@@ -123,6 +124,7 @@ struct DummyGUIHelper : public GUIHelperInterface
 	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId){return -1;}
 	virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) {return -1;}
     virtual void removeAllGraphicsInstances(){}
+	virtual void removeGraphicsInstance(int graphicsUid){}
 
 	virtual Common2dCanvasInterface* get2dCanvasInterface()
 	{

examples/CommonInterfaces/CommonRenderInterface.h

@@ -23,6 +23,7 @@ struct CommonRenderInterface
 	virtual void init()=0;
 	virtual void updateCamera(int upAxis)=0;
 	virtual void removeAllInstances() = 0;
+	virtual void removeGraphicsInstance(int instanceUid) = 0;
 
 	virtual const CommonCameraInterface* getActiveCamera() const =0;
 	virtual CommonCameraInterface* getActiveCamera()=0;

examples/CommonInterfaces/CommonRigidBodyBase.h

@@ -410,6 +410,18 @@ struct CommonRigidBodyBase : public CommonExampleInterface
 		return box;
 	}
 
+	void deleteRigidBody(btRigidBody* body)
+	{
+		int graphicsUid = body->getUserIndex();
+		m_guiHelper->removeGraphicsInstance(graphicsUid);
+
+		m_dynamicsWorld->removeRigidBody(body);
+		btMotionState* ms = body->getMotionState();
+		delete body;
+		delete ms;
+
+	}
+
 	btRigidBody*	createRigidBody(float mass, const btTransform& startTransform, btCollisionShape* shape,  const btVector4& color = btVector4(1, 0, 0, 1))
 	{
 		btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));

examples/ExampleBrowser/OpenGLGuiHelper.cpp

@@ -241,6 +241,12 @@ void OpenGLGuiHelper::removeAllGraphicsInstances()
     m_data->m_glApp->m_renderer->removeAllInstances();
 }
 
+void OpenGLGuiHelper::removeGraphicsInstance(int graphicsUid)
+{
+	m_data->m_glApp->m_renderer->removeGraphicsInstance(graphicsUid);
+}
+
+
 void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
 {
 	//already has a graphics object?
@@ -587,7 +593,7 @@ void OpenGLGuiHelper::autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWor
 		btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i];
 		sortedObjects.push_back(colObj);
 	}
-	sortedObjects.quickSort(shapePointerCompareFunc);
+	//sortedObjects.quickSort(shapePointerCompareFunc);
 	for (int i=0;i<sortedObjects.size();i++)
 	{
 		btCollisionObject* colObj = sortedObjects[i];

examples/ExampleBrowser/OpenGLGuiHelper.h

@@ -25,6 +25,7 @@ struct OpenGLGuiHelper : public GUIHelperInterface
 	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId);
 	virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling);
 	virtual void removeAllGraphicsInstances();
+	virtual void removeGraphicsInstance(int graphicsUid);
 
 	virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape);
 

examples/OpenGLWindow/GLInstancingRenderer.cpp

@@ -53,6 +53,8 @@ float shadowMapWorldSize=10;
 #include "Bullet3Common/b3Vector3.h"
 #include "Bullet3Common/b3Quaternion.h"
 #include "Bullet3Common/b3Matrix3x3.h"
+#include "Bullet3Common/b3ResizablePool.h"
+
 #include "LoadShader.h"
 
 #include "GLInstanceRendererInternalData.h"
@@ -90,7 +92,7 @@ struct b3GraphicsInstance
 	int m_numVertices;
 
 	int m_numGraphicsInstances;
-
+	b3AlignedObjectArray<int> m_tempObjectUids;
 	int m_instanceOffset;
 	int m_vertexArrayOffset;
 	int	m_primitiveType;
@@ -144,7 +146,18 @@ struct InternalTextureHandle
     int m_height;
 };
 
+struct b3PublicGraphicsInstanceData
+{
+	int m_shapeIndex;
+	int m_internalInstanceIndex;
+	GLfloat m_position[4];
+	GLfloat m_orientation[4];
+	GLfloat m_color[4];
+	GLfloat m_scale[4];
 
+};
+
+typedef b3PoolBodyHandle<b3PublicGraphicsInstanceData> b3PublicGraphicsInstance;
 
 struct InternalDataRenderer : public GLInstanceRendererInternalData
 {
@@ -166,6 +179,9 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
 	GLuint				m_renderFrameBuffer;
 	
 
+
+	b3ResizablePool< b3PublicGraphicsInstance> m_publicGraphicsInstances;
+
 	InternalDataRenderer() :
 	m_activeCamera(&m_defaultCamera1),
 		m_shadowMap(0),
@@ -284,6 +300,8 @@ void GLInstancingRenderer::removeAllInstances()
 		delete m_graphicsInstances[i];
 	}
 	m_graphicsInstances.clear();
+	m_data->m_publicGraphicsInstances.exitHandles();
+	m_data->m_publicGraphicsInstances.initHandles();
 }
 
 
@@ -313,8 +331,13 @@ GLInstancingRenderer::~GLInstancingRenderer()
 
 
 
-void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex)
+void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int bodyUniqueId)
 {
+
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
+	b3Assert(pg);
+	int srcIndex = pg->m_internalInstanceIndex;
+
 	b3Assert(srcIndex<m_data->m_totalNumInstances);
 	b3Assert(srcIndex>=0);
 	m_data->m_instance_positions_ptr[srcIndex*4+0]=position[0];
@@ -327,16 +350,16 @@ void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* positi
 	m_data->m_instance_quaternion_ptr[srcIndex*4+2]=orientation[2];
 	m_data->m_instance_quaternion_ptr[srcIndex*4+3]=orientation[3];
 
-/*	m_data->m_instance_colors_ptr[srcIndex*4+0]=color[0];
-	m_data->m_instance_colors_ptr[srcIndex*4+1]=color[1];
-	m_data->m_instance_colors_ptr[srcIndex*4+2]=color[2];
-	m_data->m_instance_colors_ptr[srcIndex*4+3]=color[3];
-	*/
 }
 
 
-void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int srcIndex, float* position, float* orientation)
+void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int bodyUniqueId, float* position, float* orientation)
 {
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
+	b3Assert(pg);
+	int srcIndex = pg->m_internalInstanceIndex;
+
+
 	b3Assert(srcIndex<m_data->m_totalNumInstances);
 	b3Assert(srcIndex>=0);
 	position[0] = m_data->m_instance_positions_ptr[srcIndex*4+0];
@@ -348,16 +371,23 @@ void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int srcIndex, floa
 	orientation[2] = m_data->m_instance_quaternion_ptr[srcIndex*4+2];
 	orientation[3] = m_data->m_instance_quaternion_ptr[srcIndex*4+3];
 }
-void GLInstancingRenderer::writeSingleInstanceColorToCPU(double* color, int srcIndex)
+void GLInstancingRenderer::writeSingleInstanceColorToCPU(double* color, int bodyUniqueId)
 {
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
+	b3Assert(pg);
+	int srcIndex = pg->m_internalInstanceIndex;
+
 	m_data->m_instance_colors_ptr[srcIndex*4+0]=float(color[0]);
 	m_data->m_instance_colors_ptr[srcIndex*4+1]=float(color[1]);
 	m_data->m_instance_colors_ptr[srcIndex*4+2]=float(color[2]);
 	m_data->m_instance_colors_ptr[srcIndex*4+3]=float(color[3]);
 }
 
-void GLInstancingRenderer::writeSingleInstanceColorToCPU(float* color, int srcIndex)
+void GLInstancingRenderer::writeSingleInstanceColorToCPU(float* color, int bodyUniqueId)
 {
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
+	b3Assert(pg);
+	int srcIndex = pg->m_internalInstanceIndex;
 
 	m_data->m_instance_colors_ptr[srcIndex*4+0]=color[0];
 	m_data->m_instance_colors_ptr[srcIndex*4+1]=color[1];
@@ -365,25 +395,37 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(float* color, int srcIn
 	m_data->m_instance_colors_ptr[srcIndex*4+3]=color[3];
 }
 
-void GLInstancingRenderer::writeSingleInstanceScaleToCPU(float* scale, int srcIndex)
+void GLInstancingRenderer::writeSingleInstanceScaleToCPU(float* scale, int bodyUniqueId)
 {
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
+	b3Assert(pg);
+	int srcIndex = pg->m_internalInstanceIndex;
+
 	m_data->m_instance_scale_ptr[srcIndex*3+0]=scale[0];
 	m_data->m_instance_scale_ptr[srcIndex*3+1]=scale[1];
 	m_data->m_instance_scale_ptr[srcIndex*3+2]=scale[2];
 }
 
-void GLInstancingRenderer::writeSingleInstanceScaleToCPU(double* scale, int srcIndex)
+void GLInstancingRenderer::writeSingleInstanceScaleToCPU(double* scale, int bodyUniqueId)
 {
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
+	b3Assert(pg);
+	int srcIndex = pg->m_internalInstanceIndex;
+
 	m_data->m_instance_scale_ptr[srcIndex*3+0]=scale[0];
 	m_data->m_instance_scale_ptr[srcIndex*3+1]=scale[1];
 	m_data->m_instance_scale_ptr[srcIndex*3+2]=scale[2];
 }
 
-void GLInstancingRenderer::writeSingleInstanceTransformToGPU(float* position, float* orientation, int objectIndex)
+void GLInstancingRenderer::writeSingleInstanceTransformToGPU(float* position, float* orientation, int objectUniqueId)
 {
 	glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
 	//glFlush();
 
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(objectUniqueId);
+	b3Assert(pg);
+	int objectIndex = pg->m_internalInstanceIndex;
+
 	char* orgBase =  (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_READ_WRITE);
 	//b3GraphicsInstance* gfxObj = m_graphicsInstances[k];
 	int totalNumInstances= 0;
@@ -574,15 +616,92 @@ int GLInstancingRenderer::registerGraphicsInstance(int shapeIndex, const double*
     return registerGraphicsInstance(shapeIndex,pos,orn,color,scaling);
 }
 
-
+void GLInstancingRenderer::rebuildGraphicsInstances()
-int GLInstancingRenderer::registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
 {
-	b3Assert(shapeIndex == (m_graphicsInstances.size()-1));
+	m_data->m_totalNumInstances = 0;
-	b3Assert(m_graphicsInstances.size()<m_data->m_maxNumObjectCapacity-1);
+
+	b3AlignedObjectArray<int> usedObjects;
+	m_data->m_publicGraphicsInstances.getUsedHandles(usedObjects);
+
+	for (int i=0;i<usedObjects.size();i++)
+	{
+		int bodyUniqueId = usedObjects[i];
+		b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
+		b3Assert(pg);
+		int srcIndex = pg->m_internalInstanceIndex;
+
+		pg->m_position[0] = m_data->m_instance_positions_ptr[srcIndex*4+0];
+		pg->m_position[1] = m_data->m_instance_positions_ptr[srcIndex*4+1];
+		pg->m_position[2] = m_data->m_instance_positions_ptr[srcIndex*4+2];
+		pg->m_orientation[0] = m_data->m_instance_quaternion_ptr[srcIndex*4+0];
+		pg->m_orientation[1] = m_data->m_instance_quaternion_ptr[srcIndex*4+1];
+		pg->m_orientation[2] = m_data->m_instance_quaternion_ptr[srcIndex*4+2];
+		pg->m_orientation[3] = m_data->m_instance_quaternion_ptr[srcIndex*4+3];
+		pg->m_color[0] = m_data->m_instance_colors_ptr[srcIndex*4+0];
+		pg->m_color[1] = m_data->m_instance_colors_ptr[srcIndex*4+1];
+		pg->m_color[2] = m_data->m_instance_colors_ptr[srcIndex*4+2];
+		pg->m_color[3] = m_data->m_instance_colors_ptr[srcIndex*4+3];
+		pg->m_scale[0] = m_data->m_instance_scale_ptr[srcIndex*3+0];
+		pg->m_scale[1] = m_data->m_instance_scale_ptr[srcIndex*3+1];
+		pg->m_scale[2] = m_data->m_instance_scale_ptr[srcIndex*3+2];
+	}
+	for (int i=0;i<m_graphicsInstances.size();i++)
+	{
+		m_graphicsInstances[i]->m_numGraphicsInstances = 0;
+		m_graphicsInstances[i]->m_instanceOffset = 0;
+		m_graphicsInstances[i]->m_tempObjectUids.clear();
+	}
+	for (int i=0;i<usedObjects.size();i++)
+	{
+		int bodyUniqueId = usedObjects[i];
+		b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
+		m_graphicsInstances[pg->m_shapeIndex]->m_tempObjectUids.push_back(bodyUniqueId);
+	}
+
+	int curOffset = 0;
+	m_data->m_totalNumInstances = 0;
+
+	for (int i=0;i<m_graphicsInstances.size();i++)
+	{
+		m_graphicsInstances[i]->m_instanceOffset = curOffset;
+		m_graphicsInstances[i]->m_numGraphicsInstances = 0;
+
+		for (int g=0;g<m_graphicsInstances[i]->m_tempObjectUids.size();g++)
+		{
+			curOffset++;
+			int objectUniqueId = m_graphicsInstances[i]->m_tempObjectUids[g];
+			b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(objectUniqueId);
+			
+			registerGraphicsInstanceInternal(objectUniqueId,pg->m_position,pg->m_orientation,pg->m_color,pg->m_scale);
+		}
+	}
+
+}
+
+void GLInstancingRenderer::removeGraphicsInstance(int instanceUid)
+{
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(instanceUid);
+	b3Assert(pg);
+	if (pg)
+	{
+		m_data->m_publicGraphicsInstances.freeHandle(instanceUid);
+		rebuildGraphicsInstances();
+	}
+}
+
+
+int GLInstancingRenderer::registerGraphicsInstanceInternal(int newUid, const float* position, const float* quaternion, const float* color, const float* scaling)
+{
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(newUid);
+	int shapeIndex = pg->m_shapeIndex;
+//	b3Assert(pg);
+//	int objectIndex = pg->m_internalInstanceIndex;
+
+	
 
 	b3GraphicsInstance* gfxObj = m_graphicsInstances[shapeIndex];
-
 	int index = gfxObj->m_numGraphicsInstances + gfxObj->m_instanceOffset;
+	pg->m_internalInstanceIndex = index;
 
 	int maxElements = m_data->m_instance_positions_ptr.size();
 	if (index*4<maxElements)
@@ -613,7 +732,50 @@ int GLInstancingRenderer::registerGraphicsInstance(int shapeIndex, const float*
 		b3Error("registerGraphicsInstance out of range, %d\n", maxElements);
 		return -1;
 	}
-	return index;//gfxObj->m_numGraphicsInstances;
+	return newUid;//gfxObj->m_numGraphicsInstances;
+}
+
+int GLInstancingRenderer::registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
+{
+	int newUid = m_data->m_publicGraphicsInstances.allocHandle();
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(newUid);
+	pg->m_shapeIndex = shapeIndex;
+
+	//b3Assert(shapeIndex == (m_graphicsInstances.size()-1));
+	b3Assert(m_graphicsInstances.size()<m_data->m_maxNumObjectCapacity-1);
+	if (shapeIndex == (m_graphicsInstances.size()-1))
+	{
+		registerGraphicsInstanceInternal(newUid, position,quaternion,color,scaling);
+	} else
+	{
+
+		int srcIndex = m_data->m_totalNumInstances++;
+		pg->m_internalInstanceIndex = srcIndex;
+
+		m_data->m_instance_positions_ptr[srcIndex*4+0] = position[0];
+		m_data->m_instance_positions_ptr[srcIndex*4+1] = position[1];
+		m_data->m_instance_positions_ptr[srcIndex*4+2] = position[2];
+		m_data->m_instance_positions_ptr[srcIndex*4+3] = 1.;
+
+		m_data->m_instance_quaternion_ptr[srcIndex*4+0] = quaternion[0];
+		m_data->m_instance_quaternion_ptr[srcIndex*4+1] = quaternion[1];
+		m_data->m_instance_quaternion_ptr[srcIndex*4+2] = quaternion[2];
+		m_data->m_instance_quaternion_ptr[srcIndex*4+3] = quaternion[3];
+
+		m_data->m_instance_colors_ptr[srcIndex*4+0] = color[0];
+		m_data->m_instance_colors_ptr[srcIndex*4+1] = color[1];
+		m_data->m_instance_colors_ptr[srcIndex*4+2] = color[2];
+		m_data->m_instance_colors_ptr[srcIndex*4+3] = color[3];
+
+		m_data->m_instance_scale_ptr[srcIndex*3+0] = scaling[0];
+		m_data->m_instance_scale_ptr[srcIndex*3+1] = scaling[1];
+		m_data->m_instance_scale_ptr[srcIndex*3+2] = scaling[2];
+
+
+		rebuildGraphicsInstances();
+	}
+
+	return newUid;
 }
 
 

examples/OpenGLWindow/GLInstancingRenderer.h

@@ -41,8 +41,8 @@ class GLInstancingRenderer : public CommonRenderInterface
 	int m_upAxis;
     bool m_enableBlend;
     
-	
+	int registerGraphicsInstanceInternal(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling);
-
+	void rebuildGraphicsInstances();
 
 	
 public:
@@ -57,6 +57,7 @@ public:
 	void InitShaders();
 	void CleanupShaders();
 	virtual void removeAllInstances();
+	virtual void removeGraphicsInstance(int instanceUid);
 
 	virtual void updateShape(int shapeIndex, const float* vertices);
 
@@ -72,6 +73,7 @@ public:
 	virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling);
 	virtual int registerGraphicsInstance(int shapeIndex, const double* position, const double* quaternion, const double* color, const double* scaling);
 
+
 	void writeTransforms();
 
 	

examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp

@@ -6,6 +6,7 @@
 #include "GLInstanceGraphicsShape.h"
 #include "Bullet3Common/b3Quaternion.h"
 #include "Bullet3Common/b3Transform.h"
+#include "Bullet3Common/b3ResizablePool.h"
 
 B3_ATTRIBUTE_ALIGNED16(struct) SimpleGL2Shape
 {
@@ -38,13 +39,18 @@ struct InternalTextureHandle2
     int m_height;
 };
 
+typedef b3PoolBodyHandle<SimpleGL2Instance> SimpleGL2InstanceHandle;
+
 struct SimpleOpenGL2RendererInternalData
 {
 	int m_width;
     int m_height;
     SimpleCamera	m_camera;
 	b3AlignedObjectArray<SimpleGL2Shape*> m_shapes;
-	b3AlignedObjectArray<SimpleGL2Instance> m_graphicsInstances;
+	//b3AlignedObjectArray<SimpleGL2Instance> m_graphicsInstances1;
+
+	b3ResizablePool<SimpleGL2InstanceHandle> m_graphicsInstancesPool;
+
 	b3AlignedObjectArray<InternalTextureHandle2>	m_textureHandles;
 
 };
@@ -118,11 +124,17 @@ void SimpleOpenGL2Renderer::removeAllInstances()
 		delete m_data->m_shapes[i];
 	}
 	m_data->m_shapes.clear();
-	m_data->m_graphicsInstances.clear();
+	m_data->m_graphicsInstancesPool.exitHandles();
+	m_data->m_graphicsInstancesPool.initHandles();
+	
 	//also destroy textures?
 	m_data->m_textureHandles.clear();
 }
 
+void SimpleOpenGL2Renderer::removeGraphicsInstance(int instanceUid)
+{
+	m_data->m_graphicsInstancesPool.freeHandle(instanceUid);
+}
 
 void SimpleOpenGL2Renderer::writeSingleInstanceColorToCPU(float* color, int srcIndex)
 {
@@ -142,7 +154,7 @@ void SimpleOpenGL2Renderer::writeSingleInstanceScaleToCPU(double* scale, int src
 
 int SimpleOpenGL2Renderer::getTotalNumInstances() const
 {
-    return m_data->m_graphicsInstances.size();
+    return m_data->m_graphicsInstancesPool.getNumHandles();
 }
 
 void	SimpleOpenGL2Renderer::getCameraViewMatrix(float viewMat[16]) const
@@ -158,7 +170,13 @@ void	SimpleOpenGL2Renderer::getCameraProjectionMatrix(float projMat[16]) const
 
 void SimpleOpenGL2Renderer::drawOpenGL(int instanceIndex)
 {
-	const SimpleGL2Instance& inst = m_data->m_graphicsInstances[instanceIndex];
+	const SimpleGL2Instance* instPtr = m_data->m_graphicsInstancesPool.getHandle(instanceIndex);
+	if (0==instPtr)
+	{
+		b3Assert(0);
+		return;
+	}
+	const SimpleGL2Instance& inst = *instPtr;
 	const SimpleGL2Shape* shape = m_data->m_shapes[inst.m_shapeIndex];
 
 
@@ -244,9 +262,11 @@ void SimpleOpenGL2Renderer::drawOpenGL(int instanceIndex)
 
 void SimpleOpenGL2Renderer::drawSceneInternal(int pass, int cameraUpAxis)
 {
-	for (int i=0;i<m_data->m_graphicsInstances.size();i++)
+	b3AlignedObjectArray<int> usedHandles;
+	m_data->m_graphicsInstancesPool.getUsedHandles(usedHandles);
+	for (int i=0;i<usedHandles.size();i++)
 	{
-		drawOpenGL(i);
+		drawOpenGL(usedHandles[i]);
 	}
 
 	#if 0
@@ -435,8 +455,12 @@ void SimpleOpenGL2Renderer::activateTexture(int textureIndex)
 
 int SimpleOpenGL2Renderer::registerGraphicsInstance(int shapeIndex, const double* position, const double* quaternion, const double* color, const double* scaling)
 {
-	int sz = m_data->m_graphicsInstances.size();
+	int newHandle = m_data->m_graphicsInstancesPool.allocHandle();
-	SimpleGL2Instance& instance = m_data->m_graphicsInstances.expand();
+
+	
+//	int sz = m_data->m_graphicsInstances.size();
+
+	SimpleGL2Instance& instance = *m_data->m_graphicsInstancesPool.getHandle(newHandle);
 	instance.m_shapeIndex = shapeIndex;
 	instance.m_position[0] = position[0];
 	instance.m_position[1] = position[1];
@@ -451,13 +475,13 @@ int SimpleOpenGL2Renderer::registerGraphicsInstance(int shapeIndex, const double
 	instance.m_scaling[0] = scaling[0];
 	instance.m_scaling[1] = scaling[1];
 	instance.m_scaling[2] = scaling[2];
-	return sz;
+	return newHandle;
 }
 
 int SimpleOpenGL2Renderer::registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
 {
-	int sz = m_data->m_graphicsInstances.size();
+	int newHandle = m_data->m_graphicsInstancesPool.allocHandle();
-	SimpleGL2Instance& instance = m_data->m_graphicsInstances.expand();
+	SimpleGL2Instance& instance = *m_data->m_graphicsInstancesPool.getHandle(newHandle);
 	instance.m_shapeIndex = shapeIndex;
 	instance.m_position[0] = position[0];
 	instance.m_position[1] = position[1];
@@ -472,7 +496,7 @@ int SimpleOpenGL2Renderer::registerGraphicsInstance(int shapeIndex, const float*
 	instance.m_scaling[0] = scaling[0];
 	instance.m_scaling[1] = scaling[1];
 	instance.m_scaling[2] = scaling[2];
-	return sz;
+	return newHandle;
 }
 
 void SimpleOpenGL2Renderer::drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float pointDrawSize)
@@ -545,26 +569,30 @@ int SimpleOpenGL2Renderer::registerShape(const float* vertices, int numvertices,
 
 void SimpleOpenGL2Renderer::writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex)
 {
-	m_data->m_graphicsInstances[srcIndex].m_position[0] = position[0];
+	SimpleGL2Instance& graphicsInstance = *m_data->m_graphicsInstancesPool.getHandle(srcIndex);
-	m_data->m_graphicsInstances[srcIndex].m_position[1] = position[1];
-	m_data->m_graphicsInstances[srcIndex].m_position[2] = position[2];
 	
-	m_data->m_graphicsInstances[srcIndex].orn[0] = orientation[0];
+	graphicsInstance.m_position[0] = position[0];
-	m_data->m_graphicsInstances[srcIndex].orn[1] = orientation[1];
+	graphicsInstance.m_position[1] = position[1];
-	m_data->m_graphicsInstances[srcIndex].orn[2] = orientation[2];
+	graphicsInstance.m_position[2] = position[2];
-	m_data->m_graphicsInstances[srcIndex].orn[3] = orientation[3];
+
+	graphicsInstance.orn[0] = orientation[0];
+	graphicsInstance.orn[1] = orientation[1];
+	graphicsInstance.orn[2] = orientation[2];
+	graphicsInstance.orn[3] = orientation[3];
 
 }
 void SimpleOpenGL2Renderer::writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex)
 {
-	m_data->m_graphicsInstances[srcIndex].m_position[0] = position[0];
+	SimpleGL2Instance& graphicsInstance = *m_data->m_graphicsInstancesPool.getHandle(srcIndex);
-	m_data->m_graphicsInstances[srcIndex].m_position[1] = position[1];
-	m_data->m_graphicsInstances[srcIndex].m_position[2] = position[2];
 	
-	m_data->m_graphicsInstances[srcIndex].orn[0] = orientation[0];
+	graphicsInstance.m_position[0] = position[0];
-	m_data->m_graphicsInstances[srcIndex].orn[1] = orientation[1];
+	graphicsInstance.m_position[1] = position[1];
-	m_data->m_graphicsInstances[srcIndex].orn[2] = orientation[2];
+	graphicsInstance.m_position[2] = position[2];
-	m_data->m_graphicsInstances[srcIndex].orn[3] = orientation[3];
+
+	graphicsInstance.orn[0] = orientation[0];
+	graphicsInstance.orn[1] = orientation[1];
+	graphicsInstance.orn[2] = orientation[2];
+	graphicsInstance.orn[3] = orientation[3];
 }
 void SimpleOpenGL2Renderer::writeTransforms()
 {

examples/OpenGLWindow/SimpleOpenGL2Renderer.h

@@ -32,7 +32,7 @@ public:
 	virtual void	resize(int width, int height);
 
     virtual void removeAllInstances();
-    
+    virtual void removeGraphicsInstance(int instanceUid);
     
     virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex);
     virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex);

examples/RoboticsLearning/b3RobotSimAPI.cpp

@@ -50,6 +50,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
 	eRobotSimGUIHelperCreateCollisionObjectGraphicsObject,
 	eRobotSimGUIHelperRemoveAllGraphicsInstances,
 	eRobotSimGUIHelperCopyCameraImageData,
+	eRobotSimGUIHelperRemoveGraphicsInstance,
 };
 
 #include <stdio.h>
@@ -320,6 +321,17 @@ public:
 		{
 		}
     }
+	int m_graphicsInstanceRemove;
+	 virtual void removeGraphicsInstance(int instanceUid)
+    {
+        m_cs->lock();
+		m_cs->setSharedParam(1,eRobotSimGUIHelperRemoveGraphicsInstance);
+		m_graphicsInstanceRemove = instanceUid;
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
+		{
+		}
+    }
 
 	virtual Common2dCanvasInterface* get2dCanvasInterface()
 	{
@@ -639,6 +651,14 @@ void b3RobotSimAPI::processMultiThreadedGraphicsRequests()
             m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
             break;
         }
+	case eRobotSimGUIHelperRemoveGraphicsInstance:
+	{
+		m_data->m_multiThreadedHelper->m_childGuiHelper->removeGraphicsInstance(m_data->m_multiThreadedHelper->m_graphicsInstanceRemove);
+        m_data->m_multiThreadedHelper->getCriticalSection()->lock();
+        m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
+        m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
+		break;
+	}
     case eRobotSimGUIHelperCopyCameraImageData:
         {
             m_data->m_multiThreadedHelper->m_childGuiHelper->copyCameraImageData(m_data->m_multiThreadedHelper->m_viewMatrix,

examples/SharedMemory/PhysicsServerExample.cpp

@@ -179,6 +179,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
 	eGUIUserDebugRemoveItem,
 	eGUIUserDebugRemoveAllItems,
 	eGUIDumpFramesToVideo,
+	eGUIHelperRemoveGraphicsInstance,
 };
 
 
@@ -778,6 +779,15 @@ public:
 		workerThreadWait();
     }
 
+	int m_graphicsInstanceRemove;
+	virtual void removeGraphicsInstance(int graphicsUid)
+	{
+		m_graphicsInstanceRemove = graphicsUid;
+		m_cs->lock();
+		m_cs->setSharedParam(1,eGUIHelperRemoveGraphicsInstance);
+		workerThreadWait();    
+	}
+
 	virtual Common2dCanvasInterface* get2dCanvasInterface()
 	{
 		return 0;
@@ -1551,6 +1561,14 @@ void	PhysicsServerExample::updateGraphics()
 
             break;
         }
+	case eGUIHelperRemoveGraphicsInstance:
+	{
+		m_multiThreadedHelper->m_childGuiHelper->removeGraphicsInstance(m_multiThreadedHelper->m_graphicsInstanceRemove);
+		m_multiThreadedHelper->mainThreadRelease();
+		break;
+	}
+       
+	
 
     case eGUIHelperCopyCameraImageData:
         {

src/Bullet3Common/b3ResizablePool.h

@@ -13,7 +13,7 @@ enum
 template <typename U>
 struct b3PoolBodyHandle : public U
 {
-	BT_DECLARE_ALIGNED_ALLOCATOR();
+	B3_DECLARE_ALIGNED_ALLOCATOR();
 
 	int m_nextFreeHandle;
 	void SetNextFree(int next)
@@ -39,10 +39,12 @@ public:
 	
 	b3ResizablePool()
 	{
+		initHandles();
 	}
 	
 	virtual ~b3ResizablePool()
 	{
+		exitHandles();
 	}
 ///handle management
 
@@ -65,8 +67,8 @@ public:
 
 	T* getHandle(int handle)
 	{
-		btAssert(handle>=0);
+		b3Assert(handle>=0);
-		btAssert(handle<m_bodyHandles.size());
+		b3Assert(handle<m_bodyHandles.size());
 		if ((handle<0) || (handle>=m_bodyHandles.size()))
 		{
 			return 0;
@@ -81,7 +83,7 @@ public:
 	void increaseHandleCapacity(int extraCapacity)
 	{
 		int curCapacity = m_bodyHandles.size();
-		btAssert(curCapacity == m_numUsedHandles);
+		//b3Assert(curCapacity == m_numUsedHandles);
 		int newCapacity = curCapacity + extraCapacity;
 		m_bodyHandles.resize(newCapacity);
 
@@ -111,7 +113,7 @@ public:
 
 	int allocHandle()
 	{
-		btAssert(m_firstFreeHandle>=0);
+		b3Assert(m_firstFreeHandle>=0);
 
 		int handle = m_firstFreeHandle;
 		m_firstFreeHandle = getHandle(handle)->GetNextFree();
@@ -134,7 +136,7 @@ public:
 
 	void freeHandle(int handle)
 	{
-		btAssert(handle >= 0);
+		b3Assert(handle >= 0);
 
 		getHandle(handle)->SetNextFree(m_firstFreeHandle);
 		m_firstFreeHandle = handle;