예제 #1
0
        /// <summary>
        /// cast a convex against another convex object
        /// </summary>
        /// <param name="fromA"></param>
        /// <param name="toA"></param>
        /// <param name="fromB"></param>
        /// <param name="toB"></param>
        /// <param name="result"></param>
        /// <returns></returns>
        public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
        {
            MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB);

            Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
            Matrix rayToLocalA   = MathHelper.InvertMatrix(toA) * toB;

            Matrix transformA = fromA;
            Matrix transformB = fromB;

            transformA.Translation = new Vector3(0, 0, 0);
            transformB.Translation = new Vector3(0, 0, 0);

            combined.TransformA = transformA;
            combined.TransformB = transformB;

            float radius = 0.01f;
            float lambda = 0;

            Vector3 s = rayFromLocalA.Translation;
            Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation;
            Vector3 x = s;
            Vector3 n = new Vector3();
            Vector3 c = new Vector3();

            bool  hasResult  = false;
            float lastLambda = lambda;

            IConvexPenetrationDepthSolver penSolver = null;
            Matrix identityTransform = Matrix.Identity;

            SphereShape raySphere = new SphereShape(0.0f);

            raySphere.Margin = 0.0f;

            Matrix sphereTransform = Matrix.Identity;

            sphereTransform.Translation = rayFromLocalA.Translation;

            result.DrawCoordSystem(sphereTransform);

            {
                PointCollector  pointCollector = new PointCollector();
                GjkPairDetector gjk            = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);

                GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
                input.TransformA = sphereTransform;
                input.TransformB = identityTransform;

                gjk.GetClosestPoints(input, pointCollector, null);

                hasResult = pointCollector.HasResult;

                c = pointCollector.PointInWorld;
                n = pointCollector.NormalOnBInWorld;
            }

            if (hasResult)
            {
                float dist = (c - x).Length();

                if (dist < radius)
                {
                    lastLambda = 1.0f;
                }

                while (dist > radius)
                {
                    n = x - c;
                    float dot = Vector3.Dot(n, r);

                    if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon))
                    {
                        return(false);
                    }

                    lambda = lambda - Vector3.Distance(n, n) / dot;
                    if (lambda <= lastLambda)
                    {
                        break;
                    }

                    lastLambda = lambda;

                    x = s + lambda * r;

                    sphereTransform.Translation = x;
                    result.DrawCoordSystem(sphereTransform);
                    PointCollector pointCollector = new PointCollector();

                    GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
                    GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
                    input.TransformA = sphereTransform;
                    input.TransformB = identityTransform;

                    gjk.GetClosestPoints(input, pointCollector, null);

                    if (pointCollector.HasResult)
                    {
                        if (pointCollector.Distance < 0.0f)
                        {
                            result.Fraction = lastLambda;
                            result.Normal   = n;
                            return(true);
                        }

                        c    = pointCollector.PointInWorld;
                        dist = (c - x).Length();
                    }
                    else
                    {
                        return(false);
                    }
                }

                if (lastLambda < 1.0f)
                {
                    result.Fraction = lastLambda;
                    result.Normal   = n;
                    return(true);
                }
            }

            return(false);
        }
예제 #2
0
        /// <summary>
        /// cast a convex against another convex object
        /// </summary>
        /// <param name="fromA"></param>
        /// <param name="toA"></param>
        /// <param name="fromB"></param>
        /// <param name="toB"></param>
        /// <param name="result"></param>
        /// <returns></returns>
		public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
		{
			MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB);

            Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
            Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB;

			Matrix transformA = fromA;
			Matrix transformB = fromB;

			transformA.Translation = new Vector3(0, 0, 0);
			transformB.Translation = new Vector3(0, 0, 0);

			combined.TransformA = transformA;
			combined.TransformB = transformB;

			float radius = 0.01f;
			float lambda = 0;

			Vector3 s = rayFromLocalA.Translation;
			Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation;
			Vector3 x = s;
			Vector3 n = new Vector3();
			Vector3 c = new Vector3();

			bool hasResult = false;
			float lastLambda = lambda;

			IConvexPenetrationDepthSolver penSolver = null;
			Matrix identityTransform = Matrix.Identity;

			SphereShape raySphere = new SphereShape(0.0f);
			raySphere.Margin=0.0f;

			Matrix sphereTransform = Matrix.Identity;
			sphereTransform.Translation = rayFromLocalA.Translation;

			result.DrawCoordSystem(sphereTransform);

			{
				PointCollector pointCollector = new PointCollector();
				GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);

				GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
				input.TransformA = sphereTransform;
				input.TransformB = identityTransform;

				gjk.GetClosestPoints(input, pointCollector, null);

				hasResult = pointCollector.HasResult;

				c = pointCollector.PointInWorld;
				n = pointCollector.NormalOnBInWorld;
			}

			if (hasResult)
			{
				float dist = (c - x).Length();

				if (dist < radius)
				{
					lastLambda = 1.0f;
				}

				while (dist > radius)
				{
					n = x - c;
					float dot = Vector3.Dot(n, r);

					if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false;

					lambda = lambda - Vector3.Distance(n, n) / dot;
					if (lambda <= lastLambda) break;

					lastLambda = lambda;

					x = s + lambda * r;

					sphereTransform.Translation = x;
					result.DrawCoordSystem(sphereTransform);
					PointCollector pointCollector = new PointCollector();

					GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
					GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
					input.TransformA = sphereTransform;
					input.TransformB = identityTransform;

					gjk.GetClosestPoints(input, pointCollector, null);

					if (pointCollector.HasResult)
					{
						if (pointCollector.Distance < 0.0f)
						{
							result.Fraction = lastLambda;
							result.Normal = n;
							return true;
						}

						c = pointCollector.PointInWorld;
						dist = (c - x).Length();
					}
					else
					{
						return false;
					}
				}

				if (lastLambda < 1.0f)
				{
					result.Fraction = lastLambda;
					result.Normal = n;
					return true;
				}
			}

			return false;
		}
예제 #3
0
		public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
		{
			_simplexSolver.Reset();

			// compute linear and angular velocity for this interval, to interpolate
			Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3();
			TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA);
			TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB);

			float boundingRadiusA = _convexA.GetAngularMotionDisc();
			float boundingRadiusB = _convexB.GetAngularMotionDisc();

			float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA +
				angVelB.Length() * boundingRadiusB;

			float radius = 0.001f;

			float lambda = 0f;
			Vector3 v = new Vector3(1f, 0f, 0f);

			int maxIter = MaxIterations;

			Vector3 n = new Vector3();
			bool hasResult = false;
			Vector3 c;

			float lastLambda = lambda;
			//float epsilon = 0.001f;

			int numIter = 0;
			//first solution, using GJK


			Matrix identityTrans = Matrix.Identity;

			SphereShape raySphere = new SphereShape(0f);
			raySphere.Margin=0f;


			//result.drawCoordSystem(sphereTr);

			PointCollector pointCollector1 = new PointCollector();

			GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
			GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();

			//we don't use margins during CCD
			gjk.setIgnoreMargin(true);

			input.TransformA = fromA;
			input.TransformB = fromB;

			DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
			gjk.GetClosestPoints(input, r, null);

			hasResult = pointCollector1.HasResult;
			c = pointCollector1.PointInWorld;

			if (hasResult)
			{
				float dist;
				dist = pointCollector1.Distance;
				n = pointCollector1.NormalOnBInWorld;

				//not close enough
				while (dist > radius)
				{
					numIter++;
					if (numIter > maxIter)
						return false; //todo: report a failure

					float dLambda = 0f;

					//calculate safe moving fraction from distance / (linear+rotational velocity)

					//float clippedDist  = GEN_min(angularConservativeRadius,dist);
					//float clippedDist  = dist;

					float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n);

					dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);

					lambda = lambda + dLambda;

					if (lambda > 1f) return false;
					if (lambda < 0f) return false;

					//todo: next check with relative epsilon
					if (lambda <= lastLambda)
						break;
					lastLambda = lambda;


					//interpolate to next lambda
					Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans;

					TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA);
					TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB);

					relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA);

					result.DebugDraw(lambda);

					PointCollector pointCollector = new PointCollector();
					gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
					input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
					input.TransformA = interpolatedTransA;
					input.TransformB = interpolatedTransB;

					// !!!!!!!!!!
					r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
					gjk.GetClosestPoints(input, r, null);

					if (pointCollector.HasResult)
					{
						if (pointCollector.Distance < 0f)
						{
							//degenerate ?!
							result.Fraction = lastLambda;
							result.Normal = n;
							return true;
						}
						c = pointCollector.PointInWorld;

						dist = pointCollector.Distance;
					}
					else
					{
						//??
						return false;
					}

				}

				result.Fraction = lambda;
				result.Normal = n;
				return true;
			}

			return false;
		}
        public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
        {
            _simplexSolver.Reset();

            // compute linear and angular velocity for this interval, to interpolate
            Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3();

            TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA);
            TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB);

            float boundingRadiusA = _convexA.GetAngularMotionDisc();
            float boundingRadiusB = _convexB.GetAngularMotionDisc();

            float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA +
                                                angVelB.Length() * boundingRadiusB;

            float radius = 0.001f;

            float   lambda = 0f;
            Vector3 v      = new Vector3(1f, 0f, 0f);

            int maxIter = MaxIterations;

            Vector3 n         = new Vector3();
            bool    hasResult = false;
            Vector3 c;

            float lastLambda = lambda;
            //float epsilon = 0.001f;

            int numIter = 0;
            //first solution, using GJK


            Matrix identityTrans = Matrix.Identity;

            SphereShape raySphere = new SphereShape(0f);

            raySphere.Margin = 0f;


            //result.drawCoordSystem(sphereTr);

            PointCollector pointCollector1 = new PointCollector();

            GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);

            GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();

            //we don't use margins during CCD
            gjk.setIgnoreMargin(true);

            input.TransformA = fromA;
            input.TransformB = fromB;

            DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
            gjk.GetClosestPoints(input, r, null);

            hasResult = pointCollector1.HasResult;
            c         = pointCollector1.PointInWorld;

            if (hasResult)
            {
                float dist;
                dist = pointCollector1.Distance;
                n    = pointCollector1.NormalOnBInWorld;

                //not close enough
                while (dist > radius)
                {
                    numIter++;
                    if (numIter > maxIter)
                    {
                        return(false);                        //todo: report a failure
                    }
                    float dLambda = 0f;

                    //calculate safe moving fraction from distance / (linear+rotational velocity)

                    //float clippedDist  = GEN_min(angularConservativeRadius,dist);
                    //float clippedDist  = dist;

                    float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n);

                    dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);

                    lambda = lambda + dLambda;

                    if (lambda > 1f)
                    {
                        return(false);
                    }
                    if (lambda < 0f)
                    {
                        return(false);
                    }

                    //todo: next check with relative epsilon
                    if (lambda <= lastLambda)
                    {
                        break;
                    }
                    lastLambda = lambda;


                    //interpolate to next lambda
                    Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans;

                    TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA);
                    TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB);

                    relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA);

                    result.DebugDraw(lambda);

                    PointCollector pointCollector = new PointCollector();
                    gjk              = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
                    input            = new DiscreteCollisionDetectorInterface.ClosestPointInput();
                    input.TransformA = interpolatedTransA;
                    input.TransformB = interpolatedTransB;

                    // !!!!!!!!!!
                    r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
                    gjk.GetClosestPoints(input, r, null);

                    if (pointCollector.HasResult)
                    {
                        if (pointCollector.Distance < 0f)
                        {
                            //degenerate ?!
                            result.Fraction = lastLambda;
                            result.Normal   = n;
                            return(true);
                        }
                        c = pointCollector.PointInWorld;

                        dist = pointCollector.Distance;
                    }
                    else
                    {
                        //??
                        return(false);
                    }
                }

                result.Fraction = lambda;
                result.Normal   = n;
                return(true);
            }

            return(false);
        }