void clientDisplay(void) {


	updateCamera();

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
	glDisable(GL_LIGHTING);

	//GL_ShapeDrawer::drawCoordSystem();

	float m[16];
	int i;
#ifdef USE_GJK
	btGjkEpaPenetrationDepthSolver epa;
	btGjkPairDetector	convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,&epa);

	btVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f);
	convexConvex.setCachedSeperatingAxis(seperatingAxis);

	btPointCollector gjkOutput;
	btGjkPairDetector::ClosestPointInput input;
	input.m_transformA = tr[0];
	input.m_transformB = tr[1];

	convexConvex.getClosestPoints(input ,gjkOutput,0);

	if (gjkOutput.m_hasResult)
	{
		btVector3 endPt = gjkOutput.m_pointInWorld +
			gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;

		 glBegin(GL_LINES);
		glColor3f(1, 0, 0);
		glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z());
		glVertex3d(endPt.x(),endPt.y(),endPt.z());
		glEnd();

	}
#else //USE_GJK

	
	struct	MyContactResultCallback : public btCollisionWorld::ContactResultCallback
	{
		virtual	btScalar	addSingleResult(btManifoldPoint& cp,	const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1)
		{
			 glBegin(GL_LINES);
			glColor3f(1, 0, 0);
			
			glVertex3d(cp.m_positionWorldOnA.getX(),cp.m_positionWorldOnA.getY(),cp.m_positionWorldOnA.getZ());
			glVertex3d(cp.m_positionWorldOnB.getX(),cp.m_positionWorldOnB.getY(),cp.m_positionWorldOnB.getZ());
			glEnd();

			return 1.f;
		}
	};

	btDefaultCollisionConfiguration collisionConfiguration;
	btCollisionDispatcher				dispatcher(&collisionConfiguration);
	btDbvtBroadphase pairCache;
	btCollisionWorld world (&dispatcher,&pairCache,&collisionConfiguration);
	world.getDispatchInfo().m_convexMaxDistanceUseCPT = true;
	MyContactResultCallback result;
	btCollisionObject obA;
	obA.setCollisionShape(shapePtr[0]);
	obA.setWorldTransform(tr[0]);
	btCollisionObject obB;
	obB.setCollisionShape(shapePtr[1]);
	obB.setWorldTransform(tr[1]);
	world.contactPairTest(&obA,&obB,result);

#endif//USE_GJK

	btVector3 worldMin(-1000,-1000,-1000);
	btVector3 worldMax(1000,1000,1000);

	for (i=0;i<numObjects;i++)
	{
		
		tr[i].getOpenGLMatrix( m );

		shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),debugMode, worldMin, worldMax);


	}

	simplex.setSimplexSolver(&sGjkSimplexSolver);
	btVector3 ybuf[4],pbuf[4],qbuf[4];
	int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
	simplex.reset();
	
	for (i=0;i<numpoints;i++)
		simplex.addVertex(ybuf[i]);

	btTransform ident;
	ident.setIdentity();
	ident.getOpenGLMatrix(m);
	shapeDrawer.drawOpenGL(m,&simplex,btVector3(1,1,1),debugMode, worldMin,worldMax);


	btQuaternion orn;
	orn.setEuler(yaw,pitch,roll);
	tr[0].setRotation(orn);
	tr[1].setRotation(orn);

	pitch += 0.005f;
	yaw += 0.01f;

	glFlush();
    glutSwapBuffers();
}
Beispiel #2
0
void clientDisplay(void) {


	updateCamera();

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
	glDisable(GL_LIGHTING);

	//GL_ShapeDrawer::drawCoordSystem();

	ATTRIBUTE_ALIGNED16(btScalar) m[16];
	int i;
#ifdef USE_GJK
	btGjkEpaPenetrationDepthSolver epa;
	btGjkPairDetector	convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,&epa);

	btVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f);
	convexConvex.setCachedSeperatingAxis(seperatingAxis);

	btPointCollector gjkOutput;
	btGjkPairDetector::ClosestPointInput input;
	input.m_transformA = tr[0];
	input.m_transformB = tr[1];

	convexConvex.getClosestPoints(input ,gjkOutput,0);

	if (gjkOutput.m_hasResult)
	{
		btVector3 endPt = gjkOutput.m_pointInWorld +
			gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;

		 glBegin(GL_LINES);
		glColor3f(1, 0, 0);
		glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z());
		glVertex3d(endPt.x(),endPt.y(),endPt.z());
		glEnd();

	}
#else //USE_GJK

	
	struct	MyContactResultCallback : public btCollisionWorld::ContactResultCallback
	{
		virtual	btScalar	addSingleResult(btManifoldPoint& cp,	const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
		{
			 glBegin(GL_LINES);
			glColor3f(1, 0, 0);
			
			glVertex3d(cp.m_positionWorldOnA.getX(),cp.m_positionWorldOnA.getY(),cp.m_positionWorldOnA.getZ());
			glVertex3d(cp.m_positionWorldOnB.getX(),cp.m_positionWorldOnB.getY(),cp.m_positionWorldOnB.getZ());
			glEnd();

			return 1.f;
		}
	};

	btDefaultCollisionConfiguration collisionConfiguration;
	btCollisionDispatcher				dispatcher(&collisionConfiguration);
	btDbvtBroadphase pairCache;
	btCollisionWorld world (&dispatcher,&pairCache,&collisionConfiguration);
	gContactBreakingThreshold=1e10f;
	
	MyContactResultCallback result;
	btCollisionObject obA;
	obA.setCollisionShape(shapePtr[0]);
	obA.setWorldTransform(tr[0]);
	btCollisionObject obB;
	obB.setCollisionShape(shapePtr[1]);
	obB.setWorldTransform(tr[1]);
	world.contactPairTest(&obA,&obB,result);

#endif//USE_GJK

	btVector3 worldMin(-1000,-1000,-1000);
	btVector3 worldMax(1000,1000,1000);

	for (i=0;i<numObjects;i++)
	{
		
		tr[i].getOpenGLMatrix( m );

		if (debugMode)
		{
			/// for polyhedral shapes
				if (shapePtr[i]->isPolyhedral())
				{
					if (!shapePtr[i]->getUserPointer())
					{
						btConvexHullComputer* convexUtil = new btConvexHullComputer();
						shapePtr[i]->setUserPointer(convexUtil);

						btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shapePtr[i];

						btAlignedObjectArray<btVector3> vertices;
						vertices.resize(polyshape->getNumVertices());
						for (int i=0;i<polyshape->getNumVertices();i++)
						{
							polyshape->getVertex(i,vertices[i]);
						}
						
						bool useDoublePrecision = false;
						convexUtil->compute(&vertices[0].getX(),sizeof(btVector3), polyshape->getNumVertices(),0,0);
					} 

					if (shapePtr[i]->getUserPointer())
					{
						btConvexHullComputer* convexUtil = (btConvexHullComputer*)shapePtr[i]->getUserPointer();
						//printf("num faces = %d\n",convexUtil->faces.size());
						for (int j=0;j<convexUtil->faces.size();j++)
						{
							int face = convexUtil->faces[j];
							//printf("face=%d\n",face);
							const btConvexHullComputer::Edge*  firstEdge = &convexUtil->edges[face];
							const btConvexHullComputer::Edge*  edge = firstEdge;

							do
							{
								int src = edge->getSourceVertex();
								int targ = edge->getTargetVertex();
								//printf("src=%d target = %d\n", src,targ);
								
								btVector3 wa = tr[i] * convexUtil->vertices[src];
								btVector3 wb = tr[i] * convexUtil->vertices[targ];

								glBegin(GL_LINES);
								glColor3f(1, 1, 1);
								glVertex3f(wa.getX(),wa.getY(),wa.getZ());
								glVertex3f(wb.getX(),wb.getY(),wb.getZ());
								glEnd();

								edge = edge->getNextEdgeOfFace();
							} while (edge!=firstEdge);

						}
					}
				}
		} else
		{
			shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),debugMode, worldMin, worldMax);
		}


	}

	simplex.setSimplexSolver(&sGjkSimplexSolver);
	btVector3 ybuf[4],pbuf[4],qbuf[4];
	int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
	simplex.reset();
	
	for (i=0;i<numpoints;i++)
		simplex.addVertex(ybuf[i]);

	btTransform ident;
	ident.setIdentity();
	ident.getOpenGLMatrix(m);
	shapeDrawer.drawOpenGL(m,&simplex,btVector3(1,1,1),debugMode, worldMin,worldMax);


	btQuaternion orn;
	orn.setEuler(yaw,pitch,roll);
	tr[0].setRotation(orn);
	tr[1].setRotation(orn);

	pitch += 0.005f;
	yaw += 0.01f;

	glFlush();
    glutSwapBuffers();
}
Beispiel #3
0
void CollisionDemo::displayCallback(void) {

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
	glDisable(GL_LIGHTING);


   
    
   btVoronoiSimplexSolver sGjkSimplexSolver;    
    
   btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0); 
   btPointCollector gjkOutput; 
   btGjkPairDetector::ClosestPointInput input; 
    

    
   input.m_transformA = tr[0]; 
   input.m_transformB = tr[1]; 
    
   convexConvex.getClosestPoints(input, gjkOutput, 0); 
    
   if (gjkOutput.m_hasResult) 
   { 
      //VECCOPY(pa, gjkOutput.m_pointInWorld); 
      //VECCOPY(pb, gjkOutput.m_pointInWorld); 
      //VECADDFAC(pb, pb, gjkOutput.m_normalOnBInWorld, gjkOutput.m_distance); 
             printf("bullet: %10.10f\n", gjkOutput.m_distance); // = 0.24 => that's absolutely wrong! 
	 		btVector3 endPt = gjkOutput.m_pointInWorld +
			gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;

			 glBegin(GL_LINES);
			glColor3f(1, 0, 0);
			glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z());
			glVertex3d(endPt.x(),endPt.y(),endPt.z());
			//glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
			//glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
			glEnd();
   } 

   //GL_ShapeDrawer::drawCoordSystem();

	btScalar m[16];
	int i;

//	btGjkPairDetector	convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0);

	convexConvex.getClosestPoints(input ,gjkOutput,0);

	btVector3 worldBoundsMin(-1000,-1000,-1000);
	btVector3 worldBoundsMax(1000,1000,1000);


	for (i=0;i<numObjects;i++)
	{
		
		tr[i].getOpenGLMatrix( m );

		m_shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);


	}

	simplex.setSimplexSolver(&sGjkSimplexSolver);
	btVector3 ybuf[4],pbuf[4],qbuf[4];
	int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
	simplex.reset();
	
	for (i=0;i<numpoints;i++)
		simplex.addVertex(ybuf[i]);

	btTransform ident;
	ident.setIdentity();
	ident.getOpenGLMatrix(m);
	m_shapeDrawer.drawOpenGL(m,&simplex,btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);


	btQuaternion orn;
	orn.setEuler(yaw,pitch,roll);
	//let it rotate
	//tr[0].setRotation(orn);

	pitch += 0.005f;
	yaw += 0.01f;

	glFlush();
    glutSwapBuffers();
}