aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs352
1 files changed, 176 insertions, 176 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs
index 4ee9599..ea7d335 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs
@@ -1,176 +1,176 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// GjkConvexCast performs a raycast on a convex object using support mapping. 30 /// GjkConvexCast performs a raycast on a convex object using support mapping.
31 /// </summary> 31 /// </summary>
32 public class GjkConvexCast : IConvexCast 32 public class GjkConvexCast : IConvexCast
33 { 33 {
34 private VoronoiSimplexSolver _simplexSolver; 34 private VoronoiSimplexSolver _simplexSolver;
35 private ConvexShape _convexA, _convexB; 35 private ConvexShape _convexA, _convexB;
36 36
37 public GjkConvexCast(ConvexShape convexShapeA, ConvexShape convexShapeB, VoronoiSimplexSolver solver) 37 public GjkConvexCast(ConvexShape convexShapeA, ConvexShape convexShapeB, VoronoiSimplexSolver solver)
38 { 38 {
39 _simplexSolver = solver; 39 _simplexSolver = solver;
40 40
41 _convexA = convexShapeA; 41 _convexA = convexShapeA;
42 _convexB = convexShapeB; 42 _convexB = convexShapeB;
43 } 43 }
44 44
45 #region IConvexCast Members 45 #region IConvexCast Members
46 46
47 /// <summary> 47 /// <summary>
48 /// cast a convex against another convex object 48 /// cast a convex against another convex object
49 /// </summary> 49 /// </summary>
50 /// <param name="fromA"></param> 50 /// <param name="fromA"></param>
51 /// <param name="toA"></param> 51 /// <param name="toA"></param>
52 /// <param name="fromB"></param> 52 /// <param name="fromB"></param>
53 /// <param name="toB"></param> 53 /// <param name="toB"></param>
54 /// <param name="result"></param> 54 /// <param name="result"></param>
55 /// <returns></returns> 55 /// <returns></returns>
56 public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) 56 public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
57 { 57 {
58 MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB); 58 MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB);
59 59
60 Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB; 60 Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
61 Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB; 61 Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB;
62 62
63 Matrix transformA = fromA; 63 Matrix transformA = fromA;
64 Matrix transformB = fromB; 64 Matrix transformB = fromB;
65 65
66 transformA.Translation = new Vector3(0, 0, 0); 66 transformA.Translation = new Vector3(0, 0, 0);
67 transformB.Translation = new Vector3(0, 0, 0); 67 transformB.Translation = new Vector3(0, 0, 0);
68 68
69 combined.TransformA = transformA; 69 combined.TransformA = transformA;
70 combined.TransformB = transformB; 70 combined.TransformB = transformB;
71 71
72 float radius = 0.01f; 72 float radius = 0.01f;
73 float lambda = 0; 73 float lambda = 0;
74 74
75 Vector3 s = rayFromLocalA.Translation; 75 Vector3 s = rayFromLocalA.Translation;
76 Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation; 76 Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation;
77 Vector3 x = s; 77 Vector3 x = s;
78 Vector3 n = new Vector3(); 78 Vector3 n = new Vector3();
79 Vector3 c = new Vector3(); 79 Vector3 c = new Vector3();
80 80
81 bool hasResult = false; 81 bool hasResult = false;
82 float lastLambda = lambda; 82 float lastLambda = lambda;
83 83
84 IConvexPenetrationDepthSolver penSolver = null; 84 IConvexPenetrationDepthSolver penSolver = null;
85 Matrix identityTransform = Matrix.Identity; 85 Matrix identityTransform = Matrix.Identity;
86 86
87 SphereShape raySphere = new SphereShape(0.0f); 87 SphereShape raySphere = new SphereShape(0.0f);
88 raySphere.Margin=0.0f; 88 raySphere.Margin=0.0f;
89 89
90 Matrix sphereTransform = Matrix.Identity; 90 Matrix sphereTransform = Matrix.Identity;
91 sphereTransform.Translation = rayFromLocalA.Translation; 91 sphereTransform.Translation = rayFromLocalA.Translation;
92 92
93 result.DrawCoordSystem(sphereTransform); 93 result.DrawCoordSystem(sphereTransform);
94 94
95 { 95 {
96 PointCollector pointCollector = new PointCollector(); 96 PointCollector pointCollector = new PointCollector();
97 GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver); 97 GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
98 98
99 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); 99 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
100 input.TransformA = sphereTransform; 100 input.TransformA = sphereTransform;
101 input.TransformB = identityTransform; 101 input.TransformB = identityTransform;
102 102
103 gjk.GetClosestPoints(input, pointCollector, null); 103 gjk.GetClosestPoints(input, pointCollector, null);
104 104
105 hasResult = pointCollector.HasResult; 105 hasResult = pointCollector.HasResult;
106 106
107 c = pointCollector.PointInWorld; 107 c = pointCollector.PointInWorld;
108 n = pointCollector.NormalOnBInWorld; 108 n = pointCollector.NormalOnBInWorld;
109 } 109 }
110 110
111 if (hasResult) 111 if (hasResult)
112 { 112 {
113 float dist = (c - x).Length(); 113 float dist = (c - x).Length();
114 114
115 if (dist < radius) 115 if (dist < radius)
116 { 116 {
117 lastLambda = 1.0f; 117 lastLambda = 1.0f;
118 } 118 }
119 119
120 while (dist > radius) 120 while (dist > radius)
121 { 121 {
122 n = x - c; 122 n = x - c;
123 float dot = Vector3.Dot(n, r); 123 float dot = Vector3.Dot(n, r);
124 124
125 if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false; 125 if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false;
126 126
127 lambda = lambda - Vector3.Distance(n, n) / dot; 127 lambda = lambda - Vector3.Distance(n, n) / dot;
128 if (lambda <= lastLambda) break; 128 if (lambda <= lastLambda) break;
129 129
130 lastLambda = lambda; 130 lastLambda = lambda;
131 131
132 x = s + lambda * r; 132 x = s + lambda * r;
133 133
134 sphereTransform.Translation = x; 134 sphereTransform.Translation = x;
135 result.DrawCoordSystem(sphereTransform); 135 result.DrawCoordSystem(sphereTransform);
136 PointCollector pointCollector = new PointCollector(); 136 PointCollector pointCollector = new PointCollector();
137 137
138 GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver); 138 GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
139 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); 139 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
140 input.TransformA = sphereTransform; 140 input.TransformA = sphereTransform;
141 input.TransformB = identityTransform; 141 input.TransformB = identityTransform;
142 142
143 gjk.GetClosestPoints(input, pointCollector, null); 143 gjk.GetClosestPoints(input, pointCollector, null);
144 144
145 if (pointCollector.HasResult) 145 if (pointCollector.HasResult)
146 { 146 {
147 if (pointCollector.Distance < 0.0f) 147 if (pointCollector.Distance < 0.0f)
148 { 148 {
149 result.Fraction = lastLambda; 149 result.Fraction = lastLambda;
150 result.Normal = n; 150 result.Normal = n;
151 return true; 151 return true;
152 } 152 }
153 153
154 c = pointCollector.PointInWorld; 154 c = pointCollector.PointInWorld;
155 dist = (c - x).Length(); 155 dist = (c - x).Length();
156 } 156 }
157 else 157 else
158 { 158 {
159 return false; 159 return false;
160 } 160 }
161 } 161 }
162 162
163 if (lastLambda < 1.0f) 163 if (lastLambda < 1.0f)
164 { 164 {
165 result.Fraction = lastLambda; 165 result.Fraction = lastLambda;
166 result.Normal = n; 166 result.Normal = n;
167 return true; 167 return true;
168 } 168 }
169 } 169 }
170 170
171 return false; 171 return false;
172 } 172 }
173 173
174 #endregion 174 #endregion
175 } 175 }
176} 176}
generated by cgit v1.1 at 2024-07-16 20:35:44 +0000