iogrecollisionshape.cpp

opcode是功能强大

	bool ICollisionShape::rayCheck(CollisionType collType,
		const Matrix4& ownMatrix,
		const Ogre::Ray& line,
		const Real dist,
		CollisionPair& collPair, bool rayCulling)
	{
		assert(COLLTYPE_IGNORE != collType);

		// setup ray collider
		Opcode::RayCollider& collider = CollisionManager::getSingletonPtr()->opcRayCollider;
		collider.SetMaxDist(dist);
		collider.SetCulling(rayCulling);
		collider.SetClosestHit(true);
		switch (collType)
		{
		case COLLTYPE_QUICK:
		case COLLTYPE_CONTACT:
			collider.SetFirstContact(false);
			break;

		case COLLTYPE_EXACT:
			collider.SetFirstContact(false);
			break;

		default:
			break;
		}

		// convert Matrix4 to Opcode Matrix4x4
		IceMaths::Matrix4x4 opcMatrix;
		IceMaths::Matrix4x4 *ptrOpcMatrix = &opcMatrix;
		// if model is in world space already (local to world matrix, ownMatrix, is identity), then pass 0
		if (ownMatrix == Matrix4::IDENTITY) {
			ptrOpcMatrix = 0;
		} else {
			OgreOpcodeUtils::ogreToIceMatrix4( ownMatrix, opcMatrix);
		}

		// build Opcode ray from line
		IceMaths::Ray ray;
			OgreOpcodeUtils::ogreToIceRay( line, ray );

		// validate settings: asserts that we can check collision
		// another option is to display some annoying error windows using: Ogre::String(collider.ValidateSettings() )
		assert( collider.ValidateSettings() == 0 );

		// perform collision
		if( !collider.Collide(ray, opcModel, ptrOpcMatrix) )
			return false;

		collPair.numBVBVTests = collider.GetNbRayBVTests();
		collPair.numBVPrimTests = collider.GetNbRayPrimTests();
		collPair.numPrimPrimTests = 0;

		// get collision result
		if (collider.GetContactStatus())
		{
			// fill out contact point and collision normal of closest contact
			const Opcode::CollisionFace* collFaces = opcFaceCache->GetFaces();
			int numFaces = opcFaceCache->GetNbFaces();
			if (numFaces > 0)
			{
				// if in closest hit mode, find the contact with the smallest distance
				int collFaceIndex = 0;
				//if (COLLTYPE_CONTACT)
				if (1==1) // FIXME
				{
					int i;
					for (i = 0; i < numFaces; i++)
					{
						if (collFaces[i].mDistance < collFaces[collFaceIndex].mDistance)
						{
							collFaceIndex = i;
						}
					}
				}
				int triangleIndex = collFaces[collFaceIndex].mFaceID;
				float thedist = collFaces[collFaceIndex].mDistance;

				// build triangle from from faceIndex
				Vector3 v0,v1,v2;
				getTriCoords(triangleIndex, v0, v1, v2);

				//const Matrix4 &myMatrix = getFullTransform();

				// Compute the centered normal
				//Vector3 vCenter = (v0+v1+v2)*0.33333333333333333333f;
				Vector3 vNormal = (v1-v0).crossProduct(v2-v0);
				vNormal.normalise();

				// Compute collision contact from barycentric coordinates
				Real mU = collFaces[0].mU;
				Real mV = collFaces[0].mV;
				Real mW = 1.0f - mU - mV;
				Vector3 vCenter(Vector3::ZERO);
				vCenter.x = (v0.x * mW) + (v1.x * mU) + (v2.x * mV);
				vCenter.y = (v0.y * mW) + (v1.y * mU) + (v2.y * mV);
				vCenter.z = (v0.z * mW) + (v1.z * mU) + (v2.z * mV);

				collPair.contact = ownMatrix * vCenter;//line.getOrigin() + (line.getDirection().normalisedCopy() * thedist);//myMatrix * vContact;//
				collPair.distance = thedist;
				collPair.this_normal = vNormal;
				collPair.other_normal = line.getDirection().normalisedCopy();//-collPair.this_normal;
				CollisionInfo collInfo;
				collInfo.contact = collPair.contact;
				collInfo.distance = collPair.distance;
				collInfo.this_normal = collPair.this_normal;
				collInfo.other_normal = collPair.other_normal;
				collPair.collInfos.push_back(collInfo);

				return true;
			}
			else
			{
				//n_printf("nOpcodeShape::rayCheck(): contact but no faces!\n");
				return false;
			}
		}
		return false;
	}

	//------------------------------------------------------------------------
	/// Check contact of a line swept sphere with shape.
	/// The collType is interpreted as follows:
	/// - COLLTYPE_IGNORE:        illegal (makes no sense)
	/// - COLLTYPE_QUICK:         first contact check only
	/// - COLLTYPE_CONTACT:       return closest contact
	/// - COLLTYPE_EXACT:         return a sorted list of contacts
	/// Currently, sphere checks always work in first contact mode (COLLTYPE_QUICK).
	/// @param  collType        see above
	/// @param  ownMatrix       position/orientation of this shape
	/// @param  ball          sphere definition in world space
	/// @param  collPair      will be filled with result
	/// @return                 true if line intersects shape
	bool ICollisionShape::sweptSphereCheck(CollisionType collType,
		const Matrix4& ownMatrix,
		const Vector3& position,
		const Vector3& movementVector,
		const Real& radius,
		CollisionPair& collPair)
	{
		assert(COLLTYPE_IGNORE != collType);

		// setup sphere collider
		Opcode::LSSCollider& collider = CollisionManager::getSingletonPtr()->opcSweptSphereCollider;
		Opcode::LSSCache& cache = CollisionManager::getSingletonPtr()->opcSweptSphereCache;

		switch (collType)
		{
		case COLLTYPE_QUICK:
		case COLLTYPE_CONTACT:
			collider.SetFirstContact(true);
			break;

		case COLLTYPE_EXACT:
			collider.SetFirstContact(false);
			break;

		default:
			break;
		}

		// convert Matrix4 to Opcode Matrix4x4
		IceMaths::Matrix4x4 opcMatrix;
		IceMaths::Matrix4x4 *ptrOpcMatrix = &opcMatrix;
		OgreOpcodeUtils::ogreToIceMatrix4( ownMatrix, opcMatrix);

		// build identity matrix because sphere is already in world space
		IceMaths::Matrix4x4 identity;
		IceMaths::Matrix4x4 *ptrIdentity = &identity;
		identity.Identity();

		// validate settings: asserts that we can check collision
		// another option is to display some annoying error windows using: String(collider.ValidateSettings() )
		assert( collider.ValidateSettings() == 0 );

		IceMaths::Point startPoint;
		IceMaths::Point endPoint;
		OgreOpcodeUtils::ogreToIceVector3(position, startPoint);
		OgreOpcodeUtils::ogreToIceVector3(position + movementVector, endPoint);
		IceMaths::Segment opcSegment(startPoint, endPoint);
		IceMaths::LSS opcLSS(opcSegment, radius);

		// perform collision
		if( !collider.Collide(cache, opcLSS, opcModel, ptrIdentity, ptrOpcMatrix) )
			return false;

		collPair.distance = movementVector.length();

		collPair.numBVBVTests = collider.GetNbVolumeBVTests();
		collPair.numBVPrimTests = collider.GetNbVolumePrimTests();
		collPair.numPrimPrimTests = 0;

		// get collision result
		if (collider.GetContactStatus())
		{
			// fill out contact point and collision normal of closest contact
			const udword* collFaces = collider.GetTouchedPrimitives();
			int numFaces = collider.GetNbTouchedPrimitives();
			//LogManager::getSingleton().logMessage("sphereCheck returned " + StringConverter::toString(numFaces) + " numfaces");
			if (numFaces > 0)
			{
				for(int i = 0; i < numFaces; i++)
				{
					//assert(1 == numFaces);

					// build triangle from from faceIndex
					Vector3 v0,v1,v2;
					getTriCoords(collFaces[i], v0, v1, v2);

					v0 = ownMatrix * v0;		// transform to world space
					v1 = ownMatrix * v1;
					v2 = ownMatrix * v2;



					testTriangleIntersection(position, movementVector, radius, v0, v1, v2, &collPair);


				}

				//if(	collPair.distance == movementVector.length())
					//return false;
				//else
					return true;
			}
			else
			{
				//n_printf("nOpcodeShape::sphereCheck(): contact but no faces!\n");
				return false;
			}
		}

		// FIXME!
		return false;
	}

////////////////////////////////////////////////////////////////////////////////
// This code adapted from code from Irrlicht (http://www.irrlicht3d.org)

	inline bool ICollisionShape::getLowestRoot(Ogre::Real a, Ogre::Real b, Ogre::Real c, Ogre::Real maxR, Ogre::Real* root)
	{
		// check if solution exists
		Ogre::Real determinant = b*b - 4.0f*a*c;

		// if determinant is negative, no solution
		if (determinant < 0.0f) return false;

		// calculate two roots: (if det==0 then x1==x2
		// but lets disregard that slight optimization)

		Ogre::Real sqrtD = (Ogre::Real)sqrt(determinant);
		Ogre::Real r1 = (-b - sqrtD) / (2*a);
		Ogre::Real r2 = (-b + sqrtD) / (2*a);

		// sort so x1 <= x2
		if (r1 > r2) { Ogre::Real tmp=r2; r2=r1; r1=tmp; }

		// get lowest root
		if (r1 > 0 && r1 < maxR)
		{
			*root = r1;
			return true;
		}

		// its possible that we want x2, this can happen if x1 < 0
		if (r2 > 0 && r2 < maxR)
		{
			*root = r2;
			return true;
		}

		return false;
	}

/////////////////////////////

	void ICollisionShape::sphereEdgeCheck(Ogre::Vector3 &velocity,
		Ogre::Vector3 &edge, Ogre::Vector3 &baseToVertex,
		Ogre::Real &t, bool &foundCollision,
		Ogre::Vector3 &collisionPoint, Ogre::Vector3 &pnt)
	{
		Ogre::Real newT, a,b,c;

		Ogre::Real edgeSqaredLength = (Ogre::Real)edge.squaredLength();
		Ogre::Real edgeDotVelocity = edge.dotProduct(velocity);
		Ogre::Real edgeDotBaseToVertex = edge.dotProduct(baseToVertex);

		// calculate parameters for equation
		Ogre::Real velocitySqaredLength = velocity.squaredLength();

		a = edgeSqaredLength* -velocitySqaredLength +
			edgeDotVelocity*edgeDotVelocity;
		b = edgeSqaredLength* (2*velocity.dotProduct(baseToVertex)) -
			2.0f*edgeDotVelocity*edgeDotBaseToVertex;
		c = (Ogre::Real)(edgeSqaredLength* (1-baseToVertex.squaredLength()) +
			edgeDotBaseToVertex*edgeDotBaseToVertex);

		// does the swept sphere collide against ininite edge?
		if (getLowestRoot(a,b,c,t,&newT))
		{
			Ogre::Real f = (edgeDotVelocity*newT - edgeDotBaseToVertex) / edgeSqaredLength;
			if (f >=0.0f && f <= 1.0f)
			{
				// intersection took place within segment
				t = newT;
				foundCollision = true;
				collisionPoint = pnt + (edge*f);
			}
		}
	}

////////////////////////////////////

	bool ICollisionShape::testTriangleIntersection(Ogre::Vector3 position,
		Ogre::Vector3 movementVector,
		Ogre::Real radius,
		Ogre::Vector3 v0, Ogre::Vector3 v1, Ogre::Vector3 v2,
		CollisionPair *cp)
	{

		if(movementVector == Ogre::Vector3::ZERO)
			return false;

		Ogre::Real scale_fact = 1.0 / radius;
		v0 *= scale_fact;
		v1 *= scale_fact;
		v2 *= scale_fact;

		position *= scale_fact;
		movementVector *= scale_fact;

		triangle triangle(v0, v1, v2);

		Ogre::Plane trianglePlane = triangle.getplane();

		trianglePlane.normal.normalise();

		// only check front facing polygons
		if (trianglePlane.getSide(position) == Ogre::Plane::POSITIVE_SIDE)
		{
			// get interval of plane intersection

			Ogre::Real t1, t0;
			bool embeddedInPlane = false;

			// calculate signed distance from sphere position to triangle plane
			Ogre::Real signedDistToTrianglePlane = trianglePlane.getDistance(position);

			Ogre::Real normalDotVelocity = trianglePlane.normal.dotProduct(movementVector);

			if (normalDotVelocity == 0.0f)
			{
				// sphere is traveling paralell to plane

				if (fabs(signedDistToTrianglePlane) >= 1.0f)
					return false; // no collision possible
				else
				{
					// sphere is embedded in plane
					embeddedInPlane = true;
					t0 = 0.0;
					t1 = 1.0;
				}
			}
			else
			{
				// N.D is not 0. Calculate intersection interval
				t0 = (-1.0-signedDistToTrianglePlane)/normalDotVelocity;
				t1 = (1.0-signedDistToTrianglePlane)/normalDotVelocity;

				// Swap so t0 < t1
				if (t0 > t1) { Ogre::Real tmp = t1; t1 = t0; t0 = tmp;	}

				// check if at least one value is within the range
				if (t0 > 1.0f || t1 < 0.0f)
					return false; // both t values are outside 1 and 0, no collision possible

				// clamp to 0 and 1
				if (t0 < 0.0) t0 = 0.0;
				if (t1 < 0.0) t1 = 0.0;
				if (t0 > 1.0) t0 = 1.0;
				if (t1 > 1.0) t1 = 1.0;
			}

			// at this point we have t0 and t1, if there is any intersection, it
			// is between this interval

			Ogre::Vector3 collisionPoint = Vector3::ZERO;
			bool foundCollision = false;
			Ogre::Real t = 1.0f;

			// first check the easy case: Collision within the triangle;
			// if this happens, it must be at t0 and this is when the sphere
			// rests on the front side of the triangle plane. This can only happen
			// if the sphere is not embedded in the triangle plane.

			if (!embeddedInPlane)
			{
				Ogre::Vector3 planeIntersectionPoint =
					(position - trianglePlane.normal)
					+ (movementVector * (Ogre::Real)t0);


				if (triangle.isPointInsideFast(planeIntersectionPoint))
				{
					foundCollision = true;
					t = (Ogre::Real)t0;
					collisionPoint = planeIntersectionPoint;
				}
			}

			// if we havent found a collision already we will have to sweep
			// the sphere against points and edges of the triangle. Note: A
			// collision inside the triangle will always happen before a
			// vertex or edge collision.

			// DAVE: is checking against points really necessary if we are checking against edges?
			// Shouldn't the edges take care of that?

			if (!foundCollision)