diff options
Diffstat (limited to '')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs | 398 |
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 @@ | |||
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 | ||
22 | using System; | 22 | using System; |
23 | using System.Collections.Generic; | 23 | using System.Collections.Generic; |
24 | using System.Text; | 24 | using System.Text; |
25 | using MonoXnaCompactMaths; | 25 | using MonoXnaCompactMaths; |
26 | 26 | ||
27 | namespace XnaDevRu.BulletX | 27 | namespace XnaDevRu.BulletX |
28 | { | 28 | { |
29 | /// <summary> | 29 | /// <summary> |
30 | /// ContinuousConvexCollision implements angular and linear time of impact for convex objects. | 30 | /// ContinuousConvexCollision implements angular and linear time of impact for convex objects. |
31 | /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis). | 31 | /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis). |
32 | /// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent. | 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 | 33 | /// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops |
34 | /// </summary> | 34 | /// </summary> |
35 | public class ContinuousConvexCollision : IConvexCast | 35 | public class ContinuousConvexCollision : IConvexCast |
36 | { | 36 | { |
37 | /// <summary> | 37 | /// <summary> |
38 | /// This maximum should not be necessary. It allows for untested/degenerate cases in production code. | 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. | 39 | /// You don't want your game ever to lock-up. |
40 | /// </summary> | 40 | /// </summary> |
41 | private const int MaxIterations = 1000; | 41 | private const int MaxIterations = 1000; |
42 | 42 | ||
43 | private ISimplexSolver _simplexSolver; | 43 | private ISimplexSolver _simplexSolver; |
44 | private IConvexPenetrationDepthSolver _penetrationDepthSolver; | 44 | private IConvexPenetrationDepthSolver _penetrationDepthSolver; |
45 | private ConvexShape _convexA; | 45 | private ConvexShape _convexA; |
46 | private ConvexShape _convexB; | 46 | private ConvexShape _convexB; |
47 | 47 | ||
48 | public ContinuousConvexCollision(ConvexShape convexA, ConvexShape convexB, | 48 | public ContinuousConvexCollision(ConvexShape convexA, ConvexShape convexB, |
49 | ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) | 49 | ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) |
50 | { | 50 | { |
51 | _simplexSolver = simplexSolver; | 51 | _simplexSolver = simplexSolver; |
52 | _penetrationDepthSolver = penetrationDepthSolver; | 52 | _penetrationDepthSolver = penetrationDepthSolver; |
53 | _convexA = convexA; | 53 | _convexA = convexA; |
54 | _convexB = convexB; | 54 | _convexB = convexB; |
55 | } | 55 | } |
56 | 56 | ||
57 | public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) | 57 | public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) |
58 | { | 58 | { |
59 | _simplexSolver.Reset(); | 59 | _simplexSolver.Reset(); |
60 | 60 | ||
61 | // compute linear and angular velocity for this interval, to interpolate | 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(); | 62 | Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3(); |
63 | TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA); | 63 | TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA); |
64 | TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB); | 64 | TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB); |
65 | 65 | ||
66 | float boundingRadiusA = _convexA.GetAngularMotionDisc(); | 66 | float boundingRadiusA = _convexA.GetAngularMotionDisc(); |
67 | float boundingRadiusB = _convexB.GetAngularMotionDisc(); | 67 | float boundingRadiusB = _convexB.GetAngularMotionDisc(); |
68 | 68 | ||
69 | float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA + | 69 | float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA + |
70 | angVelB.Length() * boundingRadiusB; | 70 | angVelB.Length() * boundingRadiusB; |
71 | 71 | ||
72 | float radius = 0.001f; | 72 | float radius = 0.001f; |
73 | 73 | ||
74 | float lambda = 0f; | 74 | float lambda = 0f; |
75 | Vector3 v = new Vector3(1f, 0f, 0f); | 75 | Vector3 v = new Vector3(1f, 0f, 0f); |
76 | 76 | ||
77 | int maxIter = MaxIterations; | 77 | int maxIter = MaxIterations; |
78 | 78 | ||
79 | Vector3 n = new Vector3(); | 79 | Vector3 n = new Vector3(); |
80 | bool hasResult = false; | 80 | bool hasResult = false; |
81 | Vector3 c; | 81 | Vector3 c; |
82 | 82 | ||
83 | float lastLambda = lambda; | 83 | float lastLambda = lambda; |
84 | //float epsilon = 0.001f; | 84 | //float epsilon = 0.001f; |
85 | 85 | ||
86 | int numIter = 0; | 86 | int numIter = 0; |
87 | //first solution, using GJK | 87 | //first solution, using GJK |
88 | 88 | ||
89 | 89 | ||
90 | Matrix identityTrans = Matrix.Identity; | 90 | Matrix identityTrans = Matrix.Identity; |
91 | 91 | ||
92 | SphereShape raySphere = new SphereShape(0f); | 92 | SphereShape raySphere = new SphereShape(0f); |
93 | raySphere.Margin=0f; | 93 | raySphere.Margin=0f; |
94 | 94 | ||
95 | 95 | ||
96 | //result.drawCoordSystem(sphereTr); | 96 | //result.drawCoordSystem(sphereTr); |
97 | 97 | ||
98 | PointCollector pointCollector1 = new PointCollector(); | 98 | PointCollector pointCollector1 = new PointCollector(); |
99 | 99 | ||
100 | GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver); | 100 | GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver); |
101 | GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | 101 | GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); |
102 | 102 | ||
103 | //we don't use margins during CCD | 103 | //we don't use margins during CCD |
104 | gjk.setIgnoreMargin(true); | 104 | gjk.setIgnoreMargin(true); |
105 | 105 | ||
106 | input.TransformA = fromA; | 106 | input.TransformA = fromA; |
107 | input.TransformB = fromB; | 107 | input.TransformB = fromB; |
108 | 108 | ||
109 | DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1; | 109 | DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1; |
110 | gjk.GetClosestPoints(input, r, null); | 110 | gjk.GetClosestPoints(input, r, null); |
111 | 111 | ||
112 | hasResult = pointCollector1.HasResult; | 112 | hasResult = pointCollector1.HasResult; |
113 | c = pointCollector1.PointInWorld; | 113 | c = pointCollector1.PointInWorld; |
114 | 114 | ||
115 | if (hasResult) | 115 | if (hasResult) |
116 | { | 116 | { |
117 | float dist; | 117 | float dist; |
118 | dist = pointCollector1.Distance; | 118 | dist = pointCollector1.Distance; |
119 | n = pointCollector1.NormalOnBInWorld; | 119 | n = pointCollector1.NormalOnBInWorld; |
120 | 120 | ||
121 | //not close enough | 121 | //not close enough |
122 | while (dist > radius) | 122 | while (dist > radius) |
123 | { | 123 | { |
124 | numIter++; | 124 | numIter++; |
125 | if (numIter > maxIter) | 125 | if (numIter > maxIter) |
126 | return false; //todo: report a failure | 126 | return false; //todo: report a failure |
127 | 127 | ||
128 | float dLambda = 0f; | 128 | float dLambda = 0f; |
129 | 129 | ||
130 | //calculate safe moving fraction from distance / (linear+rotational velocity) | 130 | //calculate safe moving fraction from distance / (linear+rotational velocity) |
131 | 131 | ||
132 | //float clippedDist = GEN_min(angularConservativeRadius,dist); | 132 | //float clippedDist = GEN_min(angularConservativeRadius,dist); |
133 | //float clippedDist = dist; | 133 | //float clippedDist = dist; |
134 | 134 | ||
135 | float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n); | 135 | float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n); |
136 | 136 | ||
137 | dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity); | 137 | dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity); |
138 | 138 | ||
139 | lambda = lambda + dLambda; | 139 | lambda = lambda + dLambda; |
140 | 140 | ||
141 | if (lambda > 1f) return false; | 141 | if (lambda > 1f) return false; |
142 | if (lambda < 0f) return false; | 142 | if (lambda < 0f) return false; |
143 | 143 | ||
144 | //todo: next check with relative epsilon | 144 | //todo: next check with relative epsilon |
145 | if (lambda <= lastLambda) | 145 | if (lambda <= lastLambda) |
146 | break; | 146 | break; |
147 | lastLambda = lambda; | 147 | lastLambda = lambda; |
148 | 148 | ||
149 | 149 | ||
150 | //interpolate to next lambda | 150 | //interpolate to next lambda |
151 | Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans; | 151 | Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans; |
152 | 152 | ||
153 | TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA); | 153 | TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA); |
154 | TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB); | 154 | TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB); |
155 | 155 | ||
156 | relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA); | 156 | relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA); |
157 | 157 | ||
158 | result.DebugDraw(lambda); | 158 | result.DebugDraw(lambda); |
159 | 159 | ||
160 | PointCollector pointCollector = new PointCollector(); | 160 | PointCollector pointCollector = new PointCollector(); |
161 | gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver); | 161 | gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver); |
162 | input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | 162 | input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); |
163 | input.TransformA = interpolatedTransA; | 163 | input.TransformA = interpolatedTransA; |
164 | input.TransformB = interpolatedTransB; | 164 | input.TransformB = interpolatedTransB; |
165 | 165 | ||
166 | // !!!!!!!!!! | 166 | // !!!!!!!!!! |
167 | r = (DiscreteCollisionDetectorInterface.Result)pointCollector1; | 167 | r = (DiscreteCollisionDetectorInterface.Result)pointCollector1; |
168 | gjk.GetClosestPoints(input, r, null); | 168 | gjk.GetClosestPoints(input, r, null); |
169 | 169 | ||
170 | if (pointCollector.HasResult) | 170 | if (pointCollector.HasResult) |
171 | { | 171 | { |
172 | if (pointCollector.Distance < 0f) | 172 | if (pointCollector.Distance < 0f) |
173 | { | 173 | { |
174 | //degenerate ?! | 174 | //degenerate ?! |
175 | result.Fraction = lastLambda; | 175 | result.Fraction = lastLambda; |
176 | result.Normal = n; | 176 | result.Normal = n; |
177 | return true; | 177 | return true; |
178 | } | 178 | } |
179 | c = pointCollector.PointInWorld; | 179 | c = pointCollector.PointInWorld; |
180 | 180 | ||
181 | dist = pointCollector.Distance; | 181 | dist = pointCollector.Distance; |
182 | } | 182 | } |
183 | else | 183 | else |
184 | { | 184 | { |
185 | //?? | 185 | //?? |
186 | return false; | 186 | return false; |
187 | } | 187 | } |
188 | 188 | ||
189 | } | 189 | } |
190 | 190 | ||
191 | result.Fraction = lambda; | 191 | result.Fraction = lambda; |
192 | result.Normal = n; | 192 | result.Normal = n; |
193 | return true; | 193 | return true; |
194 | } | 194 | } |
195 | 195 | ||
196 | return false; | 196 | return false; |
197 | } | 197 | } |
198 | } | 198 | } |
199 | } | 199 | } |