Esempio n. 1
0
		public GjkConvexCast(ConvexShape convexShapeA, ConvexShape convexShapeB, VoronoiSimplexSolver solver)
		{
			_simplexSolver = solver;

			_convexA = convexShapeA;
			_convexB = convexShapeB;
		}
Esempio n. 2
0
        public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            //Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold

            //Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
            //col0->m_worldTransform,
            float resultFraction = 1f;

            float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared();
            float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared();

            if (squareMotA < colA.CcdSquareMotionThreshold &&
                squareMotB < colB.CcdSquareMotionThreshold)
                return resultFraction;

            if (DisableCcd)
                return 1f;

            //An adhoc way of testing the Continuous Collision Detection algorithms
            //One object is approximated as a sphere, to simplify things
            //Starting in penetration should report no time of impact
            //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
            //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)

            // Convex0 against sphere for Convex1
            {
                ConvexShape convexA = colA.CollisionShape as ConvexShape;

                SphereShape sphereB = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
                CastResult result = new CastResult();
                VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                //Simplification, one object is simplified as a sphere
                GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex);
                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
                if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
                    colB.WorldTransform, colB.InterpolationWorldTransform, result))
                {
                    //store result.m_fraction in both bodies
                    if (colA.HitFraction > result.Fraction)
                        colA.HitFraction = result.Fraction;

                    if (colB.HitFraction > result.Fraction)
                        colB.HitFraction = result.Fraction;

                    if (resultFraction > result.Fraction)
                        resultFraction = result.Fraction;
                }
            }

            // Sphere (for convex0) against Convex1
            {
                ConvexShape convexB = colB.CollisionShape as ConvexShape;

                SphereShape sphereA = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
                CastResult result = new CastResult();
                VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                ///Simplification, one object is simplified as a sphere
                GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex);
                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
                if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
                    colB.WorldTransform, colB.InterpolationWorldTransform, result))
                {
                    //store result.m_fraction in both bodies
                    if (colA.HitFraction > result.Fraction)
                        colA.HitFraction = result.Fraction;

                    if (colB.HitFraction > result.Fraction)
                        colB.HitFraction = result.Fraction;

                    if (resultFraction > result.Fraction)
                        resultFraction = result.Fraction;
                }
            }
            return resultFraction;
        }
Esempio n. 3
0
		// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
		// In a future implementation, we consider moving the ray test as a virtual method in CollisionShape.
		// This allows more customization.
		public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans,
						  CollisionObject collisionObject,
						  CollisionShape collisionShape,
						  Matrix colObjWorldTransform,
						  RayResultCallback resultCallback)
		{
			SphereShape pointShape=new SphereShape(0.0f);

			if (collisionShape.IsConvex)
			{
				CastResult castResult = new CastResult();
				castResult.Fraction = 1f;//??

				ConvexShape convexShape = collisionShape as ConvexShape;
				VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
				SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(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.Normal.LengthSquared() > 0.0001f)
					{
						castResult.Normal.Normalize();
						if (castResult.Fraction < resultCallback.ClosestHitFraction)
						{

							CollisionWorld.LocalRayResult localRayResult = new LocalRayResult
								(
									collisionObject,
									new LocalShapeInfo(),
									castResult.Normal,
									castResult.Fraction
								);

							resultCallback.AddSingleResult(localRayResult);
						}
					}
				}
				else
				{
					if (collisionShape.IsConcave)
					{

						TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape;

						Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform);

						Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject);
						Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject);

						BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh);
						rcb.HitFraction = resultCallback.ClosestHitFraction;

						Vector3 rayAabbMinLocal = rayFromLocal;
						MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal);
						Vector3 rayAabbMaxLocal = rayFromLocal;
						MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal);

						triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal);
					}
					else
					{
						//todo: use AABB tree or other BVH acceleration structure!
						if (collisionShape.IsCompound)
						{
							CompoundShape compoundShape = collisionShape as CompoundShape;
							for (int i = 0; i < compoundShape.ChildShapeCount; i++)
							{
								Matrix childTrans = compoundShape.GetChildTransform(i);
								CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
								Matrix childWorldTrans = colObjWorldTransform * childTrans;
								RayTestSingle(rayFromTrans, rayToTrans,
									collisionObject,
									childCollisionShape,
									childWorldTrans,
									resultCallback);
							}
						}
					}
				}
			}
		}
			public void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex)
			{
				//do a swept sphere for now
				Matrix ident = Matrix.Identity;
				CastResult castResult = new CastResult();
				castResult.Fraction = _hitFraction;
				SphereShape pointShape = new SphereShape(_ccdSphereRadius);
				TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]);
				VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
				SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver);
				//GjkConvexCast	convexCaster(&pointShape,convexShape,&simplexSolver);
				//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
				//local space?

				if (convexCaster.CalcTimeOfImpact(_ccdSphereFromTrans, _ccdSphereToTrans,
					ident, ident, castResult))
				{
					if (_hitFraction > castResult.Fraction)
						_hitFraction = castResult.Fraction;
				}
			}