1 files changed, 199 insertions, 199 deletions

diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs
index 497f220..d011ef2 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs
@@ -1,199 +1,199 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-        /// <summary>

-        /// ContinuousConvexCollision implements angular and linear time of impact for convex objects.

-        /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).

-        /// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.

-        /// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops

-        /// </summary>

-        public class ContinuousConvexCollision : IConvexCast

-        {

-                /// <summary>

-                /// This maximum should not be necessary. It allows for untested/degenerate cases in production code.

-                /// You don't want your game ever to lock-up.

-                /// </summary>

-                private const int MaxIterations = 1000;

-

-                private ISimplexSolver _simplexSolver;

-                private IConvexPenetrationDepthSolver _penetrationDepthSolver;

-                private ConvexShape _convexA;

-                private ConvexShape _convexB;

-

-                public ContinuousConvexCollision(ConvexShape convexA, ConvexShape convexB,

-                        ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)

-                {

-                        _simplexSolver = simplexSolver;

-                        _penetrationDepthSolver = penetrationDepthSolver;

-                        _convexA = convexA;

-                        _convexB = convexB;

-                }

-

-                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;

-                }

-        }

-}

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+        /// <summary>
+        /// ContinuousConvexCollision implements angular and linear time of impact for convex objects.
+        /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).
+        /// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.
+        /// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops
+        /// </summary>
+        public class ContinuousConvexCollision : IConvexCast
+        {
+                /// <summary>
+                /// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
+                /// You don't want your game ever to lock-up.
+                /// </summary>
+                private const int MaxIterations = 1000;
+
+                private ISimplexSolver _simplexSolver;
+                private IConvexPenetrationDepthSolver _penetrationDepthSolver;
+                private ConvexShape _convexA;
+                private ConvexShape _convexB;
+
+                public ContinuousConvexCollision(ConvexShape convexA, ConvexShape convexB,
+                        ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
+                {
+                        _simplexSolver = simplexSolver;
+                        _penetrationDepthSolver = penetrationDepthSolver;
+                        _convexA = convexA;
+                        _convexB = convexB;
+                }
+
+                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;
+                }
+        }
+}