1 files changed, 199 insertions, 0 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs
new file mode 100644
index 0000000..497f220
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs
@@ -0,0 +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;
+                }
+        }
+}

diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs new file mode 100644 index 0000000..497f220 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs
@@ -0,0 +1,199 @@
	1	/*
	2	Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
	3	Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
	4
	5	This software is provided 'as-is', without any express or implied
	6	warranty. In no event will the authors be held liable for any damages
	7	arising from the use of this software.
	8
	9	Permission is granted to anyone to use this software for any purpose,
	10	including commercial applications, and to alter it and redistribute it
	11	freely, subject to the following restrictions:
	12
	13	1. The origin of this software must not be misrepresented; you must not
	14	claim that you wrote the original software. If you use this software
	15	in a product, an acknowledgment in the product documentation would be
	16	appreciated but is not required.
	17	2. Altered source versions must be plainly marked as such, and must not be
	18	misrepresented as being the original software.
	19	3. This notice may not be removed or altered from any source distribution.
	20	*/
	21
	22	using System;
	23	using System.Collections.Generic;
	24	using System.Text;
	25	using MonoXnaCompactMaths;
	26
	27	namespace XnaDevRu.BulletX
	28	{
	29	/// <summary>
	30	/// ContinuousConvexCollision implements angular and linear time of impact for convex objects.
	31	/// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).
	32	/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.
	33	/// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops
	34	/// </summary>
	35	public class ContinuousConvexCollision : IConvexCast
	36	{
	37	/// <summary>
	38	/// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
	39	/// You don't want your game ever to lock-up.
	40	/// </summary>
	41	private const int MaxIterations = 1000;
	42
	43	private ISimplexSolver _simplexSolver;
	44	private IConvexPenetrationDepthSolver _penetrationDepthSolver;
	45	private ConvexShape _convexA;
	46	private ConvexShape _convexB;
	47
	48	public ContinuousConvexCollision(ConvexShape convexA, ConvexShape convexB,
	49	ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
	50	{
	51	_simplexSolver = simplexSolver;
	52	_penetrationDepthSolver = penetrationDepthSolver;
	53	_convexA = convexA;
	54	_convexB = convexB;
	55	}
	56
	57	public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
	58	{
	59	_simplexSolver.Reset();
	60
	61	// compute linear and angular velocity for this interval, to interpolate
	62	Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3();
	63	TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA);
	64	TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB);
	65
	66	float boundingRadiusA = _convexA.GetAngularMotionDisc();
	67	float boundingRadiusB = _convexB.GetAngularMotionDisc();
	68
	69	float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA +
	70	angVelB.Length() * boundingRadiusB;
	71
	72	float radius = 0.001f;
	73
	74	float lambda = 0f;
	75	Vector3 v = new Vector3(1f, 0f, 0f);
	76
	77	int maxIter = MaxIterations;
	78
	79	Vector3 n = new Vector3();
	80	bool hasResult = false;
	81	Vector3 c;
	82
	83	float lastLambda = lambda;
	84	//float epsilon = 0.001f;
	85
	86	int numIter = 0;
	87	//first solution, using GJK
	88
	89
	90	Matrix identityTrans = Matrix.Identity;
	91
	92	SphereShape raySphere = new SphereShape(0f);
	93	raySphere.Margin=0f;
	94
	95
	96	//result.drawCoordSystem(sphereTr);
	97
	98	PointCollector pointCollector1 = new PointCollector();
	99
	100	GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
	101	GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
	102
	103	//we don't use margins during CCD
	104	gjk.setIgnoreMargin(true);
	105
	106	input.TransformA = fromA;
	107	input.TransformB = fromB;
	108
	109	DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
	110	gjk.GetClosestPoints(input, r, null);
	111
	112	hasResult = pointCollector1.HasResult;
	113	c = pointCollector1.PointInWorld;
	114
	115	if (hasResult)
	116	{
	117	float dist;
	118	dist = pointCollector1.Distance;
	119	n = pointCollector1.NormalOnBInWorld;
	120
	121	//not close enough
	122	while (dist > radius)
	123	{
	124	numIter++;
	125	if (numIter > maxIter)
	126	return false; //todo: report a failure
	127
	128	float dLambda = 0f;
	129
	130	//calculate safe moving fraction from distance / (linear+rotational velocity)
	131
	132	//float clippedDist = GEN_min(angularConservativeRadius,dist);
	133	//float clippedDist = dist;
	134
	135	float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n);
	136
	137	dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
	138
	139	lambda = lambda + dLambda;
	140
	141	if (lambda > 1f) return false;
	142	if (lambda < 0f) return false;
	143
	144	//todo: next check with relative epsilon
	145	if (lambda <= lastLambda)
	146	break;
	147	lastLambda = lambda;
	148
	149
	150	//interpolate to next lambda
	151	Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans;
	152
	153	TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA);
	154	TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB);
	155
	156	relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA);
	157
	158	result.DebugDraw(lambda);
	159
	160	PointCollector pointCollector = new PointCollector();
	161	gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
	162	input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
	163	input.TransformA = interpolatedTransA;
	164	input.TransformB = interpolatedTransB;
	165
	166	// !!!!!!!!!!
	167	r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
	168	gjk.GetClosestPoints(input, r, null);
	169
	170	if (pointCollector.HasResult)
	171	{
	172	if (pointCollector.Distance < 0f)
	173	{
	174	//degenerate ?!
	175	result.Fraction = lastLambda;
	176	result.Normal = n;
	177	return true;
	178	}
	179	c = pointCollector.PointInWorld;
	180
	181	dist = pointCollector.Distance;
	182	}
	183	else
	184	{
	185	//??
	186	return false;
	187	}
	188
	189	}
	190
	191	result.Fraction = lambda;
	192	result.Normal = n;
	193	return true;
	194	}
	195
	196	return false;
	197	}
	198	}
	199	}