1 files changed, 176 insertions, 0 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs
new file mode 100644
index 0000000..4ee9599
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs
@@ -0,0 +1,176 @@
+/*
+  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>
+    /// GjkConvexCast performs a raycast on a convex object using support mapping.
+    /// </summary>
+        public class GjkConvexCast : IConvexCast
+        {
+                private VoronoiSimplexSolver _simplexSolver;
+                private ConvexShape _convexA, _convexB;
+                public GjkConvexCast(ConvexShape convexShapeA, ConvexShape convexShapeB, VoronoiSimplexSolver solver)
+                {
+                        _simplexSolver = solver;
+                        _convexA = convexShapeA;
+                        _convexB = convexShapeB;
+                }
+                #region IConvexCast Members
+        /// <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;
+                }
+                #endregion
+        }
+}

diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs new file mode 100644 index 0000000..4ee9599 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs
@@ -0,0 +1,176 @@
	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	/// GjkConvexCast performs a raycast on a convex object using support mapping.
	31	/// </summary>
	32	public class GjkConvexCast : IConvexCast
	33	{
	34	private VoronoiSimplexSolver _simplexSolver;
	35	private ConvexShape _convexA, _convexB;
	36
	37	public GjkConvexCast(ConvexShape convexShapeA, ConvexShape convexShapeB, VoronoiSimplexSolver solver)
	38	{
	39	_simplexSolver = solver;
	40
	41	_convexA = convexShapeA;
	42	_convexB = convexShapeB;
	43	}
	44
	45	#region IConvexCast Members
	46
	47	/// <summary>
	48	/// cast a convex against another convex object
	49	/// </summary>
	50	/// <param name="fromA"></param>
	51	/// <param name="toA"></param>
	52	/// <param name="fromB"></param>
	53	/// <param name="toB"></param>
	54	/// <param name="result"></param>
	55	/// <returns></returns>
	56	public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
	57	{
	58	MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB);
	59
	60	Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
	61	Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB;
	62
	63	Matrix transformA = fromA;
	64	Matrix transformB = fromB;
	65
	66	transformA.Translation = new Vector3(0, 0, 0);
	67	transformB.Translation = new Vector3(0, 0, 0);
	68
	69	combined.TransformA = transformA;
	70	combined.TransformB = transformB;
	71
	72	float radius = 0.01f;
	73	float lambda = 0;
	74
	75	Vector3 s = rayFromLocalA.Translation;
	76	Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation;
	77	Vector3 x = s;
	78	Vector3 n = new Vector3();
	79	Vector3 c = new Vector3();
	80
	81	bool hasResult = false;
	82	float lastLambda = lambda;
	83
	84	IConvexPenetrationDepthSolver penSolver = null;
	85	Matrix identityTransform = Matrix.Identity;
	86
	87	SphereShape raySphere = new SphereShape(0.0f);
	88	raySphere.Margin=0.0f;
	89
	90	Matrix sphereTransform = Matrix.Identity;
	91	sphereTransform.Translation = rayFromLocalA.Translation;
	92
	93	result.DrawCoordSystem(sphereTransform);
	94
	95	{
	96	PointCollector pointCollector = new PointCollector();
	97	GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
	98
	99	GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
	100	input.TransformA = sphereTransform;
	101	input.TransformB = identityTransform;
	102
	103	gjk.GetClosestPoints(input, pointCollector, null);
	104
	105	hasResult = pointCollector.HasResult;
	106
	107	c = pointCollector.PointInWorld;
	108	n = pointCollector.NormalOnBInWorld;
	109	}
	110
	111	if (hasResult)
	112	{
	113	float dist = (c - x).Length();
	114
	115	if (dist < radius)
	116	{
	117	lastLambda = 1.0f;
	118	}
	119
	120	while (dist > radius)
	121	{
	122	n = x - c;
	123	float dot = Vector3.Dot(n, r);
	124
	125	if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false;
	126
	127	lambda = lambda - Vector3.Distance(n, n) / dot;
	128	if (lambda <= lastLambda) break;
	129
	130	lastLambda = lambda;
	131
	132	x = s + lambda * r;
	133
	134	sphereTransform.Translation = x;
	135	result.DrawCoordSystem(sphereTransform);
	136	PointCollector pointCollector = new PointCollector();
	137
	138	GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
	139	GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
	140	input.TransformA = sphereTransform;
	141	input.TransformB = identityTransform;
	142
	143	gjk.GetClosestPoints(input, pointCollector, null);
	144
	145	if (pointCollector.HasResult)
	146	{
	147	if (pointCollector.Distance < 0.0f)
	148	{
	149	result.Fraction = lastLambda;
	150	result.Normal = n;
	151	return true;
	152	}
	153
	154	c = pointCollector.PointInWorld;
	155	dist = (c - x).Length();
	156	}
	157	else
	158	{
	159	return false;
	160	}
	161	}
	162
	163	if (lastLambda < 1.0f)
	164	{
	165	result.Fraction = lastLambda;
	166	result.Normal = n;
	167	return true;
	168	}
	169	}
	170
	171	return false;
	172	}
	173
	174	#endregion
	175	}
	176	}