void SimpleOpenGL3App::registerGrid(int cells_x, int cells_z, float color0[4], float color1[4])
{
	b3Vector3 cubeExtents=b3MakeVector3(0.5,0.5,0.5);
	cubeExtents[m_data->m_upAxis] = 0;
	int cubeId = registerCubeShape(cubeExtents[0],cubeExtents[1],cubeExtents[2]);

	b3Quaternion orn(0,0,0,1);
	b3Vector3 center=b3MakeVector3(0,0,0,1);
	b3Vector3 scaling=b3MakeVector3(1,1,1,1);

	for ( int i = 0; i < cells_x; i++) 
	{
		for (int j = 0; j < cells_z; j++) 
		{
			float* color =0;
			if ((i + j) % 2 == 0) 
			{
				color = (float*)color0;
			} else {
				color = (float*)color1;
			}
			if (this->m_data->m_upAxis==1)
			{
				center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, 0.f, (j + 0.5f) - cells_z * 0.5f);
			} else
			{
				center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, (j + 0.5f) - cells_z * 0.5f,0.f );
			}
			m_instancingRenderer->registerGraphicsInstance(cubeId,center,orn,color,scaling);
		}
	}
	
}

void GpuConvexPlaneScene::createStaticEnvironment(const ConstructionInfo& ci)
{
	int strideInBytes = 9*sizeof(float);
	int numVertices = sizeof(cube_vertices)/strideInBytes;
	int numIndices = sizeof(cube_indices)/sizeof(int);
	//int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
	int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
	int group=1;
	int mask=1;
	int index=0;


	{
		b3Vector4 scaling=b3MakeVector4(400,400,400,1);
		int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
		b3Vector3 position=b3MakeVector3(0,-400,0);
		b3Quaternion orn(0,0,0,1);

		b3Vector4 color=b3MakeVector4(0,0,1,1);

		int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
		int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index,false);

	}

}

    virtual void    initPhysics()
    {
        
        ///create some graphics proxy for the tracking target
        ///the endeffector tries to track it using Inverse Kinematics
        {
            int sphereId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_MEDIUM);
            b3Quaternion orn(0, 0, 0, 1);
            b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1);
            b3Vector3 scaling = b3MakeVector3(.02, .02, .02);
            m_targetSphereInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, m_targetPos, orn, color, scaling);
        }
        m_app->m_renderer->writeTransforms();

        
        
        
        int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER;
		m_robotSim.setGuiHelper(m_guiHelper);
		bool connected = m_robotSim.connect(mode);
			
//			0;//m_robotSim.connect(m_guiHelper);
		b3Printf("robotSim connected = %d",connected);
		
		
        {
			m_kukaIndex  = m_robotSim.loadURDF("kuka_iiwa/model.urdf");
			if (m_kukaIndex >=0)
            {
                int numJoints = m_robotSim.getNumJoints(m_kukaIndex);
                b3Printf("numJoints = %d",numJoints);

                for (int i=0;i<numJoints;i++)
                {
                    b3JointInfo jointInfo;
                    m_robotSim.getJointInfo(m_kukaIndex,i,&jointInfo);
                    b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
                }
                /*
                int wheelJointIndices[4]={2,3,6,7};
                int wheelTargetVelocities[4]={-10,-10,-10,-10};
                for (int i=0;i<4;i++)
                {
                    b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
                    controlArgs.m_targetVelocity = wheelTargetVelocities[i];
                    controlArgs.m_maxTorqueValue = 1e30;
                    m_robotSim.setJointMotorControl(m_kukaIndex,wheelJointIndices[i],controlArgs);
                }
                 */
            }
			
			{
				m_robotSim.loadURDF("plane.urdf");
				m_robotSim.setGravity(b3MakeVector3(0,0,0));
			}
	
		}
		
    }

void pl_rigidbody_SetOrientation(PlRigidBody* object, const PlQuaternion* orientation)
{
	CAST_ASSERT(object,btRigidBody*,body);

	btQuaternion orn(orientation[0],orientation[1],orientation[2],orientation[3]);
	btTransform worldTrans = body->getWorldTransform();
	worldTrans.setRotation(orn);
	body->setWorldTransform(worldTrans);
}

void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation)
{
	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
	btAssert(body);
	btQuaternion orn(orientation[0],orientation[1],orientation[2],orientation[3]);
	btTransform worldTrans = body->getWorldTransform();
	worldTrans.setRotation(orn);
	body->setWorldTransform(worldTrans);
}

    virtual void	renderScene()
    {
		m_robotSim.renderScene();

        
        b3Quaternion orn(0, 0, 0, 1);
    
        
        m_app->m_renderer->writeSingleInstanceTransformToCPU(m_targetPos, orn, m_targetSphereInstance);
        m_app->m_renderer->writeTransforms();
        
        //draw the end-effector target sphere
        
		//m_app->m_renderer->renderScene();
    }

void writeTransforms()
{


	glFlush();
	char* bla =  (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_READ_WRITE);//GL_WRITE_ONLY

	float* positions = (float*)(bla+sizeof(cube_vertices));
	float* orientations = (float*)(bla+sizeof(cube_vertices) + POSITION_BUFFER_SIZE);
	//	positions[0]+=0.001f;

	static int offset=0;
	//offset++;

	static btVector3 axis(1,0,0);
	angle += 0.01f;
	int index=0;
	btQuaternion orn(axis,angle);
	for (int i=0;i<NUM_OBJECTS_X;i++)
	{
		for (int j=0;j<NUM_OBJECTS_Y;j++)
		{
			for (int k=0;k<NUM_OBJECTS_Z;k++)
			{
				//if (!((index+offset)%15))
				{
					instance_positions_ptr[index*4+1]-=0.01f;
					positions[index*4]=instance_positions_ptr[index*4];
					positions[index*4+1]=instance_positions_ptr[index*4+1];
					positions[index*4+2]=instance_positions_ptr[index*4+2];
					positions[index*4+3]=instance_positions_ptr[index*4+3];

					orientations[index*4] = orn[0];
					orientations[index*4+1] = orn[1];
					orientations[index*4+2] = orn[2];
					orientations[index*4+3] = orn[3];
				}
				//				memcpy((void*)&orientations[index*4],orn,sizeof(btQuaternion));
				index++;
			}
		}
	}

	glUnmapBuffer( GL_ARRAY_BUFFER);
	//if this glFinish is removed, the animation is not always working/blocks
	//@todo: figure out why
	glFlush();
}

void mutex_lock(uint8_t r, uint8_t p) {
   //Serial.print("[Lock] r:");
   //Serial.print(r);
   //Serial.print(" p:");
   //Serial.println(p);
   entrance_intents[r][p] = true;
   while (orn(entrance_intents[r], p)) {
      if (turn[r] != 0) {
         entrance_intents[r][0] = false;
         while (turn[r] != 0) {
           // busy wait
         }
         entrance_intents[r][0] = true;
      }
   }
 }

void ConcaveSphereScene::createDynamicObjects(const ConstructionInfo& ci)
{
		b3Vector4 colors[4] = 
	{
		b3MakeVector4(1,0,0,1),
		b3MakeVector4(0,1,0,1),
		b3MakeVector4(0,1,1,1),
		b3MakeVector4(1,1,0,1),
	};

	int index=0;
	int curColor = 0;
	float radius = 1;
	//int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
	int colIndex = m_data->m_np->registerSphereShape(radius);//>registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
	int prevGraphicsShapeIndex = registerGraphicsSphereShape(ci,radius,false);

	for (int i=0;i<ci.arraySizeX;i++)
	{
		for (int j=0;j<ci.arraySizeY;j++)
		{
			for (int k=0;k<ci.arraySizeZ;k++)
			{
				float mass = 1.f;

				
				b3Vector3 position=b3MakeVector3(-(ci.arraySizeX/2)*8+i*8,50+j*8,-(ci.arraySizeZ/2)*8+k*8);
					
				//b3Vector3 position(0,-41,0);//0,0,0);//i*radius*3,-41+j*radius*3,k*radius*3);
					
				b3Quaternion orn(0,0,0,1);
				
				b3Vector4 color = colors[curColor];
				curColor++;
				curColor&=3;
				b3Vector4 scaling=b3MakeVector4(radius,radius,radius,1);
				int id = ci.m_instancingRenderer->registerGraphicsInstance(prevGraphicsShapeIndex,position,orn,color,scaling);
				int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false);
				
				index++;
			}
		}
	}
}

    RenderInstancingDemo(CommonGraphicsApp* app)
    :m_app(app),
    m_x(0),
    m_y(0),
	m_z(0)
    {
		m_app->setUpAxis(2);
        
		 {
             b3Vector3 extents=b3MakeVector3(100,100,100);
             extents[m_app->getUpAxis()]=1;
             
			 int xres = 20;
			 int yres = 20;
			 
			 b3Vector4 color0=b3MakeVector4(0.1, 0.1, 0.1,1);
			 b3Vector4 color1=b3MakeVector4(0.6, 0.6, 0.6,1);
            m_app->registerGrid(xres, yres, color0, color1);
        }
		 
        {
            int boxId = m_app->registerCubeShape(0.1,0.1,0.1);
           
            
            
            for (int i=-numCubesX/2;i<numCubesX/2;i++)
            {
                for (int j = -numCubesY/2;j<numCubesY/2;j++)
                {
                    b3Vector3 pos=b3MakeVector3(i,j,j);
                    pos[app->getUpAxis()] = 1;
                    b3Quaternion orn(0,0,0,1);
                    b3Vector4 color=b3MakeVector4(0.3,0.3,0.3,1);
                    b3Vector3 scaling=b3MakeVector3(1,1,1);
                   int instanceId = m_app->m_renderer->registerGraphicsInstance(boxId,pos,orn,color,scaling);
				   m_movingInstances.push_back(instanceId);
                }
            }
        }

		 m_app->m_renderer->writeTransforms();
    }

bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger)
{
	tr.setIdentity();
	
	{
		const char* xyz_str = xml->Attribute("xyz");
		if (xyz_str)
		{
			parseVector3(tr.getOrigin(),std::string(xyz_str),logger);
		}
	}
	
	{
		const char* rpy_str = xml->Attribute("rpy");
		if (rpy_str != NULL)
		{
			btVector3 rpy;
			if (parseVector3(rpy,std::string(rpy_str),logger))
			{
				double phi, the, psi;
				double roll = rpy[0];
				double pitch = rpy[1];
				double yaw = rpy[2];
				
				phi = roll / 2.0;
				the = pitch / 2.0;
				psi = yaw / 2.0;
			  
				btQuaternion orn(
								sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
								cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
								cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
								 cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi));
			  
				orn.normalize();
				tr.setRotation(orn);
			}
		}
	}
	return true;
}

btQuaternion getLuaQuaternionArg(lua_State* L, int index)
{
	btQuaternion orn(0,0,0,1);

	int sz = luaL_returnlen(L, index);  // get size of table
	{
		lua_rawgeti(L, index, 1);  // push t[i]
		orn[0] = lua_tonumber(L,-1);
		lua_pop(L, 1);
		lua_rawgeti(L, index, 2);  // push t[i]
		orn[1] = lua_tonumber(L,-1);
		lua_pop(L, 1);
		lua_rawgeti(L, index, 3);  // push t[i]
		orn[2] = lua_tonumber(L,-1);
		lua_pop(L, 1);
		lua_rawgeti(L, index, 4);  // push t[i]
		orn[3] = lua_tonumber(L,-1);
		lua_pop(L, 1);
	}
	return orn;
}

//.........这里部分代码省略.........
					{
						edgeCrossB*=-1;
					}
				}

				btScalar	angle2 = 0;
				btScalar	ang4 = 0.f;


				btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB);
				btScalar len2 = calculatedEdge.length2();

				btScalar correctedAngle(0);
				btVector3 calculatedNormalB = normalA;
				bool isConvex = false;

				if (len2<m_triangleInfoMap->m_planarEpsilon)
				{
					angle2 = 0.f;
					ang4 = 0.f;
				} else
				{

					calculatedEdge.normalize();
					btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA);
					calculatedNormalA.normalize();
					angle2 = btGetAngle(calculatedNormalA,edgeCrossA,edgeCrossB);
					ang4 = SIMD_PI-angle2;
					btScalar dotA = normalA.dot(edgeCrossB);
					///@todo: check if we need some epsilon, due to floating point imprecision
					isConvex = (dotA<0.);

					correctedAngle = isConvex ? ang4 : -ang4;
					btQuaternion orn2(calculatedEdge,-correctedAngle);
					calculatedNormalB = btMatrix3x3(orn2)*normalA;


				}

				

				
							
				//alternatively use 
				//btVector3 calculatedNormalB2 = quatRotate(orn,normalA);


				switch (sumvertsA)
				{
				case 1:
					{
						btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1];
						btQuaternion orn(edge,-correctedAngle);
						btVector3 computedNormalB = quatRotate(orn,normalA);
						btScalar bla = computedNormalB.dot(normalB);
						if (bla<0)
						{
							computedNormalB*=-1;
							info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB;
						}
#ifdef DEBUG_INTERNAL_EDGE
						if ((computedNormalB-normalB).length()>0.0001)
						{
							printf("warning: normals not identical\n");
						}
#endif//DEBUG_INTERNAL_EDGE

/// Changes a btManifoldPoint collision normal to the normal from the mesh.
void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* colObj0,const btCollisionObject* colObj1, int partId0, int index0, int normalAdjustFlags)
{
	//btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
	if (colObj0->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
		return;

	btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape();
	btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
	if (!triangleInfoMapPtr)
		return;

	int hash = btGetHash(partId0,index0);


	btTriangleInfo* info = triangleInfoMapPtr->find(hash);
	if (!info)
		return;

	btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f;
	
	const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0->getCollisionShape());
	btVector3 v0,v1,v2;
	tri_shape->getVertex(0,v0);
	tri_shape->getVertex(1,v1);
	tri_shape->getVertex(2,v2);

	btVector3 center = (v0+v1+v2)*btScalar(1./3.);

	btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0);
	btVector3 tri_normal;
	tri_shape->calcNormal(tri_normal);

	//btScalar dot = tri_normal.dot(cp.m_normalWorldOnB);
	btVector3 nearest;
	btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest);

	btVector3 contact = cp.m_localPointB;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
	const btTransform& tr = colObj0->getWorldTransform();
	btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW



	bool isNearEdge = false;

	int numConcaveEdgeHits = 0;
	int numConvexEdgeHits = 0;

	btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
	localContactNormalOnB.normalize();//is this necessary?

	if ((info->m_edgeV0V1Angle)< SIMD_2_PI)
	{
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
		btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
#endif
		btScalar len = (contact-nearest).length();
		if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
		{
			btVector3 edge(v0-v1);
			isNearEdge = true;

			if (info->m_edgeV0V1Angle==btScalar(0))
			{
				numConcaveEdgeHits++;
			} else
			{

				bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX);
				btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
	#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
				btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
	#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

				btVector3 nA = swapFactor * tri_normal;

				btQuaternion orn(edge,info->m_edgeV0V1Angle);
				btVector3 computedNormalB = quatRotate(orn,tri_normal);
				if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB)
					computedNormalB*=-1;
				btVector3 nB = swapFactor*computedNormalB;

				btScalar	NdotA = localContactNormalOnB.dot(nA);
				btScalar	NdotB = localContactNormalOnB.dot(nB);
				bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);

#ifdef DEBUG_INTERNAL_EDGE
				{
					
					btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
				}
#endif //DEBUG_INTERNAL_EDGE


				if (backFacingNormal)
				{
					numConcaveEdgeHits++;
				}
//.........这里部分代码省略.........

int main(int argc, char* argv[])
{
	{
		b3CommandLineArgs myArgs(argc, argv);


		app = new SimpleOpenGLApp("SimpleOpenGLApp", gWidth, gHeight);

		app->m_renderer->getActiveCamera()->setCameraDistance(13);
		app->m_renderer->getActiveCamera()->setCameraPitch(0);
		app->m_renderer->getActiveCamera()->setCameraTargetPosition(0, 0, 0);

		sOldKeyboardCB = app->m_window->getKeyboardCallback();
		app->m_window->setKeyboardCallback(MyKeyboardCallback);
		sOldMouseMoveCB = app->m_window->getMouseMoveCallback();
		app->m_window->setMouseMoveCallback(MyMouseMoveCallback);
		sOldMouseButtonCB = app->m_window->getMouseButtonCallback();
		app->m_window->setMouseButtonCallback(MyMouseButtonCallback);
		sOldWheelCB = app->m_window->getWheelCallback();
		app->m_window->setWheelCallback(MyWheelCallback);
		sOldResizeCB = app->m_window->getResizeCallback();
		app->m_window->setResizeCallback(MyResizeCallback);


		myArgs.GetCmdLineArgument("mp4_file", gVideoFileName);
		if (gVideoFileName)
			app->dumpFramesToVideo(gVideoFileName);

		myArgs.GetCmdLineArgument("png_file", gPngFileName);
		char fileName[1024];

		int textureWidth = 128;
		int textureHeight = 128;

		unsigned char*	image = new unsigned char[textureWidth*textureHeight * 4];


		int textureHandle = app->m_renderer->registerTexture(image, textureWidth, textureHeight);

		int cubeIndex = app->registerCubeShape(1, 1, 1);

		b3Vector3 pos = b3MakeVector3(0, 0, 0);
		b3Quaternion orn(0, 0, 0, 1);
		b3Vector3 color = b3MakeVector3(1, 0, 0);
		b3Vector3 scaling = b3MakeVector3 (1, 1, 1);
		app->m_renderer->registerGraphicsInstance(cubeIndex, pos, orn, color, scaling);
		app->m_renderer->writeTransforms();

		do
		{
			static int frameCount = 0;
			frameCount++;
			if (gPngFileName)
			{
				printf("gPngFileName=%s\n", gPngFileName);

				sprintf(fileName, "%s%d.png", gPngFileName, frameCount++);
				app->dumpNextFrameToPng(fileName);
			}

			
			


			//update the texels of the texture using a simple pattern, animated using frame index
			for (int y = 0; y < textureHeight; ++y)
			{
				const int	t = (y + frameCount) >> 4;
				unsigned char*	pi = image + y*textureWidth * 3;
				for (int x = 0; x < textureWidth; ++x)
				{
					const int		s = x >> 4;
					const unsigned char	b = 180;
					unsigned char			c = b + ((s + (t & 1)) & 1)*(255 - b);
					pi[0] = pi[1] = pi[2] = pi[3] = c; pi += 3;
				}
			}

			app->m_renderer->activateTexture(textureHandle);
			app->m_renderer->updateTexture(textureHandle, image);

			//float color[4] = { 255, 1, 1, 1 };
			//app->m_primRenderer->drawTexturedRect(100, 200, gWidth / 2 - 50, gHeight / 2 - 50, color, 0, 0, 1, 1, true);


			app->m_renderer->init();
			app->m_renderer->updateCamera(1);

			app->m_renderer->renderScene();
			app->drawGrid();
			char bla[1024];
			sprintf(bla, "Simple test frame %d", frameCount);

			//app->drawText(bla, 10, 10);

#ifdef B3_USE_IMGUI
			{
				bool show_test_window = true;
				bool show_another_window = false;
				ImVec4 clear_color = ImColor(114, 144, 154);
//.........这里部分代码省略.........

//.........这里部分代码省略.........
			trimeshShape = (btBvhTriangleMeshShape*)import.getCollisionShapeByIndex(0);
			
			//if you know the name, you can also try to get the shape by name:
			const char* meshName = import.getNameForPointer(trimeshShape);
			if (meshName)
				trimeshShape = (btBvhTriangleMeshShape*)import.getCollisionShapeByName(meshName);
			
		}
	}


#endif

	btCollisionShape* groundShape = trimeshShape;
	
#else
	btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
	m_collisionShapes.push_back(groundShape);

#endif //USE_TRIMESH_SHAPE

	m_collisionConfiguration = new btDefaultCollisionConfiguration();

#ifdef USE_PARALLEL_DISPATCHER

#ifdef USE_WIN32_THREADING

	int maxNumOutstandingTasks = 4;//number of maximum outstanding tasks
	Win32ThreadSupport* threadSupport = new Win32ThreadSupport(Win32ThreadSupport::Win32ThreadConstructionInfo(
								"collision",
								processCollisionTask,
								createCollisionLocalStoreMemory,
								maxNumOutstandingTasks));
#else
///@todo show other platform threading
///Playstation 3 SPU (SPURS)  version is available through PS3 Devnet
///Libspe2 SPU support will be available soon
///pthreads version
///you can hook it up to your custom task scheduler by deriving from btThreadSupportInterface
#endif

	m_dispatcher = new	SpuGatheringCollisionDispatcher(threadSupport,maxNumOutstandingTasks,m_collisionConfiguration);
#else
	m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
#endif//USE_PARALLEL_DISPATCHER


	btVector3 worldMin(-1000,-1000,-1000);
	btVector3 worldMax(1000,1000,1000);
	m_broadphase = new btAxisSweep3(worldMin,worldMax);
	m_solver = new btSequentialImpulseConstraintSolver();
	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
#ifdef USE_PARALLEL_DISPATCHER
	m_dynamicsWorld->getDispatchInfo().m_enableSPU=true;
#endif //USE_PARALLEL_DISPATCHER
	
	float mass = 0.f;
	btTransform	startTransform;
	startTransform.setIdentity();
	startTransform.setOrigin(btVector3(0,-2,0));

#ifdef USE_BOX_SHAPE
	btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1));
#else
	
	btCompoundShape* colShape = new btCompoundShape;
	btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(4,1,1));
	btCollisionShape* boxShape = new btBoxShape(btVector3(4,1,1));
	btTransform localTransform;
	localTransform.setIdentity();
	colShape->addChildShape(localTransform,boxShape);
	btQuaternion orn(SIMD_HALF_PI,0,0);
	localTransform.setRotation(orn);
	colShape->addChildShape(localTransform,cylinderShape);
	
#endif //USE_BOX_SHAPE


	m_collisionShapes.push_back(colShape);

	{
		for (int i=0;i<10;i++)
		{
			startTransform.setOrigin(btVector3(2,10+i*2,1));
			localCreateRigidBody(1, startTransform,colShape);
		}
	}

	startTransform.setIdentity();
	staticBody = localCreateRigidBody(mass, startTransform,groundShape);

	staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);//STATIC_OBJECT);

	//enable custom material callback
	staticBody->setCollisionFlags(staticBody->getCollisionFlags()  | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);


	
	
}

	btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, const btMultiBodySettings& settings)
	{
		static int curColor=0;
					
	
		int cubeShapeId = m_glApp->registerCubeShape();
		
		int n_links = settings.m_numLinks;
		float mass = 13.5*scaling;
		btVector3 inertia = btVector3 (91,344,253)*scaling*scaling;
	
	
		btMultiBody * bod = new btMultiBody(n_links, mass, inertia, settings.m_isFixedBase, settings.m_canSleep);
	//		bod->setHasSelfCollision(false);

		//btQuaternion orn(btVector3(0,0,1),-0.25*SIMD_HALF_PI);//0,0,0,1);
		btQuaternion orn(0,0,0,1);
		bod->setBasePos(settings.m_basePosition);
		bod->setWorldToBaseRot(orn);
		btVector3 vel(0,0,0);
		bod->setBaseVel(vel);

		{
			
			btVector3 joint_axis_hinge(1,0,0);
			btVector3 joint_axis_prismatic(0,0,1);
			btQuaternion parent_to_child = orn.inverse();
			btVector3 joint_axis_child_prismatic = quatRotate(parent_to_child ,joint_axis_prismatic);
			btVector3 joint_axis_child_hinge = quatRotate(parent_to_child , joint_axis_hinge);
        
			int this_link_num = -1;
			int link_num_counter = 0;

		

			btVector3 pos = btVector3 (0,0,9.0500002)*scaling;

			btVector3 joint_axis_position = btVector3 (0,0,4.5250001)*scaling;

			for (int i=0;i<n_links;i++)
			{
				float initial_joint_angle=0.3;
				if (i>0)
					initial_joint_angle = -0.06f;

				const int child_link_num = link_num_counter++;

			

				if (settings.m_usePrismatic)// && i==(n_links-1))
				{
						bod->setupPrismatic(child_link_num, mass, inertia, this_link_num,
							parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos),settings.m_disableParentCollision);

				} else
				{
					bod->setupRevolute(child_link_num, mass, inertia, this_link_num,parent_to_child, joint_axis_child_hinge,
											joint_axis_position,quatRotate(parent_to_child , (pos - joint_axis_position)),settings.m_disableParentCollision);
				}
				bod->setJointPos(child_link_num, initial_joint_angle);
				this_link_num = i;
		
				if (0)//!useGroundShape && i==4)
				{
					btVector3 pivotInAworld(0,20,46);
					btVector3 pivotInAlocal = bod->worldPosToLocal(i, pivotInAworld);
					btVector3 pivotInBworld = pivotInAworld;
					btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(bod,i,&btTypedConstraint::getFixedBody(),pivotInAlocal,pivotInBworld);
					world->addMultiBodyConstraint(p2p);
				}
				//add some constraint limit
				if (settings.m_usePrismatic)
				{
		//			btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3);
			
					if (settings.m_createConstraints)
					{	
						btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1);
						world->addMultiBodyConstraint(con);
					}
			
				} else
				{
					if (settings.m_createConstraints)
					{	
						if (1)
						{
							btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,500000); 
							world->addMultiBodyConstraint(con);
						}

						btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1);
						world->addMultiBodyConstraint(con);
					}

				}
			}
		}

		//add a collider for the base
//.........这里部分代码省略.........

void ConcaveScene::createDynamicObjects(const ConstructionInfo& ci)
{
		int strideInBytes = 9*sizeof(float);
	int numVertices = sizeof(cube_vertices)/strideInBytes;
	int numIndices = sizeof(cube_indices)/sizeof(int);
	//int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
	int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
	int group=1;
	int mask=1;
	
	
	int index=0;
	

	if (1)
	{
		int curColor = 0;
		b3Vector4 colors[4] = 
		{
			b3MakeVector4(1,1,1,1),
			b3MakeVector4(1,1,0.3,1),
			b3MakeVector4(0.3,1,1,1),
			b3MakeVector4(0.3,0.3,1,1),
		};


		b3ConvexUtility* utilPtr = new b3ConvexUtility();
		b3Vector4 scaling=b3MakeVector4(1,1,1,1);

		{
			b3AlignedObjectArray<b3Vector3> verts;

			unsigned char* vts = (unsigned char*) cube_vertices;
			for (int i=0;i<numVertices;i++)
			{
				float* vertex = (float*) &vts[i*strideInBytes];
				verts.push_back(b3MakeVector3(vertex[0]*scaling[0],vertex[1]*scaling[1],vertex[2]*scaling[2]));
			}

			bool merge = true;
			if (numVertices)
			{
				utilPtr->initializePolyhedralFeatures(&verts[0],verts.size(),merge);
			}
		}

//		int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);

		int colIndex=-1;
		if (ci.m_useInstancedCollisionShapes)
			colIndex = m_data->m_np->registerConvexHullShape(utilPtr);

		for (int i=0;i<ci.arraySizeX;i++)
		{

			for (int j=0;j<ci.arraySizeY;j++)
			{
				for (int k=0;k<ci.arraySizeZ;k++)
				{
					if (!ci.m_useInstancedCollisionShapes)
						colIndex = m_data->m_np->registerConvexHullShape(utilPtr);

					float mass = 1;

					//b3Vector3 position(-2*ci.gapX+i*ci.gapX,25+j*ci.gapY,-2*ci.gapZ+k*ci.gapZ);
					b3Vector3 position=b3MakeVector3(-(ci.arraySizeX/2)*CONCAVE_GAPX+i*CONCAVE_GAPX,
						23+j*CONCAVE_GAPY,
						-(ci.arraySizeZ/2)*CONCAVE_GAPZ+k*CONCAVE_GAPZ);
					b3Quaternion orn(0,0,0,1);
				
					b3Vector4 color = colors[curColor];
					curColor++;
					curColor&=3;
				
					int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
					int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false);
				
					index++;
				}
			}
		}
	}

}

void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,  const btMatrix4x4& parentTransMat)
{
	const char* nodeName = node->Attribute("id");
	printf("processing node %s\n", nodeName);

	
	btMatrix4x4 nodeTrans;
	nodeTrans.setIdentity();

	///todo(erwincoumans) we probably have to read the elements 'translate', 'scale', 'rotate' and 'matrix' in-order and accumulate them...
	{
		for (TiXmlElement* transElem = node->FirstChildElement("matrix");transElem;transElem=node->NextSiblingElement("matrix"))
		{
			if (transElem->GetText())
			{
				btAlignedObjectArray<float> floatArray;
				TokenFloatArray adder(floatArray);
				tokenize(transElem->GetText(),adder);
				if (floatArray.size()==16)
				{
					btMatrix4x4 t(floatArray[0],floatArray[1],floatArray[2],floatArray[3],
									floatArray[4],floatArray[5],floatArray[6],floatArray[7],
									floatArray[8],floatArray[9],floatArray[10],floatArray[11],
									floatArray[12],floatArray[13],floatArray[14],floatArray[15]);

					nodeTrans = nodeTrans*t;
				} else
				{
					printf("Error: expected 16 elements in a <matrix> element, skipping\n");
				}
			}
		}
	}

	{
		for (TiXmlElement* transElem = node->FirstChildElement("translate");transElem;transElem=node->NextSiblingElement("translate"))
		{
			if (transElem->GetText())
			{
				btVector3 pos = getVector3FromXmlText(transElem->GetText());
				//nodePos+= unitScaling*parentScaling*pos;
				btMatrix4x4 t;
				t.setPureTranslation(pos);
				nodeTrans = nodeTrans*t;

			}
		}
	}
	{
		for(TiXmlElement* scaleElem = node->FirstChildElement("scale");
				scaleElem!= NULL; scaleElem= node->NextSiblingElement("scale")) 
		{
			if (scaleElem->GetText())
			{
				btVector3 scaling = getVector3FromXmlText(scaleElem->GetText());
				btMatrix4x4 t;
				t.setPureScaling(scaling);
				nodeTrans = nodeTrans*t;
			}
		}
	}
	{
		for(TiXmlElement* rotateElem = node->FirstChildElement("rotate");
				rotateElem!= NULL; rotateElem= node->NextSiblingElement("rotate")) 
		{
			if (rotateElem->GetText())
			{
				//accumulate orientation
				btVector4 rotate = getVector4FromXmlText(rotateElem->GetText());
				btQuaternion orn(btVector3(rotate),btRadians(rotate[3]));//COLLADA DAE rotate is in degrees, convert to radians
				btMatrix4x4 t;
				t.setPureRotation(orn);
				nodeTrans = nodeTrans*t;
			}
		}
	}
	
	nodeTrans = parentTransMat*nodeTrans;
	
	for (TiXmlElement* instanceGeom = node->FirstChildElement("instance_geometry");
				instanceGeom!=0;
				instanceGeom=instanceGeom->NextSiblingElement("instance_geometry"))
	{
		const char* geomUrl = instanceGeom->Attribute("url");
		printf("node referring to geom %s\n", geomUrl);
		geomUrl++;
		int* shapeIndexPtr = name2Shape[geomUrl];
		if (shapeIndexPtr)
		{
			int index = *shapeIndexPtr;
			printf("found geom with index %d\n", *shapeIndexPtr);
			ColladaGraphicsInstance& instance = visualShapeInstances.expand();
			instance.m_shapeIndex = *shapeIndexPtr;
			instance.m_worldTransform = nodeTrans;
		} else
		{
			printf("geom not found\n");
		}
	}

//.........这里部分代码省略.........

    CollisionTutorialBullet2(GUIHelperInterface* guiHelper, int tutorialIndex)
    :m_app(guiHelper->getAppInterface()),
	m_guiHelper(guiHelper),
	m_tutorialIndex(tutorialIndex),
	m_collisionSdkHandle(0),
	m_collisionWorldHandle(0),
	m_stage(0),
	m_counter(0),
	m_timeSeriesCanvas0(0)
    {
		
		gTotalPoints = 0;
		m_app->setUpAxis(1);
		m_app->m_renderer->enableBlend(true);
		
		switch (m_tutorialIndex)
		{
			case TUT_SPHERE_PLANE_RTB3:
			case TUT_SPHERE_PLANE_BULLET2:
			{
				
				if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2)
				{
					m_collisionSdkHandle = plCreateBullet2CollisionSdk();
				} else
				{
#ifndef DISABLE_REAL_TIME_BULLET3_COLLISION_SDK
					m_collisionSdkHandle = plCreateRealTimeBullet3CollisionSdk();
#endif //DISABLE_REAL_TIME_BULLET3_COLLISION_SDK
				}
				if (m_collisionSdkHandle)
				{
					int maxNumObjsCapacity=1024;
					int maxNumShapesCapacity=1024;
					int maxNumPairsCapacity=16384;
					btAlignedObjectArray<plCollisionObjectHandle> colliders;
					m_collisionWorldHandle = plCreateCollisionWorld(m_collisionSdkHandle,maxNumObjsCapacity,maxNumShapesCapacity,maxNumPairsCapacity);
					//create objects, do query etc
					{
						float radius = 1.f;

						void* userPointer = 0;
						{
							for (int j=0;j<sNumCompounds;j++)
							{
								plCollisionShapeHandle compoundShape =  plCreateCompoundShape(m_collisionSdkHandle,m_collisionWorldHandle);

								for (int i=0;i<sNumSpheres;i++)
								{
									btVector3 childPos(i*1.5,0,0);
									btQuaternion childOrn(0,0,0,1);
								
									btVector3 scaling(radius,radius,radius);
						
									plCollisionShapeHandle childShape = plCreateSphereShape(m_collisionSdkHandle, m_collisionWorldHandle,radius);
									plAddChildShape(m_collisionSdkHandle,m_collisionWorldHandle,compoundShape, childShape,childPos,childOrn);
						
								
									//m_guiHelper->createCollisionObjectGraphicsObject(colObj,color);
								
								}
								if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2)
								{
									btCollisionShape* colShape = (btCollisionShape*) compoundShape;
									m_guiHelper->createCollisionShapeGraphicsObject(colShape);
								} else
								{
								}
							
								{
									btVector3 pos(j*sNumSpheres*1.5,-2.4,0);
									btQuaternion orn(0,0,0,1);
									plCollisionObjectHandle colObjHandle = plCreateCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,userPointer, -1,compoundShape,pos,orn);
									if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2)
									{
										btCollisionObject* colObj = (btCollisionObject*) colObjHandle;
										btVector4 color=sColors[j&3];
										m_guiHelper->createCollisionObjectGraphicsObject(colObj,color);
										colliders.push_back(colObjHandle);
										plAddCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle,colObjHandle);
									}
								}
							}
						}
					}

					{
						plCollisionShapeHandle colShape = plCreatePlaneShape(m_collisionSdkHandle, m_collisionWorldHandle,0,1,0,-3.5);
						btVector3 pos(0,0,0);
						btQuaternion orn(0,0,0,1);
						void* userPointer = 0;
						plCollisionObjectHandle colObj = plCreateCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,userPointer, 0,colShape,pos,orn);
						colliders.push_back(colObj);
						plAddCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle,colObj);
					}

                    int numContacts = plCollide(m_collisionSdkHandle,m_collisionWorldHandle,colliders[0],colliders[1],pointsOut,sPointCapacity);
                    printf("numContacts = %d\n", numContacts);
                    void* myUserPtr = 0;
                    
//.........这里部分代码省略.........