virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
		{
			(void)partId;
			(void)triangleIndex;
			//do a swept sphere for now
			btTransform ident;
			ident.setIdentity();
			btConvexCast::CastResult castResult;
			castResult.m_fraction = m_hitFraction;
			btSphereShape	pointShape(m_ccdSphereRadius);
			btTriangleShape	triShape(triangle[0],triangle[1],triangle[2]);
			btVoronoiSimplexSolver	simplexSolver;
			btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
			//GjkConvexCast	convexCaster(&pointShape,convexShape,&simplexSolver);
			//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
			//local space?

			if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
				ident,ident,castResult))
			{
				if (m_hitFraction > castResult.m_fraction)
					m_hitFraction = castResult.m_fraction;
			}

		}
예제 #2
0
파일: Raytracer.cpp 프로젝트: asumin/onibi
bool	Raytracer::lowlevelRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint)
{

	btScalar closestHitResults = 1.f;

	bool hasHit = false;
	btConvexCast::CastResult rayResult;
	btSphereShape pointShape(0.0f);
	btTransform rayFromTrans;
	btTransform rayToTrans;

	rayFromTrans.setIdentity();
	rayFromTrans.setOrigin(rayFrom);
	rayToTrans.setIdentity();
	rayToTrans.setOrigin(rayTo);

	for (int s=0;s<numObjects;s++)
	{
		
		//do some culling, ray versus aabb
		btVector3 aabbMin,aabbMax;
		shapePtr[s]->getAabb(transforms[s],aabbMin,aabbMax);
		btScalar hitLambda = 1.f;
		btVector3 hitNormal;
		btCollisionObject	tmpObj;
		tmpObj.setWorldTransform(transforms[s]);


		if (btRayAabb(rayFrom,rayTo,aabbMin,aabbMax,hitLambda,hitNormal))
		{
			//reset previous result

			//choose the continuous collision detection method
			btSubsimplexConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver);
			//btGjkConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver);
			//btContinuousConvexCollision convexCaster(&pointShape,shapePtr[s],&simplexSolver,0);

			if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,transforms[s],transforms[s],rayResult))
			{
				if (rayResult.m_fraction < closestHitResults)
				{
					closestHitResults = rayResult.m_fraction;

					worldNormal = transforms[s].getBasis() *rayResult.m_normal;
					worldNormal.normalize();
					hasHit = true;
				}
			}
		}
	}

	return hasHit;

}
예제 #3
0
파일: Raytracer.cpp 프로젝트: asumin/onibi
bool	Raytracer::singleObjectRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint)
{

	btScalar closestHitResults = 1.f;

	ClosestRayResultCallback resultCallback(rayFrom,rayTo);

	bool hasHit = false;
	btConvexCast::CastResult rayResult;
	btSphereShape pointShape(0.0f);
	btTransform rayFromTrans;
	btTransform rayToTrans;

	rayFromTrans.setIdentity();
	rayFromTrans.setOrigin(rayFrom);
	rayToTrans.setIdentity();
	rayToTrans.setOrigin(rayTo);

	for (int s=0;s<numObjects;s++)
	{
		//comment-out next line to get all hits, instead of just the closest hit
		//resultCallback.m_closestHitFraction = 1.f;

		//do some culling, ray versus aabb
		btVector3 aabbMin,aabbMax;
		shapePtr[s]->getAabb(transforms[s],aabbMin,aabbMax);
		btScalar hitLambda = 1.f;
		btVector3 hitNormal;
		btCollisionObject	tmpObj;
		tmpObj.setWorldTransform(transforms[s]);


		if (btRayAabb(rayFrom,rayTo,aabbMin,aabbMax,hitLambda,hitNormal))
		{
			//reset previous result

			btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans, &tmpObj, shapePtr[s], transforms[s], resultCallback);
			if (resultCallback.hasHit())
			{
				//float fog = 1.f - 0.1f * rayResult.m_fraction;
				resultCallback.m_hitNormalWorld.normalize();//.m_normal.normalize();
				worldNormal = resultCallback.m_hitNormalWorld;
				//worldNormal = transforms[s].getBasis() *rayResult.m_normal;
				worldNormal.normalize();
				hasHit = true;
			}
		}
	}

	return hasHit;
}
예제 #4
0
void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
					  btCollisionObject* collisionObject,
					  const btCollisionShape* collisionShape,
					  const btTransform& colObjWorldTransform,
					  RayResultCallback& resultCallback,short int collisionFilterMask)
{
	
	btSphereShape pointShape(btScalar(0.0));
	pointShape.setMargin(0.f);

	objectQuerySingle(&pointShape,rayFromTrans,rayToTrans,
					  collisionObject,
					  collisionShape,
					  colObjWorldTransform,
					  resultCallback,collisionFilterMask);
}
예제 #5
0
void Raytracer::displayCallback() 
{

	updateCamera();

	for (int i=0;i<numObjects;i++)
	{
		transforms[i].setIdentity();
		SimdVector3	pos(-3.5f+i*2.5f,0.f,0.f);
		transforms[i].setOrigin( pos );
		SimdQuaternion orn;
		if (i < 2)
		{
			orn.setEuler(yaw,pitch,roll);
			transforms[i].setRotation(orn);
		}
	}
	myMink.SetTransformA(SimdTransform(transforms[0].getRotation()));

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
	glDisable(GL_LIGHTING);
	if (once)
	{
		glGenTextures(1, &glTextureId);
		glBindTexture(GL_TEXTURE_2D,glTextureId );
		once = 0;
		glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	}



	glDisable(GL_TEXTURE_2D);
	glDisable(GL_BLEND);

#define RAYTRACER
#ifdef RAYTRACER






	SimdVector4 rgba(1.f,0.f,0.f,0.5f);

	float top = 1.f;
	float bottom = -1.f;
	float nearPlane = 1.f;

	float tanFov = (top-bottom)*0.5f / nearPlane;

	float fov = 2.0 * atanf (tanFov);


	SimdVector3	rayFrom = getCameraPosition();
	SimdVector3 rayForward = getCameraTargetPosition()-getCameraPosition();
	rayForward.normalize();
	float farPlane = 600.f;
	rayForward*= farPlane;

	SimdVector3 rightOffset;
	SimdVector3 vertical(0.f,1.f,0.f);
	SimdVector3 hor;
	hor = rayForward.cross(vertical);
	hor.normalize();
	vertical = hor.cross(rayForward);
	vertical.normalize();

	float tanfov = tanf(0.5f*fov);

	hor *= 2.f * farPlane * tanfov;
	vertical *= 2.f * farPlane * tanfov;

	SimdVector3 rayToCenter = rayFrom + rayForward;

	SimdVector3 dHor = hor * 1.f/float(screenWidth);
	SimdVector3 dVert = vertical * 1.f/float(screenHeight);

	SimdTransform rayFromTrans;
	rayFromTrans.setIdentity();
	rayFromTrans.setOrigin(rayFrom);

	SimdTransform rayFromLocal;
	SimdTransform	rayToLocal;


	SphereShape pointShape(0.0f);


	///clear texture
	for (int x=0;x<screenWidth;x++)
	{
		for (int y=0;y<screenHeight;y++)
		{
			SimdVector4 rgba(0.f,0.f,0.f,0.f);
			raytracePicture->SetPixel(x,y,rgba);
		}
	}
	

	ConvexCast::CastResult rayResult;
	SimdTransform rayToTrans;
	rayToTrans.setIdentity();
	SimdVector3 rayTo;
	for (int x=0;x<screenWidth;x++)
	{
		for (int y=0;y<screenHeight;y++)
		{
			rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
			rayTo += x * dHor;
			rayTo -= y * dVert;
			rayToTrans.setOrigin(rayTo);
			for (int s=0;s<numObjects;s++)
			{
			//	rayFromLocal = transforms[s].inverse()* rayFromTrans;
			//	rayToLocal = transforms[s].inverse()* rayToTrans;

				//choose the continuous collision detection method
				SubsimplexConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver);
				//GjkConvexCast convexCaster(&pointShape,shapePtr[0],&simplexSolver);
				//ContinuousConvexCollision convexCaster(&pointShape,shapePtr[0],&simplexSolver,0);
				
				//	BU_Simplex1to4	ptShape(SimdVector3(0,0,0));//algebraic needs features, doesnt use 'supporting vertex'
				//	BU_CollisionPair convexCaster(&ptShape,shapePtr[0]);


				//reset previous result
				rayResult.m_fraction = 1.f;


				if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,transforms[s],transforms[s],rayResult))
				{
					//float fog = 1.f - 0.1f * rayResult.m_fraction;
					rayResult.m_normal.normalize();

					SimdVector3 worldNormal;
					worldNormal = transforms[s].getBasis() *rayResult.m_normal;

					float light = worldNormal.dot(SimdVector3(0.4f,-1.f,-0.4f));
					if (light < 0.2f)
						light = 0.2f;
					if (light > 1.f)
						light = 1.f;

					rgba = SimdVector4(light,light,light,1.f);
					raytracePicture->SetPixel(x,y,rgba);
				} else
				{
					//clear is already done
					//rgba = SimdVector4(0.f,0.f,0.f,0.f);
					//raytracePicture->SetPixel(x,y,rgba);

				}

				
			}
		}
	}

#define TEST_PRINTF
#ifdef TEST_PRINTF

	
	extern BMF_FontData BMF_font_helv10;
	
	raytracePicture->Printf("CCD RAYTRACER",&BMF_font_helv10);
	char buffer[256];
	sprintf(buffer,"%d RAYS / Frame",screenWidth*screenHeight*numObjects);
	raytracePicture->Printf(buffer,&BMF_font_helv10,0,10);
	

#endif //TEST_PRINTF

	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	glFrustum(-1.0,1.0,-1.0,1.0,3,2020.0);

	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glLoadIdentity();									// Reset The Modelview Matrix
	glTranslatef(0.0f,0.0f,-3.0f);						// Move Into The Screen 5 Units



	glEnable(GL_TEXTURE_2D);
	glBindTexture(GL_TEXTURE_2D,glTextureId );

	const unsigned char *ptr = raytracePicture->GetBuffer();
	glTexImage2D(GL_TEXTURE_2D, 
		0, 
		GL_RGBA, 
		raytracePicture->GetWidth(),raytracePicture->GetHeight(), 
		0, 
		GL_RGBA, 
		GL_UNSIGNED_BYTE, 
		ptr);


	glEnable (GL_BLEND);
	glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	glColor4f (1,1,1,1); // alpha=0.5=half visible

	glBegin(GL_QUADS);
	glTexCoord2f(0.0f, 0.0f);
	glVertex2f(-1,1);
	glTexCoord2f(1.0f, 0.0f);
	glVertex2f(1,1);
	glTexCoord2f(1.0f, 1.0f);
	glVertex2f(1,-1);
	glTexCoord2f(0.0f, 1.0f);
	glVertex2f(-1,-1);
	glEnd();



	glMatrixMode(GL_MODELVIEW);
	glPopMatrix();
	glMatrixMode(GL_PROJECTION);
	glPopMatrix();
	glMatrixMode(GL_MODELVIEW);

#endif //RAYRACER

	glDisable(GL_TEXTURE_2D);
	glDisable(GL_DEPTH_TEST);

	GL_ShapeDrawer::DrawCoordSystem();

	glPushMatrix();



	
	/*
	/// normal opengl rendering
	float m[16];
	int i;

	for (i=0;i<numObjects;i++)
	{


		transA.getOpenGLMatrix( m );
		/// draw the simplex
		GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1));
		/// calculate closest point from simplex to the origin, and draw this vector
		simplex.CalcClosest(m);

	}
	*/

	glPopMatrix();

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

	glFlush();
	glutSwapBuffers();
}
예제 #6
0
void	CollisionWorld::RayTestSingle(const SimdTransform& rayFromTrans,const SimdTransform& rayToTrans,
					  CollisionObject* collisionObject,
					  const CollisionShape* collisionShape,
					  const SimdTransform& colObjWorldTransform,
					  RayResultCallback& resultCallback)
{
	
	SphereShape pointShape(0.0f);

	if (collisionShape->IsConvex())
			{
				ConvexCast::CastResult castResult;
				castResult.m_fraction = 1.f;//??

				ConvexShape* convexShape = (ConvexShape*) collisionShape;
				VoronoiSimplexSolver	simplexSolver;
				SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
				//GjkConvexCast	convexCaster(&pointShape,convexShape,&simplexSolver);
				//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
				
				if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
				{
					//add hit
					if (castResult.m_normal.length2() > 0.0001f)
					{
						castResult.m_normal.normalize();
						if (castResult.m_fraction < resultCallback.m_closestHitFraction)
						{
							

							CollisionWorld::LocalRayResult localRayResult
								(
									collisionObject, 
									0,
									castResult.m_normal,
									castResult.m_fraction
								);

							resultCallback.AddSingleResult(localRayResult);

						}
					}
				}
			}
			else
			{
				
				if (collisionShape->IsConcave())
					{

						TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionShape;
						
						SimdTransform worldTocollisionObject = colObjWorldTransform.inverse();

						SimdVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
						SimdVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();

						//ConvexCast::CastResult

						struct BridgeTriangleRaycastCallback : public TriangleRaycastCallback 
						{
							CollisionWorld::RayResultCallback* m_resultCallback;
							CollisionObject*	m_collisionObject;
							TriangleMeshShape*	m_triangleMesh;

							BridgeTriangleRaycastCallback( const SimdVector3& from,const SimdVector3& to,
								CollisionWorld::RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape*	triangleMesh):
								TriangleRaycastCallback(from,to),
									m_resultCallback(resultCallback),
									m_collisionObject(collisionObject),
									m_triangleMesh(triangleMesh)
								{
								}


							virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex )
							{
								CollisionWorld::LocalShapeInfo	shapeInfo;
								shapeInfo.m_shapePart = partId;
								shapeInfo.m_triangleIndex = triangleIndex;
								
								CollisionWorld::LocalRayResult rayResult
								(m_collisionObject, 
									&shapeInfo,
									hitNormalLocal,
									hitFraction);
								
								return m_resultCallback->AddSingleResult(rayResult);
								
								
							}
	
						};


						BridgeTriangleRaycastCallback	rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
						rcb.m_hitFraction = resultCallback.m_closestHitFraction;

						SimdVector3 rayAabbMinLocal = rayFromLocal;
						rayAabbMinLocal.setMin(rayToLocal);
						SimdVector3 rayAabbMaxLocal = rayFromLocal;
						rayAabbMaxLocal.setMax(rayToLocal);

						triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
											
					} else
					{
						//todo: use AABB tree or other BVH acceleration structure!
						if (collisionShape->IsCompound())
						{
							const CompoundShape* compoundShape = static_cast<const CompoundShape*>(collisionShape);
							int i=0;
							for (i=0;i<compoundShape->GetNumChildShapes();i++)
							{
								SimdTransform childTrans = compoundShape->GetChildTransform(i);
								const CollisionShape* childCollisionShape = compoundShape->GetChildShape(i);
								SimdTransform childWorldTrans = colObjWorldTransform * childTrans;
								RayTestSingle(rayFromTrans,rayToTrans,
									collisionObject,
									childCollisionShape,
									childWorldTrans,
									resultCallback);

							}


						}
					}
			}
}
예제 #7
0
void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
					  btCollisionObject* collisionObject,
					  const btCollisionShape* collisionShape,
					  const btTransform& colObjWorldTransform,
					  RayResultCallback& resultCallback)
{
	btSphereShape pointShape(btScalar(0.0));
	pointShape.setMargin(0.f);
	const btConvexShape* castShape = &pointShape;

	if (collisionShape->isConvex())
	{
//		BT_PROFILE("rayTestConvex");
		btConvexCast::CastResult castResult;
		castResult.m_fraction = resultCallback.m_closestHitFraction;

		btConvexShape* convexShape = (btConvexShape*) collisionShape;
		btVoronoiSimplexSolver	simplexSolver;
#define USE_SUBSIMPLEX_CONVEX_CAST 1
#ifdef USE_SUBSIMPLEX_CONVEX_CAST
		btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
#else
		//btGjkConvexCast	convexCaster(castShape,convexShape,&simplexSolver);
		//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
#endif //#USE_SUBSIMPLEX_CONVEX_CAST

		if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
		{
			//add hit
			if (castResult.m_normal.length2() > btScalar(0.0001))
			{
				if (castResult.m_fraction < resultCallback.m_closestHitFraction)
				{
#ifdef USE_SUBSIMPLEX_CONVEX_CAST
					//rotate normal into worldspace
					castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
#endif //USE_SUBSIMPLEX_CONVEX_CAST

					castResult.m_normal.normalize();
					btCollisionWorld::LocalRayResult localRayResult
						(
							collisionObject,
							0,
							castResult.m_normal,
							castResult.m_fraction
						);

					bool normalInWorldSpace = true;
					resultCallback.addSingleResult(localRayResult, normalInWorldSpace);

				}
			}
		}
	} else {
		if (collisionShape->isConcave())
		{
//			BT_PROFILE("rayTestConcave");
			if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
			{
				///optimized version for btBvhTriangleMeshShape
				btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
				btTransform worldTocollisionObject = colObjWorldTransform.inverse();
				btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
				btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();

				//ConvexCast::CastResult
				struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
				{
					btCollisionWorld::RayResultCallback* m_resultCallback;
					btCollisionObject*	m_collisionObject;
					btTriangleMeshShape*	m_triangleMesh;

					BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
						btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*	triangleMesh):
						btTriangleRaycastCallback(from,to),
							m_resultCallback(resultCallback),
							m_collisionObject(collisionObject),
							m_triangleMesh(triangleMesh)
						{
						}


					virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
					{
						btCollisionWorld::LocalShapeInfo	shapeInfo;
						shapeInfo.m_shapePart = partId;
						shapeInfo.m_triangleIndex = triangleIndex;

						btCollisionWorld::LocalRayResult rayResult
						(m_collisionObject,
							&shapeInfo,
							hitNormalLocal,
							hitFraction);

						bool	normalInWorldSpace = false;
						return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
					}

				};

				BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
				triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
			} else
			{
				//generic (slower) case
				btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;

				btTransform worldTocollisionObject = colObjWorldTransform.inverse();

				btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
				btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();

				//ConvexCast::CastResult

				struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
				{
					btCollisionWorld::RayResultCallback* m_resultCallback;
					btCollisionObject*	m_collisionObject;
					btConcaveShape*	m_triangleMesh;

					BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
						btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape*	triangleMesh):
						btTriangleRaycastCallback(from,to),
							m_resultCallback(resultCallback),
							m_collisionObject(collisionObject),
							m_triangleMesh(triangleMesh)
						{
						}


					virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
					{
						btCollisionWorld::LocalShapeInfo	shapeInfo;
						shapeInfo.m_shapePart = partId;
						shapeInfo.m_triangleIndex = triangleIndex;

						btCollisionWorld::LocalRayResult rayResult
						(m_collisionObject,
							&shapeInfo,
							hitNormalLocal,
							hitFraction);

						bool	normalInWorldSpace = false;
						return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);


					}

				};


				BridgeTriangleRaycastCallback	rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape);
				rcb.m_hitFraction = resultCallback.m_closestHitFraction;

				btVector3 rayAabbMinLocal = rayFromLocal;
				rayAabbMinLocal.setMin(rayToLocal);
				btVector3 rayAabbMaxLocal = rayFromLocal;
				rayAabbMaxLocal.setMax(rayToLocal);

				concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
			}
		} else {
//			BT_PROFILE("rayTestCompound");
			///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
			if (collisionShape->isCompound())
			{
				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
				int i=0;
				for (i=0;i<compoundShape->getNumChildShapes();i++)
				{
					btTransform childTrans = compoundShape->getChildTransform(i);
					const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
					btTransform childWorldTrans = colObjWorldTransform * childTrans;
					// replace collision shape so that callback can determine the triangle
					btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
					collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
					rayTestSingle(rayFromTrans,rayToTrans,
						collisionObject,
						childCollisionShape,
						childWorldTrans,
						resultCallback);
					// restore
					collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
				}
			}
		}
	}
}
예제 #8
0
void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
										btCollisionObject* collisionObject,
										const btCollisionShape* collisionShape,
										const btTransform& colObjWorldTransform,
										RayResultCallback& resultCallback)
{
	btSphereShape pointShape(btScalar(0.0));
	pointShape.setMargin(0.f);
	const btConvexShape* castShape = &pointShape;

	if (collisionShape->isConvex())
	{
		//		BT_PROFILE("rayTestConvex");
		btConvexCast::CastResult castResult;
		castResult.m_fraction = resultCallback.m_closestHitFraction;

		btConvexShape* convexShape = (btConvexShape*) collisionShape;
		btVoronoiSimplexSolver	simplexSolver;
#define USE_SUBSIMPLEX_CONVEX_CAST 1
#ifdef USE_SUBSIMPLEX_CONVEX_CAST
		btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
#else
		//btGjkConvexCast	convexCaster(castShape,convexShape,&simplexSolver);
		//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
#endif //#USE_SUBSIMPLEX_CONVEX_CAST

		if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
		{
			//add hit
			if (castResult.m_normal.length2() > btScalar(0.0001))
			{
				if (castResult.m_fraction < resultCallback.m_closestHitFraction)
				{
#ifdef USE_SUBSIMPLEX_CONVEX_CAST
					//rotate normal into worldspace
					castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
#endif //USE_SUBSIMPLEX_CONVEX_CAST

					castResult.m_normal.normalize();
					btCollisionWorld::LocalRayResult localRayResult
						(
						collisionObject,
						0,
						castResult.m_normal,
						castResult.m_fraction
						);

					bool normalInWorldSpace = true;
					resultCallback.addSingleResult(localRayResult, normalInWorldSpace);

				}
			}
		}
	} else {
		if (collisionShape->isConcave())
		{
			//			BT_PROFILE("rayTestConcave");
			if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
			{
				///optimized version for btBvhTriangleMeshShape
				btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
				btTransform worldTocollisionObject = colObjWorldTransform.inverse();
				btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
				btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();

				//ConvexCast::CastResult
				struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
				{
					btCollisionWorld::RayResultCallback* m_resultCallback;
					btCollisionObject*	m_collisionObject;
					btTriangleMeshShape*	m_triangleMesh;

					btTransform m_colObjWorldTransform;

					BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
						btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*	triangleMesh,const btTransform& colObjWorldTransform):
					//@BP Mod
					btTriangleRaycastCallback(from,to, resultCallback->m_flags),
						m_resultCallback(resultCallback),
						m_collisionObject(collisionObject),
						m_triangleMesh(triangleMesh),
						m_colObjWorldTransform(colObjWorldTransform)
					{
					}


					virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
					{
						btCollisionWorld::LocalShapeInfo	shapeInfo;
						shapeInfo.m_shapePart = partId;
						shapeInfo.m_triangleIndex = triangleIndex;

						btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;

						btCollisionWorld::LocalRayResult rayResult
							(m_collisionObject,
							&shapeInfo,
							hitNormalWorld,
							hitFraction);

						bool	normalInWorldSpace = true;
						return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
					}

				};

				BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh,colObjWorldTransform);
				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
				triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
			} else
			{
				//generic (slower) case
				btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;

				btTransform worldTocollisionObject = colObjWorldTransform.inverse();

				btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
				btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();

				//ConvexCast::CastResult

				struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
				{
					btCollisionWorld::RayResultCallback* m_resultCallback;
					btCollisionObject*	m_collisionObject;
					btConcaveShape*	m_triangleMesh;

					btTransform m_colObjWorldTransform;

					BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
						btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape*	triangleMesh, const btTransform& colObjWorldTransform):
					//@BP Mod
					btTriangleRaycastCallback(from,to, resultCallback->m_flags),
						m_resultCallback(resultCallback),
						m_collisionObject(collisionObject),
						m_triangleMesh(triangleMesh),
						m_colObjWorldTransform(colObjWorldTransform)
					{
					}


					virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
					{
						btCollisionWorld::LocalShapeInfo	shapeInfo;
						shapeInfo.m_shapePart = partId;
						shapeInfo.m_triangleIndex = triangleIndex;

						btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;

						btCollisionWorld::LocalRayResult rayResult
							(m_collisionObject,
							&shapeInfo,
							hitNormalWorld,
							hitFraction);

						bool	normalInWorldSpace = true;
						return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
					}

				};


				BridgeTriangleRaycastCallback	rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
				rcb.m_hitFraction = resultCallback.m_closestHitFraction;

				btVector3 rayAabbMinLocal = rayFromLocal;
				rayAabbMinLocal.setMin(rayToLocal);
				btVector3 rayAabbMaxLocal = rayFromLocal;
				rayAabbMaxLocal.setMax(rayToLocal);

				concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
			}
		} else {
			//			BT_PROFILE("rayTestCompound");
			if (collisionShape->isCompound())
			{
				struct LocalInfoAdder2 : public RayResultCallback
				{
					RayResultCallback* m_userCallback;
					int m_i;
					
					LocalInfoAdder2 (int i, RayResultCallback *user)
						: m_userCallback(user), m_i(i)
					{ 
						m_closestHitFraction = m_userCallback->m_closestHitFraction;
					}
					virtual bool needsCollision(btBroadphaseProxy* p) const
					{
						return m_userCallback->needsCollision(p);
					}

					virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b)
					{
						btCollisionWorld::LocalShapeInfo shapeInfo;
						shapeInfo.m_shapePart = -1;
						shapeInfo.m_triangleIndex = m_i;
						if (r.m_localShapeInfo == NULL)
							r.m_localShapeInfo = &shapeInfo;

						const btScalar result = m_userCallback->addSingleResult(r, b);
						m_closestHitFraction = m_userCallback->m_closestHitFraction;
						return result;
					}
				};
				
				struct RayTester : btDbvt::ICollide
				{
					btCollisionObject* m_collisionObject;
					const btCompoundShape* m_compoundShape;
					const btTransform& m_colObjWorldTransform;
					const btTransform& m_rayFromTrans;
					const btTransform& m_rayToTrans;
					RayResultCallback& m_resultCallback;
					
					RayTester(btCollisionObject* collisionObject,
							const btCompoundShape* compoundShape,
							const btTransform& colObjWorldTransform,
							const btTransform& rayFromTrans,
							const btTransform& rayToTrans,
							RayResultCallback& resultCallback):
						m_collisionObject(collisionObject),
						m_compoundShape(compoundShape),
						m_colObjWorldTransform(colObjWorldTransform),
						m_rayFromTrans(rayFromTrans),
						m_rayToTrans(rayToTrans),
						m_resultCallback(resultCallback)
					{
						
					}
					
					void Process(int i)
					{
						const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i);
						const btTransform& childTrans = m_compoundShape->getChildTransform(i);
						btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
						
						// replace collision shape so that callback can determine the triangle
						btCollisionShape* saveCollisionShape = m_collisionObject->getCollisionShape();
						m_collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);

						LocalInfoAdder2 my_cb(i, &m_resultCallback);

						rayTestSingle(
							m_rayFromTrans,
							m_rayToTrans,
							m_collisionObject,
							childCollisionShape,
							childWorldTrans,
							my_cb);
						
						// restore
						m_collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
					}
					
					void Process(const btDbvtNode* leaf)
					{
						Process(leaf->dataAsInt);
					}
				};
				
				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
				const btDbvt* dbvt = compoundShape->getDynamicAabbTree();


				RayTester rayCB(
					collisionObject,
					compoundShape,
					colObjWorldTransform,
					rayFromTrans,
					rayToTrans,
					resultCallback);
#ifndef	DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
				if (dbvt)
				{
					btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin();
					btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin();
					btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB);
				}
				else
#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
				{
					for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i)
					{
						rayCB.Process(i);
					}	
				}
			}
		}
	}
}