aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs284
1 files changed, 142 insertions, 142 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs
index 6d320c4..5c92a0c 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs
@@ -1,142 +1,142 @@
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 /// SubsimplexConvexCast implements Gino van den Bergens' paper 30 /// SubsimplexConvexCast implements Gino van den Bergens' paper
31 /// "Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection" 31 /// "Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"
32 /// GJK based Ray Cast, optimized version 32 /// GJK based Ray Cast, optimized version
33 /// Objects should not start in overlap, otherwise results are not defined. 33 /// Objects should not start in overlap, otherwise results are not defined.
34 /// </summary> 34 /// </summary>
35 public class SubsimplexConvexCast : IConvexCast 35 public class SubsimplexConvexCast : IConvexCast
36 { 36 {
37 private ISimplexSolver _simplexSolver; 37 private ISimplexSolver _simplexSolver;
38 private ConvexShape _convexA; 38 private ConvexShape _convexA;
39 private ConvexShape _convexB; 39 private ConvexShape _convexB;
40 40
41 /// <summary> 41 /// <summary>
42 /// Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. 42 /// Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
43 /// See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565 43 /// See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
44 /// </summary> 44 /// </summary>
45 private const int MaxIterations = 32; 45 private const int MaxIterations = 32;
46 46
47 public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolver simplexSolver) 47 public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolver simplexSolver)
48 { 48 {
49 _simplexSolver = simplexSolver; 49 _simplexSolver = simplexSolver;
50 _convexA = shapeA; 50 _convexA = shapeA;
51 _convexB = shapeB; 51 _convexB = shapeB;
52 } 52 }
53 53
54 #region IConvexCast Members 54 #region IConvexCast Members
55 /// <summary> 55 /// <summary>
56 /// SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects. 56 /// SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
57 /// Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using GjkPairDetector. 57 /// Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using GjkPairDetector.
58 /// </summary> 58 /// </summary>
59 /// <param name="fromA"></param> 59 /// <param name="fromA"></param>
60 /// <param name="toA"></param> 60 /// <param name="toA"></param>
61 /// <param name="fromB"></param> 61 /// <param name="fromB"></param>
62 /// <param name="toB"></param> 62 /// <param name="toB"></param>
63 /// <param name="result"></param> 63 /// <param name="result"></param>
64 /// <returns></returns> 64 /// <returns></returns>
65 public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) 65 public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
66 { 66 {
67 MinkowskiSumShape convex = new MinkowskiSumShape(_convexA, _convexB); 67 MinkowskiSumShape convex = new MinkowskiSumShape(_convexA, _convexB);
68 68
69 Matrix rayFromLocalA; 69 Matrix rayFromLocalA;
70 Matrix rayToLocalA; 70 Matrix rayToLocalA;
71 71
72 rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB; 72 rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
73 rayToLocalA = MathHelper.InvertMatrix(toA) * toB; 73 rayToLocalA = MathHelper.InvertMatrix(toA) * toB;
74 74
75 _simplexSolver.Reset(); 75 _simplexSolver.Reset();
76 76
77 convex.TransformB = rayFromLocalA; 77 convex.TransformB = rayFromLocalA;
78 78
79 float lambda = 0; 79 float lambda = 0;
80 //todo: need to verify this: 80 //todo: need to verify this:
81 //because of minkowski difference, we need the inverse direction 81 //because of minkowski difference, we need the inverse direction
82 82
83 Vector3 s = -rayFromLocalA.Translation; 83 Vector3 s = -rayFromLocalA.Translation;
84 Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation); 84 Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation);
85 Vector3 x = s; 85 Vector3 x = s;
86 Vector3 v; 86 Vector3 v;
87 Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(r); 87 Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(r);
88 88
89 v = x - arbitraryPoint; 89 v = x - arbitraryPoint;
90 90
91 int maxIter = MaxIterations; 91 int maxIter = MaxIterations;
92 92
93 Vector3 n = new Vector3(); 93 Vector3 n = new Vector3();
94 float lastLambda = lambda; 94 float lastLambda = lambda;
95 95
96 float dist2 = v.LengthSquared(); 96 float dist2 = v.LengthSquared();
97 float epsilon = 0.0001f; 97 float epsilon = 0.0001f;
98 98
99 Vector3 w, p; 99 Vector3 w, p;
100 float VdotR; 100 float VdotR;
101 101
102 while ((dist2 > epsilon) && (maxIter-- != 0)) 102 while ((dist2 > epsilon) && (maxIter-- != 0))
103 { 103 {
104 p = convex.LocalGetSupportingVertex(v); 104 p = convex.LocalGetSupportingVertex(v);
105 w = x - p; 105 w = x - p;
106 106
107 float VdotW = Vector3.Dot(v, w); 107 float VdotW = Vector3.Dot(v, w);
108 108
109 if (VdotW > 0) 109 if (VdotW > 0)
110 { 110 {
111 VdotR = Vector3.Dot(v, r); 111 VdotR = Vector3.Dot(v, r);
112 112
113 if (VdotR >= -(MathHelper.Epsilon * MathHelper.Epsilon)) 113 if (VdotR >= -(MathHelper.Epsilon * MathHelper.Epsilon))
114 return false; 114 return false;
115 else 115 else
116 { 116 {
117 lambda = lambda - VdotW / VdotR; 117 lambda = lambda - VdotW / VdotR;
118 x = s + lambda * r; 118 x = s + lambda * r;
119 _simplexSolver.Reset(); 119 _simplexSolver.Reset();
120 //check next line 120 //check next line
121 w = x - p; 121 w = x - p;
122 lastLambda = lambda; 122 lastLambda = lambda;
123 n = v; 123 n = v;
124 } 124 }
125 } 125 }
126 _simplexSolver.AddVertex(w, x, p); 126 _simplexSolver.AddVertex(w, x, p);
127 if (_simplexSolver.Closest(out v)) 127 if (_simplexSolver.Closest(out v))
128 { 128 {
129 dist2 = v.LengthSquared(); 129 dist2 = v.LengthSquared();
130 } 130 }
131 else 131 else
132 { 132 {
133 dist2 = 0f; 133 dist2 = 0f;
134 } 134 }
135 } 135 }
136 result.Fraction = lambda; 136 result.Fraction = lambda;
137 result.Normal = n; 137 result.Normal = n;
138 return true; 138 return true;
139 } 139 }
140 #endregion 140 #endregion
141 } 141 }
142} 142}
generated by cgit v1.1 at 2024-07-16 19:23:40 +0000