diff options
author | Sean Dague | 2007-07-16 15:40:11 +0000 |
---|---|---|
committer | Sean Dague | 2007-07-16 15:40:11 +0000 |
commit | 2a3c79df83e800d5dfe75a1a3b140ed81da2b1d6 (patch) | |
tree | e3f80ad51736cf17e856547b1bcf956010927434 /libraries/ModifiedBulletX/ModifiedBulletX | |
parent | *Trunk compiles now (diff) | |
download | opensim-SC_OLD-2a3c79df83e800d5dfe75a1a3b140ed81da2b1d6.zip opensim-SC_OLD-2a3c79df83e800d5dfe75a1a3b140ed81da2b1d6.tar.gz opensim-SC_OLD-2a3c79df83e800d5dfe75a1a3b140ed81da2b1d6.tar.bz2 opensim-SC_OLD-2a3c79df83e800d5dfe75a1a3b140ed81da2b1d6.tar.xz |
changed to native line ending encoding
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX')
114 files changed, 18972 insertions, 18972 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs b/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs index d0d5845..a4cbfdd 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs | |||
@@ -1,68 +1,68 @@ | |||
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 System.Diagnostics; | 25 | using System.Diagnostics; |
26 | 26 | ||
27 | namespace XnaDevRu.BulletX | 27 | namespace XnaDevRu.BulletX |
28 | { | 28 | { |
29 | internal static class BulletDebug | 29 | internal static class BulletDebug |
30 | { | 30 | { |
31 | [Conditional("DEBUG")] | 31 | [Conditional("DEBUG")] |
32 | public static void Assert(Boolean condition) | 32 | public static void Assert(Boolean condition) |
33 | { | 33 | { |
34 | //if (!condition) | 34 | //if (!condition) |
35 | //{ | 35 | //{ |
36 | // Throw("No info available"); | 36 | // Throw("No info available"); |
37 | //} | 37 | //} |
38 | Debug.Assert(condition); | 38 | Debug.Assert(condition); |
39 | } | 39 | } |
40 | 40 | ||
41 | [Conditional("DEBUG")] | 41 | [Conditional("DEBUG")] |
42 | public static void Assert(Boolean condition, String message) | 42 | public static void Assert(Boolean condition, String message) |
43 | { | 43 | { |
44 | //if (!condition) | 44 | //if (!condition) |
45 | //{ | 45 | //{ |
46 | // Throw(message); | 46 | // Throw(message); |
47 | //} | 47 | //} |
48 | Debug.Assert(condition, message); | 48 | Debug.Assert(condition, message); |
49 | } | 49 | } |
50 | 50 | ||
51 | [Conditional("DEBUG")] | 51 | [Conditional("DEBUG")] |
52 | public static void Assert(Boolean condition, String message, String detailMessage) | 52 | public static void Assert(Boolean condition, String message, String detailMessage) |
53 | { | 53 | { |
54 | //if (!condition) | 54 | //if (!condition) |
55 | //{ | 55 | //{ |
56 | // Throw(message); | 56 | // Throw(message); |
57 | //} | 57 | //} |
58 | Debug.Assert(condition, message, detailMessage); | 58 | Debug.Assert(condition, message, detailMessage); |
59 | } | 59 | } |
60 | 60 | ||
61 | private static void Throw(String message) | 61 | private static void Throw(String message) |
62 | { | 62 | { |
63 | String msg = String.Format("Assertion Error: {0}", message); | 63 | String msg = String.Format("Assertion Error: {0}", message); |
64 | 64 | ||
65 | throw new BulletException(msg); | 65 | throw new BulletException(msg); |
66 | } | 66 | } |
67 | } | 67 | } |
68 | } | 68 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs index 12692ea..168d947 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs | |||
@@ -1,623 +1,623 @@ | |||
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 | public class AxisSweep3: OverlappingPairCache | 29 | public class AxisSweep3: OverlappingPairCache |
30 | { | 30 | { |
31 | Vector3 _worldAabbMin; | 31 | Vector3 _worldAabbMin; |
32 | Vector3 _worldAabbMax; | 32 | Vector3 _worldAabbMax; |
33 | 33 | ||
34 | Vector3 _quantize; | 34 | Vector3 _quantize; |
35 | 35 | ||
36 | int _numHandles; | 36 | int _numHandles; |
37 | int _maxHandles; | 37 | int _maxHandles; |
38 | 38 | ||
39 | Handle[] _handles; | 39 | Handle[] _handles; |
40 | Edge[][] _edges = new Edge[3][]; | 40 | Edge[][] _edges = new Edge[3][]; |
41 | 41 | ||
42 | ushort _firstFreeHandle; | 42 | ushort _firstFreeHandle; |
43 | 43 | ||
44 | int _invalidPair; | 44 | int _invalidPair; |
45 | 45 | ||
46 | public AxisSweep3(Vector3 worldAabbMin, Vector3 worldAabbMax, int maxHandles) | 46 | public AxisSweep3(Vector3 worldAabbMin, Vector3 worldAabbMax, int maxHandles) |
47 | : base() | 47 | : base() |
48 | { | 48 | { |
49 | BulletDebug.Assert(maxHandles > 1 && maxHandles < 32767); | 49 | BulletDebug.Assert(maxHandles > 1 && maxHandles < 32767); |
50 | 50 | ||
51 | // init bounds | 51 | // init bounds |
52 | _worldAabbMin = worldAabbMin; | 52 | _worldAabbMin = worldAabbMin; |
53 | _worldAabbMax = worldAabbMax; | 53 | _worldAabbMax = worldAabbMax; |
54 | 54 | ||
55 | Vector3 aabbSize = _worldAabbMax - _worldAabbMin; | 55 | Vector3 aabbSize = _worldAabbMax - _worldAabbMin; |
56 | _quantize = new Vector3(65535.0f, 65535.0f, 65535.0f) / aabbSize; | 56 | _quantize = new Vector3(65535.0f, 65535.0f, 65535.0f) / aabbSize; |
57 | 57 | ||
58 | // allocate handles buffer and put all handles on free list | 58 | // allocate handles buffer and put all handles on free list |
59 | _handles = new Handle[maxHandles]; | 59 | _handles = new Handle[maxHandles]; |
60 | for (int i = 0; i < maxHandles; i++) | 60 | for (int i = 0; i < maxHandles; i++) |
61 | _handles[i] = new Handle(); | 61 | _handles[i] = new Handle(); |
62 | _maxHandles = maxHandles; | 62 | _maxHandles = maxHandles; |
63 | _numHandles = 0; | 63 | _numHandles = 0; |
64 | 64 | ||
65 | // handle 0 is reserved as the null index, and is also used as the sentinel | 65 | // handle 0 is reserved as the null index, and is also used as the sentinel |
66 | _firstFreeHandle = 1; | 66 | _firstFreeHandle = 1; |
67 | { | 67 | { |
68 | for (int i = _firstFreeHandle; i < maxHandles; i++) | 68 | for (int i = _firstFreeHandle; i < maxHandles; i++) |
69 | { | 69 | { |
70 | _handles[i].NextFree = (ushort)(i + 1); | 70 | _handles[i].NextFree = (ushort)(i + 1); |
71 | } | 71 | } |
72 | _handles[maxHandles - 1].NextFree = 0; | 72 | _handles[maxHandles - 1].NextFree = 0; |
73 | } | 73 | } |
74 | 74 | ||
75 | { | 75 | { |
76 | // allocate edge buffers | 76 | // allocate edge buffers |
77 | for (int i = 0; i < 3; i++) | 77 | for (int i = 0; i < 3; i++) |
78 | { | 78 | { |
79 | _edges[i] = new Edge[maxHandles * 2]; | 79 | _edges[i] = new Edge[maxHandles * 2]; |
80 | for (int j = 0; j < maxHandles * 2; j++) | 80 | for (int j = 0; j < maxHandles * 2; j++) |
81 | { | 81 | { |
82 | _edges[i][j] = new Edge(); | 82 | _edges[i][j] = new Edge(); |
83 | } | 83 | } |
84 | } | 84 | } |
85 | } | 85 | } |
86 | //removed overlap management | 86 | //removed overlap management |
87 | 87 | ||
88 | // make boundary sentinels | 88 | // make boundary sentinels |
89 | 89 | ||
90 | _handles[0].ClientData = 0; | 90 | _handles[0].ClientData = 0; |
91 | 91 | ||
92 | for (int axis = 0; axis < 3; axis++) | 92 | for (int axis = 0; axis < 3; axis++) |
93 | { | 93 | { |
94 | _handles[0].MinEdges[axis] = 0; | 94 | _handles[0].MinEdges[axis] = 0; |
95 | _handles[0].MaxEdges[axis] = 1; | 95 | _handles[0].MaxEdges[axis] = 1; |
96 | 96 | ||
97 | _edges[axis][0].Position = 0; | 97 | _edges[axis][0].Position = 0; |
98 | _edges[axis][0].Handle = 0; | 98 | _edges[axis][0].Handle = 0; |
99 | _edges[axis][1].Position = 0xffff; | 99 | _edges[axis][1].Position = 0xffff; |
100 | _edges[axis][1].Handle = 0; | 100 | _edges[axis][1].Handle = 0; |
101 | } | 101 | } |
102 | } | 102 | } |
103 | 103 | ||
104 | public ushort AddHandle(Vector3 aabbMin, Vector3 aabbMax, object owner, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) | 104 | public ushort AddHandle(Vector3 aabbMin, Vector3 aabbMax, object owner, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) |
105 | { | 105 | { |
106 | ushort[] min = new ushort[3], max = new ushort[3]; | 106 | ushort[] min = new ushort[3], max = new ushort[3]; |
107 | Quantize(out min, aabbMin, 0); | 107 | Quantize(out min, aabbMin, 0); |
108 | Quantize(out max, aabbMax, 1); | 108 | Quantize(out max, aabbMax, 1); |
109 | 109 | ||
110 | ushort handle = AllocateHandle(); | 110 | ushort handle = AllocateHandle(); |
111 | Handle oHandle = GetHandle(handle); | 111 | Handle oHandle = GetHandle(handle); |
112 | 112 | ||
113 | oHandle.HandleID = handle; | 113 | oHandle.HandleID = handle; |
114 | oHandle.ClientData = owner; | 114 | oHandle.ClientData = owner; |
115 | oHandle.CollisionFilterGroup = collisionFilterGroup; | 115 | oHandle.CollisionFilterGroup = collisionFilterGroup; |
116 | oHandle.CollisionFilterMask = collisionFilterMask; | 116 | oHandle.CollisionFilterMask = collisionFilterMask; |
117 | 117 | ||
118 | int limit = _numHandles * 2; | 118 | int limit = _numHandles * 2; |
119 | 119 | ||
120 | // (Gluk ) | 120 | // (Gluk ) |
121 | // ( Inside ) | 121 | // ( Inside ) |
122 | for (int axis = 0; axis < 3; axis++) | 122 | for (int axis = 0; axis < 3; axis++) |
123 | { | 123 | { |
124 | _handles[0].MaxEdges[axis] += 2; | 124 | _handles[0].MaxEdges[axis] += 2; |
125 | 125 | ||
126 | _edges[axis][limit + 1].Position = _edges[axis][limit - 1].Position; | 126 | _edges[axis][limit + 1].Position = _edges[axis][limit - 1].Position; |
127 | _edges[axis][limit + 1].Handle = _edges[axis][limit - 1].Handle; | 127 | _edges[axis][limit + 1].Handle = _edges[axis][limit - 1].Handle; |
128 | 128 | ||
129 | _edges[axis][limit - 1].Position = min[axis]; | 129 | _edges[axis][limit - 1].Position = min[axis]; |
130 | _edges[axis][limit - 1].Handle = handle; | 130 | _edges[axis][limit - 1].Handle = handle; |
131 | 131 | ||
132 | _edges[axis][limit].Position = max[axis]; | 132 | _edges[axis][limit].Position = max[axis]; |
133 | _edges[axis][limit].Handle = handle; | 133 | _edges[axis][limit].Handle = handle; |
134 | 134 | ||
135 | oHandle.MinEdges[axis] = (ushort)(limit - 1); | 135 | oHandle.MinEdges[axis] = (ushort)(limit - 1); |
136 | oHandle.MaxEdges[axis] = (ushort)limit; | 136 | oHandle.MaxEdges[axis] = (ushort)limit; |
137 | } | 137 | } |
138 | 138 | ||
139 | SortMinDown(0, oHandle.MinEdges[0], false); | 139 | SortMinDown(0, oHandle.MinEdges[0], false); |
140 | SortMaxDown(0, oHandle.MaxEdges[0], false); | 140 | SortMaxDown(0, oHandle.MaxEdges[0], false); |
141 | SortMinDown(1, oHandle.MinEdges[1], false); | 141 | SortMinDown(1, oHandle.MinEdges[1], false); |
142 | SortMaxDown(1, oHandle.MaxEdges[1], false); | 142 | SortMaxDown(1, oHandle.MaxEdges[1], false); |
143 | SortMinDown(2, oHandle.MinEdges[2], true); | 143 | SortMinDown(2, oHandle.MinEdges[2], true); |
144 | SortMaxDown(2, oHandle.MaxEdges[2], true); | 144 | SortMaxDown(2, oHandle.MaxEdges[2], true); |
145 | 145 | ||
146 | return handle; | 146 | return handle; |
147 | } | 147 | } |
148 | 148 | ||
149 | public void RemoveHandle(ushort handle) | 149 | public void RemoveHandle(ushort handle) |
150 | { | 150 | { |
151 | Handle pHandle = GetHandle(handle); | 151 | Handle pHandle = GetHandle(handle); |
152 | 152 | ||
153 | //explicitly remove the pairs containing the proxy | 153 | //explicitly remove the pairs containing the proxy |
154 | //we could do it also in the sortMinUp (passing true) | 154 | //we could do it also in the sortMinUp (passing true) |
155 | //todo: compare performance | 155 | //todo: compare performance |
156 | RemoveOverlappingPairsContainingProxy(pHandle); | 156 | RemoveOverlappingPairsContainingProxy(pHandle); |
157 | 157 | ||
158 | 158 | ||
159 | // compute current limit of edge arrays | 159 | // compute current limit of edge arrays |
160 | int limit = _numHandles * 2; | 160 | int limit = _numHandles * 2; |
161 | int axis; | 161 | int axis; |
162 | 162 | ||
163 | for (axis = 0; axis < 3; axis++) | 163 | for (axis = 0; axis < 3; axis++) |
164 | { | 164 | { |
165 | _handles[0].MaxEdges[axis] -= 2; | 165 | _handles[0].MaxEdges[axis] -= 2; |
166 | } | 166 | } |
167 | 167 | ||
168 | // remove the edges by sorting them up to the end of the list | 168 | // remove the edges by sorting them up to the end of the list |
169 | for (axis = 0; axis < 3; axis++) | 169 | for (axis = 0; axis < 3; axis++) |
170 | { | 170 | { |
171 | Edge[] pEdges = _edges[axis]; | 171 | Edge[] pEdges = _edges[axis]; |
172 | ushort max = pHandle.MaxEdges[axis]; | 172 | ushort max = pHandle.MaxEdges[axis]; |
173 | pEdges[max].Position = 0xffff; | 173 | pEdges[max].Position = 0xffff; |
174 | 174 | ||
175 | SortMaxUp(axis, max, false); | 175 | SortMaxUp(axis, max, false); |
176 | 176 | ||
177 | ushort i = pHandle.MinEdges[axis]; | 177 | ushort i = pHandle.MinEdges[axis]; |
178 | pEdges[i].Position = 0xffff; | 178 | pEdges[i].Position = 0xffff; |
179 | 179 | ||
180 | SortMinUp(axis, i, false); | 180 | SortMinUp(axis, i, false); |
181 | 181 | ||
182 | pEdges[limit - 1].Handle = 0; | 182 | pEdges[limit - 1].Handle = 0; |
183 | pEdges[limit - 1].Position = 0xffff; | 183 | pEdges[limit - 1].Position = 0xffff; |
184 | } | 184 | } |
185 | 185 | ||
186 | // free the handle | 186 | // free the handle |
187 | FreeHandle(handle); | 187 | FreeHandle(handle); |
188 | } | 188 | } |
189 | 189 | ||
190 | public override void ProcessAllOverlappingPairs(IOverlapCallback callback) | 190 | public override void ProcessAllOverlappingPairs(IOverlapCallback callback) |
191 | { | 191 | { |
192 | OverlappingPairs.Sort(new Comparison<BroadphasePair>(BroadphasePair.ComparisonSort)); | 192 | OverlappingPairs.Sort(new Comparison<BroadphasePair>(BroadphasePair.ComparisonSort)); |
193 | 193 | ||
194 | if (_invalidPair != 0) | 194 | if (_invalidPair != 0) |
195 | OverlappingPairs.RemoveRange(OverlappingPairs.Count - _invalidPair, _invalidPair); | 195 | OverlappingPairs.RemoveRange(OverlappingPairs.Count - _invalidPair, _invalidPair); |
196 | _invalidPair = 0; | 196 | _invalidPair = 0; |
197 | 197 | ||
198 | BroadphasePair previousPair = new BroadphasePair(); | 198 | BroadphasePair previousPair = new BroadphasePair(); |
199 | previousPair.ProxyA = null; | 199 | previousPair.ProxyA = null; |
200 | previousPair.ProxyB = null; | 200 | previousPair.ProxyB = null; |
201 | previousPair.CollisionAlgorithm = null; | 201 | previousPair.CollisionAlgorithm = null; |
202 | 202 | ||
203 | List<BroadphasePair> removal = new List<BroadphasePair>(); | 203 | List<BroadphasePair> removal = new List<BroadphasePair>(); |
204 | 204 | ||
205 | for (int i = 0; i < OverlappingPairs.Count; i++) | 205 | for (int i = 0; i < OverlappingPairs.Count; i++) |
206 | { | 206 | { |
207 | bool isDuplicate = (OverlappingPairs[i] == previousPair); | 207 | bool isDuplicate = (OverlappingPairs[i] == previousPair); |
208 | previousPair = OverlappingPairs[i]; | 208 | previousPair = OverlappingPairs[i]; |
209 | bool needsRemoval; | 209 | bool needsRemoval; |
210 | if (!isDuplicate) | 210 | if (!isDuplicate) |
211 | { | 211 | { |
212 | bool hasOverlap = TestOverlap(previousPair.ProxyA, previousPair.ProxyB); | 212 | bool hasOverlap = TestOverlap(previousPair.ProxyA, previousPair.ProxyB); |
213 | if (hasOverlap) | 213 | if (hasOverlap) |
214 | { | 214 | { |
215 | needsRemoval = callback.ProcessOverlap(ref previousPair); | 215 | needsRemoval = callback.ProcessOverlap(ref previousPair); |
216 | } | 216 | } |
217 | else | 217 | else |
218 | { | 218 | { |
219 | needsRemoval = true; | 219 | needsRemoval = true; |
220 | } | 220 | } |
221 | } | 221 | } |
222 | else | 222 | else |
223 | { | 223 | { |
224 | needsRemoval = true; | 224 | needsRemoval = true; |
225 | BulletDebug.Assert(previousPair.CollisionAlgorithm == null); | 225 | BulletDebug.Assert(previousPair.CollisionAlgorithm == null); |
226 | } | 226 | } |
227 | 227 | ||
228 | if (needsRemoval) | 228 | if (needsRemoval) |
229 | { | 229 | { |
230 | removal.Add(previousPair); | 230 | removal.Add(previousPair); |
231 | } | 231 | } |
232 | } | 232 | } |
233 | 233 | ||
234 | for (int i = 0; i < removal.Count; i++) | 234 | for (int i = 0; i < removal.Count; i++) |
235 | { | 235 | { |
236 | BroadphasePair pair = removal[i]; | 236 | BroadphasePair pair = removal[i]; |
237 | CleanOverlappingPair(ref pair); | 237 | CleanOverlappingPair(ref pair); |
238 | pair.ProxyA = null; | 238 | pair.ProxyA = null; |
239 | pair.ProxyB = null; | 239 | pair.ProxyB = null; |
240 | _invalidPair++; | 240 | _invalidPair++; |
241 | OverlappingPairCount--; | 241 | OverlappingPairCount--; |
242 | } | 242 | } |
243 | } | 243 | } |
244 | 244 | ||
245 | private bool TestOverlap(BroadphaseProxy proxyA, BroadphaseProxy proxyB) | 245 | private bool TestOverlap(BroadphaseProxy proxyA, BroadphaseProxy proxyB) |
246 | { | 246 | { |
247 | if (proxyA == null || proxyB == null) | 247 | if (proxyA == null || proxyB == null) |
248 | return false; | 248 | return false; |
249 | 249 | ||
250 | Handle handleA = proxyA as Handle; | 250 | Handle handleA = proxyA as Handle; |
251 | Handle handleB = proxyB as Handle; | 251 | Handle handleB = proxyB as Handle; |
252 | 252 | ||
253 | for (int axis = 0; axis < 3; axis++) | 253 | for (int axis = 0; axis < 3; axis++) |
254 | { | 254 | { |
255 | if (handleA.MaxEdges[axis] < handleB.MinEdges[axis] || | 255 | if (handleA.MaxEdges[axis] < handleB.MinEdges[axis] || |
256 | handleB.MaxEdges[axis] < handleA.MinEdges[axis]) | 256 | handleB.MaxEdges[axis] < handleA.MinEdges[axis]) |
257 | { | 257 | { |
258 | return false; | 258 | return false; |
259 | } | 259 | } |
260 | } | 260 | } |
261 | return true; | 261 | return true; |
262 | } | 262 | } |
263 | 263 | ||
264 | private bool TestOverlap(int ignoreAxis, Handle pHandleA, Handle pHandleB) | 264 | private bool TestOverlap(int ignoreAxis, Handle pHandleA, Handle pHandleB) |
265 | { | 265 | { |
266 | for (int axis = 0; axis < 3; axis++) | 266 | for (int axis = 0; axis < 3; axis++) |
267 | { | 267 | { |
268 | if (axis != ignoreAxis) | 268 | if (axis != ignoreAxis) |
269 | { | 269 | { |
270 | if (pHandleA.MaxEdges[axis] < pHandleB.MinEdges[axis] || | 270 | if (pHandleA.MaxEdges[axis] < pHandleB.MinEdges[axis] || |
271 | pHandleB.MaxEdges[axis] < pHandleA.MinEdges[axis]) | 271 | pHandleB.MaxEdges[axis] < pHandleA.MinEdges[axis]) |
272 | { | 272 | { |
273 | return false; | 273 | return false; |
274 | } | 274 | } |
275 | } | 275 | } |
276 | } | 276 | } |
277 | 277 | ||
278 | return true; | 278 | return true; |
279 | } | 279 | } |
280 | 280 | ||
281 | private ushort AllocateHandle() | 281 | private ushort AllocateHandle() |
282 | { | 282 | { |
283 | ushort handle = _firstFreeHandle; | 283 | ushort handle = _firstFreeHandle; |
284 | _firstFreeHandle = GetHandle(handle).NextFree; | 284 | _firstFreeHandle = GetHandle(handle).NextFree; |
285 | _numHandles++; | 285 | _numHandles++; |
286 | 286 | ||
287 | return handle; | 287 | return handle; |
288 | } | 288 | } |
289 | 289 | ||
290 | private void FreeHandle(ushort handle) | 290 | private void FreeHandle(ushort handle) |
291 | { | 291 | { |
292 | BulletDebug.Assert(handle > 0 && handle < _maxHandles); | 292 | BulletDebug.Assert(handle > 0 && handle < _maxHandles); |
293 | 293 | ||
294 | GetHandle(handle).NextFree = _firstFreeHandle; | 294 | GetHandle(handle).NextFree = _firstFreeHandle; |
295 | _firstFreeHandle = handle; | 295 | _firstFreeHandle = handle; |
296 | 296 | ||
297 | _numHandles--; | 297 | _numHandles--; |
298 | } | 298 | } |
299 | 299 | ||
300 | private Handle GetHandle(ushort handle) | 300 | private Handle GetHandle(ushort handle) |
301 | { | 301 | { |
302 | return _handles[handle]; | 302 | return _handles[handle]; |
303 | } | 303 | } |
304 | 304 | ||
305 | private void UpdateHandle(ushort handle, Vector3 aabbMin, Vector3 aabbMax) | 305 | private void UpdateHandle(ushort handle, Vector3 aabbMin, Vector3 aabbMax) |
306 | { | 306 | { |
307 | Handle pHandle = GetHandle(handle); | 307 | Handle pHandle = GetHandle(handle); |
308 | 308 | ||
309 | // quantize the new bounds | 309 | // quantize the new bounds |
310 | ushort[] min = new ushort[3]; | 310 | ushort[] min = new ushort[3]; |
311 | ushort[] max = new ushort[3]; | 311 | ushort[] max = new ushort[3]; |
312 | Quantize(out min, aabbMin, 0); | 312 | Quantize(out min, aabbMin, 0); |
313 | Quantize(out max, aabbMax, 1); | 313 | Quantize(out max, aabbMax, 1); |
314 | 314 | ||
315 | // update changed edges | 315 | // update changed edges |
316 | for (int axis = 0; axis < 3; axis++) | 316 | for (int axis = 0; axis < 3; axis++) |
317 | { | 317 | { |
318 | ushort emin = pHandle.MinEdges[axis]; | 318 | ushort emin = pHandle.MinEdges[axis]; |
319 | ushort emax = pHandle.MaxEdges[axis]; | 319 | ushort emax = pHandle.MaxEdges[axis]; |
320 | 320 | ||
321 | int dmin = (int)min[axis] - (int)_edges[axis][emin].Position; | 321 | int dmin = (int)min[axis] - (int)_edges[axis][emin].Position; |
322 | int dmax = (int)max[axis] - (int)_edges[axis][emax].Position; | 322 | int dmax = (int)max[axis] - (int)_edges[axis][emax].Position; |
323 | 323 | ||
324 | _edges[axis][emin].Position = min[axis]; | 324 | _edges[axis][emin].Position = min[axis]; |
325 | _edges[axis][emax].Position = max[axis]; | 325 | _edges[axis][emax].Position = max[axis]; |
326 | 326 | ||
327 | // expand (only adds overlaps) | 327 | // expand (only adds overlaps) |
328 | if (dmin < 0) | 328 | if (dmin < 0) |
329 | SortMinDown(axis, emin, true); | 329 | SortMinDown(axis, emin, true); |
330 | 330 | ||
331 | if (dmax > 0) | 331 | if (dmax > 0) |
332 | SortMaxUp(axis, emax, true); | 332 | SortMaxUp(axis, emax, true); |
333 | 333 | ||
334 | // shrink (only removes overlaps) | 334 | // shrink (only removes overlaps) |
335 | if (dmin > 0) | 335 | if (dmin > 0) |
336 | SortMinUp(axis, emin, true); | 336 | SortMinUp(axis, emin, true); |
337 | 337 | ||
338 | if (dmax < 0) | 338 | if (dmax < 0) |
339 | SortMaxDown(axis, emax, true); | 339 | SortMaxDown(axis, emax, true); |
340 | } | 340 | } |
341 | } | 341 | } |
342 | 342 | ||
343 | private void Quantize(out ushort[] result, Vector3 point, int isMax) | 343 | private void Quantize(out ushort[] result, Vector3 point, int isMax) |
344 | { | 344 | { |
345 | Vector3 clampedPoint = new Vector3( | 345 | Vector3 clampedPoint = new Vector3( |
346 | point.X, | 346 | point.X, |
347 | point.Y, | 347 | point.Y, |
348 | point.Z | 348 | point.Z |
349 | ); | 349 | ); |
350 | 350 | ||
351 | MathHelper.SetMax(ref clampedPoint, _worldAabbMin); | 351 | MathHelper.SetMax(ref clampedPoint, _worldAabbMin); |
352 | MathHelper.SetMin(ref clampedPoint, _worldAabbMax); | 352 | MathHelper.SetMin(ref clampedPoint, _worldAabbMax); |
353 | 353 | ||
354 | Vector3 v = (clampedPoint - _worldAabbMin) * _quantize; | 354 | Vector3 v = (clampedPoint - _worldAabbMin) * _quantize; |
355 | 355 | ||
356 | result = new ushort[3]; | 356 | result = new ushort[3]; |
357 | result[0] = (ushort)(((int)v.X & 0xfffe) | isMax); | 357 | result[0] = (ushort)(((int)v.X & 0xfffe) | isMax); |
358 | result[1] = (ushort)(((int)v.Y & 0xfffe) | isMax); | 358 | result[1] = (ushort)(((int)v.Y & 0xfffe) | isMax); |
359 | result[2] = (ushort)(((int)v.Z & 0xfffe) | isMax); | 359 | result[2] = (ushort)(((int)v.Z & 0xfffe) | isMax); |
360 | } | 360 | } |
361 | 361 | ||
362 | private void SortMinDown(int axis, ushort edge, bool updateOverlaps) | 362 | private void SortMinDown(int axis, ushort edge, bool updateOverlaps) |
363 | { | 363 | { |
364 | Edge pEdge = _edges[axis][edge]; | 364 | Edge pEdge = _edges[axis][edge]; |
365 | Edge pPrev = _edges[axis][edge - 1]; | 365 | Edge pPrev = _edges[axis][edge - 1]; |
366 | Handle pHandleEdge = GetHandle(pEdge.Handle); | 366 | Handle pHandleEdge = GetHandle(pEdge.Handle); |
367 | 367 | ||
368 | while (pEdge.Position < pPrev.Position) | 368 | while (pEdge.Position < pPrev.Position) |
369 | { | 369 | { |
370 | Handle pHandlePrev = GetHandle(pPrev.Handle); | 370 | Handle pHandlePrev = GetHandle(pPrev.Handle); |
371 | 371 | ||
372 | if (pPrev.IsMax()) | 372 | if (pPrev.IsMax()) |
373 | { | 373 | { |
374 | // if previous edge is a maximum check the bounds and add an overlap if necessary | 374 | // if previous edge is a maximum check the bounds and add an overlap if necessary |
375 | if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandlePrev)) | 375 | if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandlePrev)) |
376 | { | 376 | { |
377 | AddOverlappingPair(pHandleEdge, pHandlePrev); | 377 | AddOverlappingPair(pHandleEdge, pHandlePrev); |
378 | } | 378 | } |
379 | 379 | ||
380 | // update edge reference in other handle | 380 | // update edge reference in other handle |
381 | pHandlePrev.MaxEdges[axis]++; | 381 | pHandlePrev.MaxEdges[axis]++; |
382 | } | 382 | } |
383 | else | 383 | else |
384 | pHandlePrev.MinEdges[axis]++; | 384 | pHandlePrev.MinEdges[axis]++; |
385 | 385 | ||
386 | pHandleEdge.MinEdges[axis]--; | 386 | pHandleEdge.MinEdges[axis]--; |
387 | 387 | ||
388 | // swap the edges | 388 | // swap the edges |
389 | pEdge.Swap(ref pPrev); | 389 | pEdge.Swap(ref pPrev); |
390 | 390 | ||
391 | // decrement | 391 | // decrement |
392 | edge--; | 392 | edge--; |
393 | pEdge = _edges[axis][edge]; | 393 | pEdge = _edges[axis][edge]; |
394 | pPrev = _edges[axis][edge - 1]; | 394 | pPrev = _edges[axis][edge - 1]; |
395 | } | 395 | } |
396 | } | 396 | } |
397 | 397 | ||
398 | private void SortMinUp(int axis, ushort edge, bool updateOverlaps) | 398 | private void SortMinUp(int axis, ushort edge, bool updateOverlaps) |
399 | { | 399 | { |
400 | Edge pEdge = _edges[axis][edge]; | 400 | Edge pEdge = _edges[axis][edge]; |
401 | Edge pNext = _edges[axis][edge + 1]; | 401 | Edge pNext = _edges[axis][edge + 1]; |
402 | Handle pHandleEdge = GetHandle(pEdge.Handle); | 402 | Handle pHandleEdge = GetHandle(pEdge.Handle); |
403 | 403 | ||
404 | while ((pNext.Handle != 0) && (pEdge.Position >= pNext.Position)) | 404 | while ((pNext.Handle != 0) && (pEdge.Position >= pNext.Position)) |
405 | { | 405 | { |
406 | Handle pHandleNext = GetHandle(pNext.Handle); | 406 | Handle pHandleNext = GetHandle(pNext.Handle); |
407 | 407 | ||
408 | if (pNext.IsMax()) | 408 | if (pNext.IsMax()) |
409 | { | 409 | { |
410 | // if next edge is maximum remove any overlap between the two handles | 410 | // if next edge is maximum remove any overlap between the two handles |
411 | if (updateOverlaps) | 411 | if (updateOverlaps) |
412 | { | 412 | { |
413 | //Handle handle0 = GetHandle(pEdge.Handle); | 413 | //Handle handle0 = GetHandle(pEdge.Handle); |
414 | //Handle handle1 = GetHandle(pNext.Handle); | 414 | //Handle handle1 = GetHandle(pNext.Handle); |
415 | //BroadphasePair tmpPair = new BroadphasePair(handle0, handle1); | 415 | //BroadphasePair tmpPair = new BroadphasePair(handle0, handle1); |
416 | //RemoveOverlappingPair(tmpPair); | 416 | //RemoveOverlappingPair(tmpPair); |
417 | } | 417 | } |
418 | 418 | ||
419 | // update edge reference in other handle | 419 | // update edge reference in other handle |
420 | pHandleNext.MaxEdges[axis]--; | 420 | pHandleNext.MaxEdges[axis]--; |
421 | } | 421 | } |
422 | else | 422 | else |
423 | pHandleNext.MinEdges[axis]--; | 423 | pHandleNext.MinEdges[axis]--; |
424 | 424 | ||
425 | pHandleEdge.MinEdges[axis]++; | 425 | pHandleEdge.MinEdges[axis]++; |
426 | 426 | ||
427 | // swap the edges | 427 | // swap the edges |
428 | pEdge.Swap(ref pNext); | 428 | pEdge.Swap(ref pNext); |
429 | 429 | ||
430 | // increment | 430 | // increment |
431 | edge++; | 431 | edge++; |
432 | pEdge = _edges[axis][edge]; | 432 | pEdge = _edges[axis][edge]; |
433 | pNext = _edges[axis][edge + 1]; | 433 | pNext = _edges[axis][edge + 1]; |
434 | } | 434 | } |
435 | } | 435 | } |
436 | 436 | ||
437 | private void SortMaxDown(int axis, ushort edge, bool updateOverlaps) | 437 | private void SortMaxDown(int axis, ushort edge, bool updateOverlaps) |
438 | { | 438 | { |
439 | Edge pEdge = _edges[axis][edge]; | 439 | Edge pEdge = _edges[axis][edge]; |
440 | Edge pPrev = _edges[axis][edge - 1]; | 440 | Edge pPrev = _edges[axis][edge - 1]; |
441 | Handle pHandleEdge = GetHandle(pEdge.Handle); | 441 | Handle pHandleEdge = GetHandle(pEdge.Handle); |
442 | 442 | ||
443 | while (pEdge.Position < pPrev.Position) | 443 | while (pEdge.Position < pPrev.Position) |
444 | { | 444 | { |
445 | Handle pHandlePrev = GetHandle(pPrev.Handle); | 445 | Handle pHandlePrev = GetHandle(pPrev.Handle); |
446 | 446 | ||
447 | if (!pPrev.IsMax()) | 447 | if (!pPrev.IsMax()) |
448 | { | 448 | { |
449 | // if previous edge was a minimum remove any overlap between the two handles | 449 | // if previous edge was a minimum remove any overlap between the two handles |
450 | if (updateOverlaps) | 450 | if (updateOverlaps) |
451 | { | 451 | { |
452 | //this is done during the overlappingpairarray iteration/narrowphase collision | 452 | //this is done during the overlappingpairarray iteration/narrowphase collision |
453 | //Handle handle0 = GetHandle(pEdge.Handle); | 453 | //Handle handle0 = GetHandle(pEdge.Handle); |
454 | //Handle handle1 = GetHandle(pPrev.Handle); | 454 | //Handle handle1 = GetHandle(pPrev.Handle); |
455 | //BroadphasePair pair = FindPair(handle0, handle1); | 455 | //BroadphasePair pair = FindPair(handle0, handle1); |
456 | 456 | ||
457 | //if (pair != null) | 457 | //if (pair != null) |
458 | //{ | 458 | //{ |
459 | // RemoveOverlappingPair(pair); | 459 | // RemoveOverlappingPair(pair); |
460 | //} | 460 | //} |
461 | } | 461 | } |
462 | 462 | ||
463 | // update edge reference in other handle | 463 | // update edge reference in other handle |
464 | pHandlePrev.MinEdges[axis]++; ; | 464 | pHandlePrev.MinEdges[axis]++; ; |
465 | } | 465 | } |
466 | else | 466 | else |
467 | pHandlePrev.MaxEdges[axis]++; | 467 | pHandlePrev.MaxEdges[axis]++; |
468 | 468 | ||
469 | pHandleEdge.MaxEdges[axis]--; | 469 | pHandleEdge.MaxEdges[axis]--; |
470 | 470 | ||
471 | // swap the edges | 471 | // swap the edges |
472 | pEdge.Swap(ref pPrev); | 472 | pEdge.Swap(ref pPrev); |
473 | 473 | ||
474 | // decrement | 474 | // decrement |
475 | edge--; | 475 | edge--; |
476 | pEdge = _edges[axis][edge]; | 476 | pEdge = _edges[axis][edge]; |
477 | pPrev = _edges[axis][edge - 1]; | 477 | pPrev = _edges[axis][edge - 1]; |
478 | } | 478 | } |
479 | } | 479 | } |
480 | 480 | ||
481 | private void SortMaxUp(int axis, ushort edge, bool updateOverlaps) | 481 | private void SortMaxUp(int axis, ushort edge, bool updateOverlaps) |
482 | { | 482 | { |
483 | Edge pEdge = _edges[axis][edge]; | 483 | Edge pEdge = _edges[axis][edge]; |
484 | Edge pNext = _edges[axis][edge + 1]; | 484 | Edge pNext = _edges[axis][edge + 1]; |
485 | Handle pHandleEdge = GetHandle(pEdge.Handle); | 485 | Handle pHandleEdge = GetHandle(pEdge.Handle); |
486 | 486 | ||
487 | while ((pNext.Handle!=0) && (pEdge.Position >= pNext.Position)) | 487 | while ((pNext.Handle!=0) && (pEdge.Position >= pNext.Position)) |
488 | { | 488 | { |
489 | Handle pHandleNext = GetHandle(pNext.Handle); | 489 | Handle pHandleNext = GetHandle(pNext.Handle); |
490 | 490 | ||
491 | if (!pNext.IsMax()) | 491 | if (!pNext.IsMax()) |
492 | { | 492 | { |
493 | // if next edge is a minimum check the bounds and add an overlap if necessary | 493 | // if next edge is a minimum check the bounds and add an overlap if necessary |
494 | if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandleNext)) | 494 | if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandleNext)) |
495 | { | 495 | { |
496 | Handle handle0 = GetHandle(pEdge.Handle); | 496 | Handle handle0 = GetHandle(pEdge.Handle); |
497 | Handle handle1 = GetHandle(pNext.Handle); | 497 | Handle handle1 = GetHandle(pNext.Handle); |
498 | AddOverlappingPair(handle0, handle1); | 498 | AddOverlappingPair(handle0, handle1); |
499 | } | 499 | } |
500 | 500 | ||
501 | // update edge reference in other handle | 501 | // update edge reference in other handle |
502 | pHandleNext.MinEdges[axis]--; | 502 | pHandleNext.MinEdges[axis]--; |
503 | } | 503 | } |
504 | else | 504 | else |
505 | pHandleNext.MaxEdges[axis]--; | 505 | pHandleNext.MaxEdges[axis]--; |
506 | 506 | ||
507 | pHandleEdge.MaxEdges[axis]++; | 507 | pHandleEdge.MaxEdges[axis]++; |
508 | 508 | ||
509 | // swap the edges | 509 | // swap the edges |
510 | pEdge.Swap(ref pNext); | 510 | pEdge.Swap(ref pNext); |
511 | 511 | ||
512 | // increment | 512 | // increment |
513 | edge++; | 513 | edge++; |
514 | pEdge = _edges[axis][edge]; | 514 | pEdge = _edges[axis][edge]; |
515 | pNext = _edges[axis][edge + 1]; | 515 | pNext = _edges[axis][edge + 1]; |
516 | } | 516 | } |
517 | } | 517 | } |
518 | 518 | ||
519 | #region Abstract | 519 | #region Abstract |
520 | 520 | ||
521 | public override void RefreshOverlappingPairs() | 521 | public override void RefreshOverlappingPairs() |
522 | { | 522 | { |
523 | } | 523 | } |
524 | 524 | ||
525 | public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) | 525 | public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) |
526 | { | 526 | { |
527 | ushort handleId = AddHandle(min, max, userData, collisionFilterGroup, collisionFilterMask); | 527 | ushort handleId = AddHandle(min, max, userData, collisionFilterGroup, collisionFilterMask); |
528 | 528 | ||
529 | Handle handle = GetHandle(handleId); | 529 | Handle handle = GetHandle(handleId); |
530 | 530 | ||
531 | return handle; | 531 | return handle; |
532 | } | 532 | } |
533 | 533 | ||
534 | public override void DestroyProxy(BroadphaseProxy proxy) | 534 | public override void DestroyProxy(BroadphaseProxy proxy) |
535 | { | 535 | { |
536 | Handle handle = proxy as Handle; | 536 | Handle handle = proxy as Handle; |
537 | RemoveHandle(handle.HandleID); | 537 | RemoveHandle(handle.HandleID); |
538 | } | 538 | } |
539 | 539 | ||
540 | public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax) | 540 | public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax) |
541 | { | 541 | { |
542 | Handle handle = proxy as Handle; | 542 | Handle handle = proxy as Handle; |
543 | UpdateHandle(handle.HandleID, aabbMin, aabbMax); | 543 | UpdateHandle(handle.HandleID, aabbMin, aabbMax); |
544 | } | 544 | } |
545 | #endregion | 545 | #endregion |
546 | } | 546 | } |
547 | 547 | ||
548 | public class Edge | 548 | public class Edge |
549 | { | 549 | { |
550 | ushort position; | 550 | ushort position; |
551 | ushort handle; | 551 | ushort handle; |
552 | 552 | ||
553 | public ushort Position | 553 | public ushort Position |
554 | { | 554 | { |
555 | get { return position; } | 555 | get { return position; } |
556 | set { position = value; } | 556 | set { position = value; } |
557 | } | 557 | } |
558 | 558 | ||
559 | public ushort Handle | 559 | public ushort Handle |
560 | { | 560 | { |
561 | get { return handle; } | 561 | get { return handle; } |
562 | set { handle = value; } | 562 | set { handle = value; } |
563 | } | 563 | } |
564 | 564 | ||
565 | public bool IsMax() | 565 | public bool IsMax() |
566 | { | 566 | { |
567 | return (position & (ushort)1) == 1; | 567 | return (position & (ushort)1) == 1; |
568 | } | 568 | } |
569 | 569 | ||
570 | public void Swap(ref Edge e) | 570 | public void Swap(ref Edge e) |
571 | { | 571 | { |
572 | ushort tmpPosition = this.position; | 572 | ushort tmpPosition = this.position; |
573 | ushort tmpHandle = this.handle; | 573 | ushort tmpHandle = this.handle; |
574 | this.position = e.position; | 574 | this.position = e.position; |
575 | this.handle = e.handle; | 575 | this.handle = e.handle; |
576 | e.position = tmpPosition; | 576 | e.position = tmpPosition; |
577 | e.handle = tmpHandle; | 577 | e.handle = tmpHandle; |
578 | } | 578 | } |
579 | } | 579 | } |
580 | 580 | ||
581 | public class Handle: BroadphaseProxy | 581 | public class Handle: BroadphaseProxy |
582 | { | 582 | { |
583 | ushort[] minEdges, maxEdges; | 583 | ushort[] minEdges, maxEdges; |
584 | ushort pad; | 584 | ushort pad; |
585 | ushort handleID; | 585 | ushort handleID; |
586 | 586 | ||
587 | public ushort[] MinEdges | 587 | public ushort[] MinEdges |
588 | { | 588 | { |
589 | get { return minEdges; } | 589 | get { return minEdges; } |
590 | set { minEdges = value; } | 590 | set { minEdges = value; } |
591 | } | 591 | } |
592 | 592 | ||
593 | public ushort[] MaxEdges | 593 | public ushort[] MaxEdges |
594 | { | 594 | { |
595 | get { return maxEdges; } | 595 | get { return maxEdges; } |
596 | set { maxEdges = value; } | 596 | set { maxEdges = value; } |
597 | } | 597 | } |
598 | 598 | ||
599 | public ushort HandleID | 599 | public ushort HandleID |
600 | { | 600 | { |
601 | get { return handleID; } | 601 | get { return handleID; } |
602 | set { handleID = value; } | 602 | set { handleID = value; } |
603 | } | 603 | } |
604 | 604 | ||
605 | public ushort Pad | 605 | public ushort Pad |
606 | { | 606 | { |
607 | get { return pad; } | 607 | get { return pad; } |
608 | set { pad = value; } | 608 | set { pad = value; } |
609 | } | 609 | } |
610 | 610 | ||
611 | public ushort NextFree | 611 | public ushort NextFree |
612 | { | 612 | { |
613 | get { return minEdges[0]; } | 613 | get { return minEdges[0]; } |
614 | set { minEdges[0] = value;} | 614 | set { minEdges[0] = value;} |
615 | } | 615 | } |
616 | 616 | ||
617 | public Handle() | 617 | public Handle() |
618 | { | 618 | { |
619 | minEdges = new ushort[3]; | 619 | minEdges = new ushort[3]; |
620 | maxEdges = new ushort[3]; | 620 | maxEdges = new ushort[3]; |
621 | } | 621 | } |
622 | } | 622 | } |
623 | } | 623 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs index dcfd0d8..eb1fd4b 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs | |||
@@ -1,68 +1,68 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX | 26 | namespace XnaDevRu.BulletX |
27 | { | 27 | { |
28 | /// Dispatcher uses these types | 28 | /// Dispatcher uses these types |
29 | /// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave | 29 | /// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave |
30 | /// to facilitate type checking | 30 | /// to facilitate type checking |
31 | public enum BroadphaseNativeTypes | 31 | public enum BroadphaseNativeTypes |
32 | { | 32 | { |
33 | // polyhedral convex shapes | 33 | // polyhedral convex shapes |
34 | Box, | 34 | Box, |
35 | Triangle, | 35 | Triangle, |
36 | Tetrahedral, | 36 | Tetrahedral, |
37 | ConvexTriangleMesh, | 37 | ConvexTriangleMesh, |
38 | ConvexHull, | 38 | ConvexHull, |
39 | //implicit convex shapes | 39 | //implicit convex shapes |
40 | ImplicitConvexShapes, | 40 | ImplicitConvexShapes, |
41 | Sphere, | 41 | Sphere, |
42 | MultiSphere, | 42 | MultiSphere, |
43 | Capsule, | 43 | Capsule, |
44 | Cone, | 44 | Cone, |
45 | Convex, | 45 | Convex, |
46 | Cylinder, | 46 | Cylinder, |
47 | MinkowskiSum, | 47 | MinkowskiSum, |
48 | MinkowskiDifference, | 48 | MinkowskiDifference, |
49 | //concave shapes | 49 | //concave shapes |
50 | ConcaveShapesStart, | 50 | ConcaveShapesStart, |
51 | //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy! | 51 | //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy! |
52 | TriangleMesh, | 52 | TriangleMesh, |
53 | //used for demo integration FAST/Swift collision library and Bullet | 53 | //used for demo integration FAST/Swift collision library and Bullet |
54 | FastConcaveMesh, | 54 | FastConcaveMesh, |
55 | //terrain | 55 | //terrain |
56 | Terrain, | 56 | Terrain, |
57 | //Used for GIMPACT Trimesh integration | 57 | //Used for GIMPACT Trimesh integration |
58 | Gimpact, | 58 | Gimpact, |
59 | 59 | ||
60 | Empty, | 60 | Empty, |
61 | StaticPlane, | 61 | StaticPlane, |
62 | ConcaveShapesEnd, | 62 | ConcaveShapesEnd, |
63 | 63 | ||
64 | Compound, | 64 | Compound, |
65 | 65 | ||
66 | MaxBroadphaseCollisionTypes, | 66 | MaxBroadphaseCollisionTypes, |
67 | } | 67 | } |
68 | } | 68 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs index 2b9e114..eec77ac 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs | |||
@@ -1,113 +1,113 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX | 26 | namespace XnaDevRu.BulletX |
27 | { | 27 | { |
28 | public class BroadphasePair | 28 | public class BroadphasePair |
29 | { | 29 | { |
30 | private BroadphaseProxy _proxyA; | 30 | private BroadphaseProxy _proxyA; |
31 | private BroadphaseProxy _proxyB; | 31 | private BroadphaseProxy _proxyB; |
32 | 32 | ||
33 | private CollisionAlgorithm _algorithm; | 33 | private CollisionAlgorithm _algorithm; |
34 | private object _userInfo; | 34 | private object _userInfo; |
35 | 35 | ||
36 | public BroadphasePair() | 36 | public BroadphasePair() |
37 | { | 37 | { |
38 | } | 38 | } |
39 | 39 | ||
40 | public BroadphasePair(BroadphasePair other) | 40 | public BroadphasePair(BroadphasePair other) |
41 | { | 41 | { |
42 | _proxyA = other._proxyA; | 42 | _proxyA = other._proxyA; |
43 | _proxyB = other._proxyB; | 43 | _proxyB = other._proxyB; |
44 | 44 | ||
45 | _algorithm = other._algorithm; | 45 | _algorithm = other._algorithm; |
46 | _userInfo = null; | 46 | _userInfo = null; |
47 | } | 47 | } |
48 | 48 | ||
49 | public BroadphasePair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) | 49 | public BroadphasePair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) |
50 | { | 50 | { |
51 | _proxyA = proxyA; | 51 | _proxyA = proxyA; |
52 | _proxyB = proxyB; | 52 | _proxyB = proxyB; |
53 | 53 | ||
54 | _algorithm = null; | 54 | _algorithm = null; |
55 | _userInfo = null; | 55 | _userInfo = null; |
56 | } | 56 | } |
57 | 57 | ||
58 | public BroadphaseProxy ProxyA { get { return _proxyA; } set { _proxyA = value; } } | 58 | public BroadphaseProxy ProxyA { get { return _proxyA; } set { _proxyA = value; } } |
59 | public BroadphaseProxy ProxyB { get { return _proxyB; } set { _proxyB = value; } } | 59 | public BroadphaseProxy ProxyB { get { return _proxyB; } set { _proxyB = value; } } |
60 | 60 | ||
61 | public CollisionAlgorithm CollisionAlgorithm { get { return _algorithm; } set { _algorithm = value; } } | 61 | public CollisionAlgorithm CollisionAlgorithm { get { return _algorithm; } set { _algorithm = value; } } |
62 | public object UserInfo { get { return _userInfo; } set { _userInfo = value; } } | 62 | public object UserInfo { get { return _userInfo; } set { _userInfo = value; } } |
63 | 63 | ||
64 | public override int GetHashCode() | 64 | public override int GetHashCode() |
65 | { | 65 | { |
66 | return _proxyA.GetHashCode() ^ _proxyB.GetHashCode(); | 66 | return _proxyA.GetHashCode() ^ _proxyB.GetHashCode(); |
67 | } | 67 | } |
68 | 68 | ||
69 | public override bool Equals(object obj) | 69 | public override bool Equals(object obj) |
70 | { | 70 | { |
71 | if (obj is BroadphasePair) | 71 | if (obj is BroadphasePair) |
72 | return this == (BroadphasePair)obj; | 72 | return this == (BroadphasePair)obj; |
73 | return false; | 73 | return false; |
74 | } | 74 | } |
75 | 75 | ||
76 | public static int ComparisonSort(BroadphasePair a, BroadphasePair b) | 76 | public static int ComparisonSort(BroadphasePair a, BroadphasePair b) |
77 | { | 77 | { |
78 | int aAId = a.ProxyA != null ? a.ProxyA.ComparisonID : -1; | 78 | int aAId = a.ProxyA != null ? a.ProxyA.ComparisonID : -1; |
79 | int aBId = a.ProxyB != null ? a.ProxyB.ComparisonID : -1; | 79 | int aBId = a.ProxyB != null ? a.ProxyB.ComparisonID : -1; |
80 | int aCId = a.CollisionAlgorithm != null ? a.CollisionAlgorithm.ComparisonID : -1; | 80 | int aCId = a.CollisionAlgorithm != null ? a.CollisionAlgorithm.ComparisonID : -1; |
81 | int bAId = b.ProxyA != null ? b.ProxyA.ComparisonID : -1; | 81 | int bAId = b.ProxyA != null ? b.ProxyA.ComparisonID : -1; |
82 | int bBId = b.ProxyB != null ? b.ProxyB.ComparisonID : -1; | 82 | int bBId = b.ProxyB != null ? b.ProxyB.ComparisonID : -1; |
83 | int bCId = b.CollisionAlgorithm != null ? b.CollisionAlgorithm.ComparisonID : -1; | 83 | int bCId = b.CollisionAlgorithm != null ? b.CollisionAlgorithm.ComparisonID : -1; |
84 | 84 | ||
85 | if (aAId > bAId || | 85 | if (aAId > bAId || |
86 | (a.ProxyA == b.ProxyA && aBId > bBId) || | 86 | (a.ProxyA == b.ProxyA && aBId > bBId) || |
87 | (a.ProxyA == b.ProxyA && a.ProxyB == b.ProxyB && aCId > bCId)) | 87 | (a.ProxyA == b.ProxyA && a.ProxyB == b.ProxyB && aCId > bCId)) |
88 | return -1; | 88 | return -1; |
89 | else | 89 | else |
90 | return 1; | 90 | return 1; |
91 | } | 91 | } |
92 | 92 | ||
93 | public static bool operator ==(BroadphasePair a, BroadphasePair b) | 93 | public static bool operator ==(BroadphasePair a, BroadphasePair b) |
94 | { | 94 | { |
95 | if (object.Equals(a, null) && object.Equals(b, null)) | 95 | if (object.Equals(a, null) && object.Equals(b, null)) |
96 | return true; | 96 | return true; |
97 | if (object.Equals(a, null) || object.Equals(b, null)) | 97 | if (object.Equals(a, null) || object.Equals(b, null)) |
98 | return false; | 98 | return false; |
99 | 99 | ||
100 | return (a.ProxyA == b.ProxyA) && (a.ProxyB == b.ProxyB); | 100 | return (a.ProxyA == b.ProxyA) && (a.ProxyB == b.ProxyB); |
101 | } | 101 | } |
102 | 102 | ||
103 | public static bool operator !=(BroadphasePair a, BroadphasePair b) | 103 | public static bool operator !=(BroadphasePair a, BroadphasePair b) |
104 | { | 104 | { |
105 | if (object.Equals(a, null) && object.Equals(b, null)) | 105 | if (object.Equals(a, null) && object.Equals(b, null)) |
106 | return true; | 106 | return true; |
107 | if (object.Equals(a, null) || object.Equals(b, null)) | 107 | if (object.Equals(a, null) || object.Equals(b, null)) |
108 | return false; | 108 | return false; |
109 | 109 | ||
110 | return (a.ProxyA != b.ProxyA) || (a.ProxyB != b.ProxyB); | 110 | return (a.ProxyA != b.ProxyA) || (a.ProxyB != b.ProxyB); |
111 | } | 111 | } |
112 | } | 112 | } |
113 | } | 113 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs index aed82d5..0abc613 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs | |||
@@ -1,91 +1,91 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX | 26 | namespace XnaDevRu.BulletX |
27 | { | 27 | { |
28 | public class BroadphaseProxy | 28 | public class BroadphaseProxy |
29 | { | 29 | { |
30 | //Usually the client CollisionObject or Rigidbody class | 30 | //Usually the client CollisionObject or Rigidbody class |
31 | private object _clientObject; | 31 | private object _clientObject; |
32 | private CollisionFilterGroups _collisionFilterGroup; | 32 | private CollisionFilterGroups _collisionFilterGroup; |
33 | private CollisionFilterGroups _collisionFilterMask; | 33 | private CollisionFilterGroups _collisionFilterMask; |
34 | private readonly int _comparisonID; | 34 | private readonly int _comparisonID; |
35 | 35 | ||
36 | private static int _globalCount = 0; | 36 | private static int _globalCount = 0; |
37 | 37 | ||
38 | public BroadphaseProxy() | 38 | public BroadphaseProxy() |
39 | { | 39 | { |
40 | _comparisonID = _globalCount++; | 40 | _comparisonID = _globalCount++; |
41 | } | 41 | } |
42 | 42 | ||
43 | public BroadphaseProxy(object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask) | 43 | public BroadphaseProxy(object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask) |
44 | : this() | 44 | : this() |
45 | { | 45 | { |
46 | _clientObject = userData; | 46 | _clientObject = userData; |
47 | _collisionFilterGroup = collisionFilterGroup; | 47 | _collisionFilterGroup = collisionFilterGroup; |
48 | _collisionFilterMask = collisionFilterMask; | 48 | _collisionFilterMask = collisionFilterMask; |
49 | } | 49 | } |
50 | 50 | ||
51 | public object ClientData { get { return _clientObject; } set { _clientObject = value; } } | 51 | public object ClientData { get { return _clientObject; } set { _clientObject = value; } } |
52 | public CollisionFilterGroups CollisionFilterGroup { get { return _collisionFilterGroup; } set { _collisionFilterGroup = value; } } | 52 | public CollisionFilterGroups CollisionFilterGroup { get { return _collisionFilterGroup; } set { _collisionFilterGroup = value; } } |
53 | public CollisionFilterGroups CollisionFilterMask { get { return _collisionFilterMask; } set { _collisionFilterMask = value; } } | 53 | public CollisionFilterGroups CollisionFilterMask { get { return _collisionFilterMask; } set { _collisionFilterMask = value; } } |
54 | internal int ComparisonID { get { return _comparisonID; } } | 54 | internal int ComparisonID { get { return _comparisonID; } } |
55 | 55 | ||
56 | public static bool IsPolyhedral(BroadphaseNativeTypes proxyType) | 56 | public static bool IsPolyhedral(BroadphaseNativeTypes proxyType) |
57 | { | 57 | { |
58 | return (proxyType < BroadphaseNativeTypes.ImplicitConvexShapes); | 58 | return (proxyType < BroadphaseNativeTypes.ImplicitConvexShapes); |
59 | } | 59 | } |
60 | 60 | ||
61 | public static bool IsConvex(BroadphaseNativeTypes proxyType) | 61 | public static bool IsConvex(BroadphaseNativeTypes proxyType) |
62 | { | 62 | { |
63 | return (proxyType < BroadphaseNativeTypes.ConcaveShapesStart); | 63 | return (proxyType < BroadphaseNativeTypes.ConcaveShapesStart); |
64 | } | 64 | } |
65 | 65 | ||
66 | public static bool IsConcave(BroadphaseNativeTypes proxyType) | 66 | public static bool IsConcave(BroadphaseNativeTypes proxyType) |
67 | { | 67 | { |
68 | return ((proxyType > BroadphaseNativeTypes.ConcaveShapesStart) && | 68 | return ((proxyType > BroadphaseNativeTypes.ConcaveShapesStart) && |
69 | (proxyType < BroadphaseNativeTypes.ConcaveShapesEnd)); | 69 | (proxyType < BroadphaseNativeTypes.ConcaveShapesEnd)); |
70 | } | 70 | } |
71 | public static bool IsCompound(BroadphaseNativeTypes proxyType) | 71 | public static bool IsCompound(BroadphaseNativeTypes proxyType) |
72 | { | 72 | { |
73 | return (proxyType == BroadphaseNativeTypes.Compound); | 73 | return (proxyType == BroadphaseNativeTypes.Compound); |
74 | } | 74 | } |
75 | public static bool IsInfinite(BroadphaseNativeTypes proxyType) | 75 | public static bool IsInfinite(BroadphaseNativeTypes proxyType) |
76 | { | 76 | { |
77 | return (proxyType == BroadphaseNativeTypes.StaticPlane); | 77 | return (proxyType == BroadphaseNativeTypes.StaticPlane); |
78 | } | 78 | } |
79 | 79 | ||
80 | //optional filtering to cull potential collisions | 80 | //optional filtering to cull potential collisions |
81 | public enum CollisionFilterGroups | 81 | public enum CollisionFilterGroups |
82 | { | 82 | { |
83 | Default = 1, | 83 | Default = 1, |
84 | Static = 2, | 84 | Static = 2, |
85 | Kinematic = 4, | 85 | Kinematic = 4, |
86 | Debris = 8, | 86 | Debris = 8, |
87 | Sensor = 16, | 87 | Sensor = 16, |
88 | All = Default | Static | Kinematic | Debris | Sensor, | 88 | All = Default | Static | Kinematic | Debris | Sensor, |
89 | } | 89 | } |
90 | } | 90 | } |
91 | } | 91 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs index 1e59ad8..387340d 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs | |||
@@ -1,51 +1,51 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX | 26 | namespace XnaDevRu.BulletX |
27 | { | 27 | { |
28 | /// <summary> | 28 | /// <summary> |
29 | /// CollisionAlgorithm is an collision interface that is compatible with the Broadphase and Dispatcher. | 29 | /// CollisionAlgorithm is an collision interface that is compatible with the Broadphase and Dispatcher. |
30 | /// It is persistent over frames | 30 | /// It is persistent over frames |
31 | /// </summary> | 31 | /// </summary> |
32 | public abstract class CollisionAlgorithm | 32 | public abstract class CollisionAlgorithm |
33 | { | 33 | { |
34 | private IDispatcher _dispatcher; | 34 | private IDispatcher _dispatcher; |
35 | private readonly int _comparisonID = 0; | 35 | private readonly int _comparisonID = 0; |
36 | 36 | ||
37 | private static int _globalCount = 0; | 37 | private static int _globalCount = 0; |
38 | 38 | ||
39 | public CollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) | 39 | public CollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) |
40 | { | 40 | { |
41 | _comparisonID = _globalCount++; | 41 | _comparisonID = _globalCount++; |
42 | _dispatcher = collisionAlgorithmConstructionInfo.Dispatcher; | 42 | _dispatcher = collisionAlgorithmConstructionInfo.Dispatcher; |
43 | } | 43 | } |
44 | 44 | ||
45 | protected IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } } | 45 | protected IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } } |
46 | internal int ComparisonID { get { return _comparisonID; } } | 46 | internal int ComparisonID { get { return _comparisonID; } } |
47 | 47 | ||
48 | public abstract void ProcessCollision(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut); | 48 | public abstract void ProcessCollision(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut); |
49 | public abstract float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut); | 49 | public abstract float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut); |
50 | } | 50 | } |
51 | } | 51 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs index 381752a..de238b9 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs | |||
@@ -1,42 +1,42 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX | 26 | namespace XnaDevRu.BulletX |
27 | { | 27 | { |
28 | public struct CollisionAlgorithmConstructionInfo | 28 | public struct CollisionAlgorithmConstructionInfo |
29 | { | 29 | { |
30 | private IDispatcher _dispatcher; | 30 | private IDispatcher _dispatcher; |
31 | private PersistentManifold _manifold; | 31 | private PersistentManifold _manifold; |
32 | 32 | ||
33 | public CollisionAlgorithmConstructionInfo(IDispatcher dispatcher) | 33 | public CollisionAlgorithmConstructionInfo(IDispatcher dispatcher) |
34 | { | 34 | { |
35 | _dispatcher = dispatcher; | 35 | _dispatcher = dispatcher; |
36 | _manifold = null; | 36 | _manifold = null; |
37 | } | 37 | } |
38 | 38 | ||
39 | public IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } } | 39 | public IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } } |
40 | public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } | 40 | public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } |
41 | } | 41 | } |
42 | } | 42 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs index 8c0367f..cde3221 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs | |||
@@ -1,54 +1,54 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX | 26 | namespace XnaDevRu.BulletX |
27 | { | 27 | { |
28 | public enum DispatchFunction | 28 | public enum DispatchFunction |
29 | { | 29 | { |
30 | Discrete = 1, | 30 | Discrete = 1, |
31 | Continuous, | 31 | Continuous, |
32 | } | 32 | } |
33 | 33 | ||
34 | public class DispatcherInfo | 34 | public class DispatcherInfo |
35 | { | 35 | { |
36 | private float _timeStep; | 36 | private float _timeStep; |
37 | private int _stepCount; | 37 | private int _stepCount; |
38 | private DispatchFunction _dispatchFunc = DispatchFunction.Discrete; | 38 | private DispatchFunction _dispatchFunc = DispatchFunction.Discrete; |
39 | private float _timeOfImpact = 1; | 39 | private float _timeOfImpact = 1; |
40 | private bool _useContinuous; | 40 | private bool _useContinuous; |
41 | private bool _enableSatConvex; | 41 | private bool _enableSatConvex; |
42 | private bool _enableSpu; | 42 | private bool _enableSpu; |
43 | private IDebugDraw _debugDraw; | 43 | private IDebugDraw _debugDraw; |
44 | 44 | ||
45 | public float TimeStep { get { return _timeStep; } set { _timeStep = value; } } | 45 | public float TimeStep { get { return _timeStep; } set { _timeStep = value; } } |
46 | public int StepCount { get { return _stepCount; } set { _stepCount = value; } } | 46 | public int StepCount { get { return _stepCount; } set { _stepCount = value; } } |
47 | public DispatchFunction DispatchFunction { get { return _dispatchFunc; } set { _dispatchFunc = value; } } | 47 | public DispatchFunction DispatchFunction { get { return _dispatchFunc; } set { _dispatchFunc = value; } } |
48 | public float TimeOfImpact { get { return _timeOfImpact; } set { _timeOfImpact = value; } } | 48 | public float TimeOfImpact { get { return _timeOfImpact; } set { _timeOfImpact = value; } } |
49 | public bool UseContinuous { get { return _useContinuous; } set { _useContinuous = value; } } | 49 | public bool UseContinuous { get { return _useContinuous; } set { _useContinuous = value; } } |
50 | public bool EnableSatConvex { get { return _enableSatConvex; } set { _enableSatConvex = value; } } | 50 | public bool EnableSatConvex { get { return _enableSatConvex; } set { _enableSatConvex = value; } } |
51 | public bool enableSpu { get { return _enableSpu; } set { _enableSpu = value; } } | 51 | public bool enableSpu { get { return _enableSpu; } set { _enableSpu = value; } } |
52 | public IDebugDraw DebugDraw { get { return _debugDraw; } set { _debugDraw = value; } } | 52 | public IDebugDraw DebugDraw { get { return _debugDraw; } set { _debugDraw = value; } } |
53 | } | 53 | } |
54 | } | 54 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs index 89618af..fddd8af 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs | |||
@@ -1,36 +1,36 @@ | |||
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 | public interface IBroadphase | 29 | public interface IBroadphase |
30 | { | 30 | { |
31 | BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask); | 31 | BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask); |
32 | void DestroyProxy(BroadphaseProxy proxy); | 32 | void DestroyProxy(BroadphaseProxy proxy); |
33 | void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax); | 33 | void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax); |
34 | void CleanProxyFromPairs(BroadphaseProxy proxy); | 34 | void CleanProxyFromPairs(BroadphaseProxy proxy); |
35 | } | 35 | } |
36 | } | 36 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs index 52831b7..7677179 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs | |||
@@ -1,42 +1,42 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX | 26 | namespace XnaDevRu.BulletX |
27 | { | 27 | { |
28 | public interface IDispatcher | 28 | public interface IDispatcher |
29 | { | 29 | { |
30 | CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold); | 30 | CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold); |
31 | CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB); | 31 | CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB); |
32 | PersistentManifold GetNewManifold(object bodyA, object bodyB); | 32 | PersistentManifold GetNewManifold(object bodyA, object bodyB); |
33 | void ReleaseManifold(PersistentManifold manifold); | 33 | void ReleaseManifold(PersistentManifold manifold); |
34 | void ClearManifold(PersistentManifold manifold); | 34 | void ClearManifold(PersistentManifold manifold); |
35 | bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB); | 35 | bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB); |
36 | bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB); | 36 | bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB); |
37 | void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo); | 37 | void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo); |
38 | PersistentManifold GetManifoldByIndex(int index); | 38 | PersistentManifold GetManifoldByIndex(int index); |
39 | 39 | ||
40 | int ManifoldCount { get; } | 40 | int ManifoldCount { get; } |
41 | } | 41 | } |
42 | } | 42 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs index 4e128da..2456911 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs | |||
@@ -1,33 +1,33 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX | 26 | namespace XnaDevRu.BulletX |
27 | { | 27 | { |
28 | public interface IOverlapCallback | 28 | public interface IOverlapCallback |
29 | { | 29 | { |
30 | //return true for deletion of the pair | 30 | //return true for deletion of the pair |
31 | bool ProcessOverlap(ref BroadphasePair pair); | 31 | bool ProcessOverlap(ref BroadphasePair pair); |
32 | } | 32 | } |
33 | } | 33 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs index 165912a..6548560 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs | |||
@@ -1,159 +1,159 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX | 26 | namespace XnaDevRu.BulletX |
27 | { | 27 | { |
28 | public abstract class OverlappingPairCache : IBroadphase | 28 | public abstract class OverlappingPairCache : IBroadphase |
29 | { | 29 | { |
30 | private static int _overlappingPairCount = 0; | 30 | private static int _overlappingPairCount = 0; |
31 | private List<BroadphasePair> _overlappingPairs = new List<BroadphasePair>(); | 31 | private List<BroadphasePair> _overlappingPairs = new List<BroadphasePair>(); |
32 | //during the dispatch, check that user doesn't destroy/create proxy | 32 | //during the dispatch, check that user doesn't destroy/create proxy |
33 | private bool _blockedForChanges; | 33 | private bool _blockedForChanges; |
34 | 34 | ||
35 | public List<BroadphasePair> OverlappingPairs { get { return _overlappingPairs; } set { _overlappingPairs = value; } } | 35 | public List<BroadphasePair> OverlappingPairs { get { return _overlappingPairs; } set { _overlappingPairs = value; } } |
36 | public bool BlockedForChanges { get { return _blockedForChanges; } set { _blockedForChanges = value; } } | 36 | public bool BlockedForChanges { get { return _blockedForChanges; } set { _blockedForChanges = value; } } |
37 | 37 | ||
38 | public static int OverlappingPairCount { get { return _overlappingPairCount; } set { _overlappingPairCount = value; } } | 38 | public static int OverlappingPairCount { get { return _overlappingPairCount; } set { _overlappingPairCount = value; } } |
39 | 39 | ||
40 | public void RemoveOverlappingPair(BroadphasePair pair) | 40 | public void RemoveOverlappingPair(BroadphasePair pair) |
41 | { | 41 | { |
42 | if (!_overlappingPairs.Contains(pair)) | 42 | if (!_overlappingPairs.Contains(pair)) |
43 | return; | 43 | return; |
44 | 44 | ||
45 | CleanOverlappingPair(ref pair); | 45 | CleanOverlappingPair(ref pair); |
46 | _overlappingPairs.Remove(pair); | 46 | _overlappingPairs.Remove(pair); |
47 | } | 47 | } |
48 | 48 | ||
49 | public void AddOverlappingPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) | 49 | public void AddOverlappingPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) |
50 | { | 50 | { |
51 | //don't add overlap with own | 51 | //don't add overlap with own |
52 | bool test = proxyA != proxyB; | 52 | bool test = proxyA != proxyB; |
53 | BulletDebug.Assert(proxyA != proxyB); | 53 | BulletDebug.Assert(proxyA != proxyB); |
54 | 54 | ||
55 | if (!NeedsBroadphaseCollision(proxyA, proxyB)) | 55 | if (!NeedsBroadphaseCollision(proxyA, proxyB)) |
56 | return; | 56 | return; |
57 | 57 | ||
58 | BroadphasePair pair = new BroadphasePair(proxyA, proxyB); | 58 | BroadphasePair pair = new BroadphasePair(proxyA, proxyB); |
59 | _overlappingPairs.Add(pair); | 59 | _overlappingPairs.Add(pair); |
60 | _overlappingPairCount++; | 60 | _overlappingPairCount++; |
61 | } | 61 | } |
62 | 62 | ||
63 | //this FindPair becomes really slow. Either sort the list to speedup the query, or | 63 | //this FindPair becomes really slow. Either sort the list to speedup the query, or |
64 | //use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed. | 64 | //use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed. |
65 | //we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address) | 65 | //we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address) |
66 | //Also we can use a 2D bitmap, which can be useful for a future GPU implementation | 66 | //Also we can use a 2D bitmap, which can be useful for a future GPU implementation |
67 | public BroadphasePair FindPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) | 67 | public BroadphasePair FindPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) |
68 | { | 68 | { |
69 | if (!NeedsBroadphaseCollision(proxyA, proxyB)) | 69 | if (!NeedsBroadphaseCollision(proxyA, proxyB)) |
70 | return null; | 70 | return null; |
71 | 71 | ||
72 | BroadphasePair pair = new BroadphasePair(proxyA, proxyB); | 72 | BroadphasePair pair = new BroadphasePair(proxyA, proxyB); |
73 | for (int i = 0; i < _overlappingPairs.Count; i++) | 73 | for (int i = 0; i < _overlappingPairs.Count; i++) |
74 | { | 74 | { |
75 | if (_overlappingPairs[i] == pair) | 75 | if (_overlappingPairs[i] == pair) |
76 | { | 76 | { |
77 | return _overlappingPairs[i]; | 77 | return _overlappingPairs[i]; |
78 | } | 78 | } |
79 | } | 79 | } |
80 | 80 | ||
81 | return null; | 81 | return null; |
82 | } | 82 | } |
83 | 83 | ||
84 | public void CleanProxyFromPairs(BroadphaseProxy proxy) | 84 | public void CleanProxyFromPairs(BroadphaseProxy proxy) |
85 | { | 85 | { |
86 | for (int i = 0; i < _overlappingPairs.Count; i++) | 86 | for (int i = 0; i < _overlappingPairs.Count; i++) |
87 | { | 87 | { |
88 | BroadphasePair pair = _overlappingPairs[i]; | 88 | BroadphasePair pair = _overlappingPairs[i]; |
89 | if (pair.ProxyA == proxy || | 89 | if (pair.ProxyA == proxy || |
90 | pair.ProxyB == proxy) | 90 | pair.ProxyB == proxy) |
91 | { | 91 | { |
92 | CleanOverlappingPair(ref pair); | 92 | CleanOverlappingPair(ref pair); |
93 | _overlappingPairs[i] = pair; | 93 | _overlappingPairs[i] = pair; |
94 | } | 94 | } |
95 | } | 95 | } |
96 | } | 96 | } |
97 | 97 | ||
98 | public void RemoveOverlappingPairsContainingProxy(BroadphaseProxy proxy) | 98 | public void RemoveOverlappingPairsContainingProxy(BroadphaseProxy proxy) |
99 | { | 99 | { |
100 | for (int i = _overlappingPairs.Count - 1; i >= 0; i--) | 100 | for (int i = _overlappingPairs.Count - 1; i >= 0; i--) |
101 | { | 101 | { |
102 | BroadphasePair pair = _overlappingPairs[i]; | 102 | BroadphasePair pair = _overlappingPairs[i]; |
103 | if (pair.ProxyA == proxy || | 103 | if (pair.ProxyA == proxy || |
104 | pair.ProxyB == proxy) | 104 | pair.ProxyB == proxy) |
105 | { | 105 | { |
106 | RemoveOverlappingPair(pair); | 106 | RemoveOverlappingPair(pair); |
107 | i++; | 107 | i++; |
108 | } | 108 | } |
109 | } | 109 | } |
110 | } | 110 | } |
111 | 111 | ||
112 | public bool NeedsBroadphaseCollision(BroadphaseProxy proxy0, BroadphaseProxy proxy1) | 112 | public bool NeedsBroadphaseCollision(BroadphaseProxy proxy0, BroadphaseProxy proxy1) |
113 | { | 113 | { |
114 | bool collides = (proxy0.CollisionFilterGroup & proxy1.CollisionFilterMask) != 0; | 114 | bool collides = (proxy0.CollisionFilterGroup & proxy1.CollisionFilterMask) != 0; |
115 | collides = collides && ((proxy1.CollisionFilterGroup & proxy0.CollisionFilterMask) != 0); | 115 | collides = collides && ((proxy1.CollisionFilterGroup & proxy0.CollisionFilterMask) != 0); |
116 | 116 | ||
117 | return collides; | 117 | return collides; |
118 | } | 118 | } |
119 | 119 | ||
120 | public virtual void ProcessAllOverlappingPairs(IOverlapCallback callback) | 120 | public virtual void ProcessAllOverlappingPairs(IOverlapCallback callback) |
121 | { | 121 | { |
122 | List<BroadphasePair> deleting = new List<BroadphasePair>(); | 122 | List<BroadphasePair> deleting = new List<BroadphasePair>(); |
123 | for (int i = 0; i < _overlappingPairs.Count; i++) | 123 | for (int i = 0; i < _overlappingPairs.Count; i++) |
124 | { | 124 | { |
125 | BroadphasePair p = _overlappingPairs[i]; | 125 | BroadphasePair p = _overlappingPairs[i]; |
126 | if (callback.ProcessOverlap(ref p)) | 126 | if (callback.ProcessOverlap(ref p)) |
127 | { | 127 | { |
128 | CleanOverlappingPair(ref p); | 128 | CleanOverlappingPair(ref p); |
129 | deleting.Add(p); | 129 | deleting.Add(p); |
130 | _overlappingPairCount--; | 130 | _overlappingPairCount--; |
131 | } | 131 | } |
132 | } | 132 | } |
133 | 133 | ||
134 | for (int i = 0; i < deleting.Count; i++) | 134 | for (int i = 0; i < deleting.Count; i++) |
135 | _overlappingPairs.Remove(deleting[i]); | 135 | _overlappingPairs.Remove(deleting[i]); |
136 | } | 136 | } |
137 | 137 | ||
138 | public void CleanOverlappingPair(ref BroadphasePair pair) | 138 | public void CleanOverlappingPair(ref BroadphasePair pair) |
139 | { | 139 | { |
140 | if (pair.CollisionAlgorithm != null) | 140 | if (pair.CollisionAlgorithm != null) |
141 | { | 141 | { |
142 | if (pair.CollisionAlgorithm is IDisposable) | 142 | if (pair.CollisionAlgorithm is IDisposable) |
143 | (pair.CollisionAlgorithm as IDisposable).Dispose(); | 143 | (pair.CollisionAlgorithm as IDisposable).Dispose(); |
144 | pair.CollisionAlgorithm = null; | 144 | pair.CollisionAlgorithm = null; |
145 | } | 145 | } |
146 | } | 146 | } |
147 | 147 | ||
148 | public abstract void RefreshOverlappingPairs(); | 148 | public abstract void RefreshOverlappingPairs(); |
149 | 149 | ||
150 | #region IBroadphase Members | 150 | #region IBroadphase Members |
151 | public abstract BroadphaseProxy CreateProxy(MonoXnaCompactMaths.Vector3 min, MonoXnaCompactMaths.Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask); | 151 | public abstract BroadphaseProxy CreateProxy(MonoXnaCompactMaths.Vector3 min, MonoXnaCompactMaths.Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask); |
152 | 152 | ||
153 | public abstract void DestroyProxy(BroadphaseProxy proxy); | 153 | public abstract void DestroyProxy(BroadphaseProxy proxy); |
154 | 154 | ||
155 | public abstract void SetAabb(BroadphaseProxy proxy, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax); | 155 | public abstract void SetAabb(BroadphaseProxy proxy, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax); |
156 | 156 | ||
157 | #endregion | 157 | #endregion |
158 | } | 158 | } |
159 | } | 159 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs index 1dc3f34..f37b3f5 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs | |||
@@ -1,128 +1,128 @@ | |||
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 | public class SimpleBroadphase : OverlappingPairCache | 29 | public class SimpleBroadphase : OverlappingPairCache |
30 | { | 30 | { |
31 | private int _maxProxies; | 31 | private int _maxProxies; |
32 | private List<SimpleBroadphaseProxy> _proxies = new List<SimpleBroadphaseProxy>(); | 32 | private List<SimpleBroadphaseProxy> _proxies = new List<SimpleBroadphaseProxy>(); |
33 | 33 | ||
34 | public SimpleBroadphase() | 34 | public SimpleBroadphase() |
35 | : this(16384) { } | 35 | : this(16384) { } |
36 | 36 | ||
37 | public SimpleBroadphase(int maxProxies) | 37 | public SimpleBroadphase(int maxProxies) |
38 | : base() | 38 | : base() |
39 | { | 39 | { |
40 | _maxProxies = maxProxies; | 40 | _maxProxies = maxProxies; |
41 | } | 41 | } |
42 | 42 | ||
43 | public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) | 43 | public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) |
44 | { | 44 | { |
45 | if (_proxies.Count >= _maxProxies) | 45 | if (_proxies.Count >= _maxProxies) |
46 | { | 46 | { |
47 | BulletDebug.Assert(false); | 47 | BulletDebug.Assert(false); |
48 | return null; //should never happen, but don't let the game crash ;-) | 48 | return null; //should never happen, but don't let the game crash ;-) |
49 | } | 49 | } |
50 | BulletDebug.Assert(min.X <= max.X && min.Y <= max.Y && min.Z <= max.Z); | 50 | BulletDebug.Assert(min.X <= max.X && min.Y <= max.Y && min.Z <= max.Z); |
51 | 51 | ||
52 | SimpleBroadphaseProxy proxy = new SimpleBroadphaseProxy(min, max, shapeType, userData, collisionFilterGroup, collisionFilterMask); | 52 | SimpleBroadphaseProxy proxy = new SimpleBroadphaseProxy(min, max, shapeType, userData, collisionFilterGroup, collisionFilterMask); |
53 | _proxies.Add(proxy); | 53 | _proxies.Add(proxy); |
54 | 54 | ||
55 | return proxy; | 55 | return proxy; |
56 | } | 56 | } |
57 | 57 | ||
58 | 58 | ||
59 | public override void DestroyProxy(BroadphaseProxy proxy) | 59 | public override void DestroyProxy(BroadphaseProxy proxy) |
60 | { | 60 | { |
61 | RemoveOverlappingPairsContainingProxy(proxy); | 61 | RemoveOverlappingPairsContainingProxy(proxy); |
62 | _proxies.Remove(proxy as SimpleBroadphaseProxy); | 62 | _proxies.Remove(proxy as SimpleBroadphaseProxy); |
63 | } | 63 | } |
64 | 64 | ||
65 | public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax) | 65 | public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax) |
66 | { | 66 | { |
67 | SimpleBroadphaseProxy simpleProxy = GetSimpleProxyFromProxy(proxy); | 67 | SimpleBroadphaseProxy simpleProxy = GetSimpleProxyFromProxy(proxy); |
68 | simpleProxy.Minimum = aabbMin; | 68 | simpleProxy.Minimum = aabbMin; |
69 | simpleProxy.Maximum = aabbMax; | 69 | simpleProxy.Maximum = aabbMax; |
70 | } | 70 | } |
71 | 71 | ||
72 | private SimpleBroadphaseProxy GetSimpleProxyFromProxy(BroadphaseProxy proxy) | 72 | private SimpleBroadphaseProxy GetSimpleProxyFromProxy(BroadphaseProxy proxy) |
73 | { | 73 | { |
74 | return proxy as SimpleBroadphaseProxy; | 74 | return proxy as SimpleBroadphaseProxy; |
75 | } | 75 | } |
76 | 76 | ||
77 | public override void RefreshOverlappingPairs() | 77 | public override void RefreshOverlappingPairs() |
78 | { | 78 | { |
79 | for (int i = 0; i < _proxies.Count; i++) | 79 | for (int i = 0; i < _proxies.Count; i++) |
80 | { | 80 | { |
81 | SimpleBroadphaseProxy proxyA = _proxies[i]; | 81 | SimpleBroadphaseProxy proxyA = _proxies[i]; |
82 | 82 | ||
83 | for (int j = i + 1; j < _proxies.Count; j++) | 83 | for (int j = i + 1; j < _proxies.Count; j++) |
84 | { | 84 | { |
85 | SimpleBroadphaseProxy proxyB = _proxies[j]; | 85 | SimpleBroadphaseProxy proxyB = _proxies[j]; |
86 | 86 | ||
87 | if (AabbOverlap(proxyA, proxyB)) | 87 | if (AabbOverlap(proxyA, proxyB)) |
88 | { | 88 | { |
89 | if (FindPair(proxyA, proxyB) == null) | 89 | if (FindPair(proxyA, proxyB) == null) |
90 | { | 90 | { |
91 | AddOverlappingPair(proxyA, proxyB); | 91 | AddOverlappingPair(proxyA, proxyB); |
92 | } | 92 | } |
93 | } | 93 | } |
94 | } | 94 | } |
95 | } | 95 | } |
96 | 96 | ||
97 | CheckOverlapCallback check = new CheckOverlapCallback(); | 97 | CheckOverlapCallback check = new CheckOverlapCallback(); |
98 | ProcessAllOverlappingPairs(check); | 98 | ProcessAllOverlappingPairs(check); |
99 | } | 99 | } |
100 | 100 | ||
101 | public static bool AabbOverlap(SimpleBroadphaseProxy proxyA, SimpleBroadphaseProxy proxyB) | 101 | public static bool AabbOverlap(SimpleBroadphaseProxy proxyA, SimpleBroadphaseProxy proxyB) |
102 | { | 102 | { |
103 | return proxyA.Minimum.X <= proxyB.Maximum.X && proxyB.Minimum.X <= proxyA.Maximum.X && | 103 | return proxyA.Minimum.X <= proxyB.Maximum.X && proxyB.Minimum.X <= proxyA.Maximum.X && |
104 | proxyA.Minimum.Y <= proxyB.Maximum.Y && proxyB.Minimum.Y <= proxyA.Maximum.Y && | 104 | proxyA.Minimum.Y <= proxyB.Maximum.Y && proxyB.Minimum.Y <= proxyA.Maximum.Y && |
105 | proxyA.Minimum.Z <= proxyB.Maximum.Z && proxyB.Minimum.Z <= proxyA.Maximum.Z; | 105 | proxyA.Minimum.Z <= proxyB.Maximum.Z && proxyB.Minimum.Z <= proxyA.Maximum.Z; |
106 | } | 106 | } |
107 | 107 | ||
108 | private void Validate() | 108 | private void Validate() |
109 | { | 109 | { |
110 | for (int i = 0; i < _proxies.Count; i++) | 110 | for (int i = 0; i < _proxies.Count; i++) |
111 | { | 111 | { |
112 | for (int j = i + 1; j < _proxies.Count; j++) | 112 | for (int j = i + 1; j < _proxies.Count; j++) |
113 | { | 113 | { |
114 | if (_proxies[i] == _proxies[j]) | 114 | if (_proxies[i] == _proxies[j]) |
115 | throw new BulletException(); | 115 | throw new BulletException(); |
116 | } | 116 | } |
117 | } | 117 | } |
118 | } | 118 | } |
119 | } | 119 | } |
120 | 120 | ||
121 | public class CheckOverlapCallback : IOverlapCallback | 121 | public class CheckOverlapCallback : IOverlapCallback |
122 | { | 122 | { |
123 | public bool ProcessOverlap(ref BroadphasePair pair) | 123 | public bool ProcessOverlap(ref BroadphasePair pair) |
124 | { | 124 | { |
125 | return (!SimpleBroadphase.AabbOverlap(pair.ProxyA as SimpleBroadphaseProxy, pair.ProxyB as SimpleBroadphaseProxy)); | 125 | return (!SimpleBroadphase.AabbOverlap(pair.ProxyA as SimpleBroadphaseProxy, pair.ProxyB as SimpleBroadphaseProxy)); |
126 | } | 126 | } |
127 | } | 127 | } |
128 | } | 128 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs index f473ef2..cfc2150 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs | |||
@@ -1,46 +1,46 @@ | |||
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 | public class SimpleBroadphaseProxy : BroadphaseProxy | 29 | public class SimpleBroadphaseProxy : BroadphaseProxy |
30 | { | 30 | { |
31 | private Vector3 _min; | 31 | private Vector3 _min; |
32 | private Vector3 _max; | 32 | private Vector3 _max; |
33 | 33 | ||
34 | public SimpleBroadphaseProxy() { } | 34 | public SimpleBroadphaseProxy() { } |
35 | 35 | ||
36 | public SimpleBroadphaseProxy(Vector3 minPoint, Vector3 maxPoint, BroadphaseNativeTypes shapeType, object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask) | 36 | public SimpleBroadphaseProxy(Vector3 minPoint, Vector3 maxPoint, BroadphaseNativeTypes shapeType, object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask) |
37 | : base(userData, collisionFilterGroup, collisionFilterMask) | 37 | : base(userData, collisionFilterGroup, collisionFilterMask) |
38 | { | 38 | { |
39 | _min = minPoint; | 39 | _min = minPoint; |
40 | _max = maxPoint; | 40 | _max = maxPoint; |
41 | } | 41 | } |
42 | 42 | ||
43 | public Vector3 Minimum { get { return _min; } set { _min = value; } } | 43 | public Vector3 Minimum { get { return _min; } set { _min = value; } } |
44 | public Vector3 Maximum { get { return _max; } set { _max = value; } } | 44 | public Vector3 Maximum { get { return _max; } set { _max = value; } } |
45 | } | 45 | } |
46 | } | 46 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs index ba638d1..6bebf93 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs | |||
@@ -1,59 +1,59 @@ | |||
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 | internal class BridgeTriangleRaycastCallback : TriangleRaycastCallback | 29 | internal class BridgeTriangleRaycastCallback : TriangleRaycastCallback |
30 | { | 30 | { |
31 | CollisionWorld.RayResultCallback _resultCallback; | 31 | CollisionWorld.RayResultCallback _resultCallback; |
32 | CollisionObject _collisionObject; | 32 | CollisionObject _collisionObject; |
33 | TriangleMeshShape _triangleMesh; | 33 | TriangleMeshShape _triangleMesh; |
34 | 34 | ||
35 | public BridgeTriangleRaycastCallback(Vector3 from, Vector3 to, | 35 | public BridgeTriangleRaycastCallback(Vector3 from, Vector3 to, |
36 | CollisionWorld.RayResultCallback resultCallback, CollisionObject collisionObject, TriangleMeshShape triangleMesh) | 36 | CollisionWorld.RayResultCallback resultCallback, CollisionObject collisionObject, TriangleMeshShape triangleMesh) |
37 | : base(from, to) | 37 | : base(from, to) |
38 | { | 38 | { |
39 | _resultCallback = resultCallback; | 39 | _resultCallback = resultCallback; |
40 | _collisionObject = collisionObject; | 40 | _collisionObject = collisionObject; |
41 | _triangleMesh = triangleMesh; | 41 | _triangleMesh = triangleMesh; |
42 | } | 42 | } |
43 | 43 | ||
44 | public override float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex) | 44 | public override float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex) |
45 | { | 45 | { |
46 | CollisionWorld.LocalShapeInfo shapeInfo = new CollisionWorld.LocalShapeInfo(); | 46 | CollisionWorld.LocalShapeInfo shapeInfo = new CollisionWorld.LocalShapeInfo(); |
47 | shapeInfo.ShapePart = partId; | 47 | shapeInfo.ShapePart = partId; |
48 | shapeInfo.TriangleIndex = triangleIndex; | 48 | shapeInfo.TriangleIndex = triangleIndex; |
49 | 49 | ||
50 | CollisionWorld.LocalRayResult rayResult = new CollisionWorld.LocalRayResult | 50 | CollisionWorld.LocalRayResult rayResult = new CollisionWorld.LocalRayResult |
51 | (_collisionObject, | 51 | (_collisionObject, |
52 | shapeInfo, | 52 | shapeInfo, |
53 | hitNormalLocal, | 53 | hitNormalLocal, |
54 | hitFraction); | 54 | hitFraction); |
55 | 55 | ||
56 | return _resultCallback.AddSingleResult(rayResult); | 56 | return _resultCallback.AddSingleResult(rayResult); |
57 | } | 57 | } |
58 | } | 58 | } |
59 | } | 59 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs index 4e3903b..3bd5601 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs | |||
@@ -1,40 +1,40 @@ | |||
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 | public class CollisionAlgorithmCreateFunction | 29 | public class CollisionAlgorithmCreateFunction |
30 | { | 30 | { |
31 | private bool _swapped; | 31 | private bool _swapped; |
32 | 32 | ||
33 | public bool IsSwapped { get { return _swapped; } set { _swapped = value; } } | 33 | public bool IsSwapped { get { return _swapped; } set { _swapped = value; } } |
34 | 34 | ||
35 | public virtual CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) | 35 | public virtual CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) |
36 | { | 36 | { |
37 | return null; | 37 | return null; |
38 | } | 38 | } |
39 | } | 39 | } |
40 | } | 40 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs index 75d35b0..336defd 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs | |||
@@ -1,280 +1,280 @@ | |||
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 | public delegate void NearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo); | 29 | public delegate void NearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo); |
30 | 30 | ||
31 | public class CollisionDispatcher : IDispatcher | 31 | public class CollisionDispatcher : IDispatcher |
32 | { | 32 | { |
33 | private List<PersistentManifold> _manifolds = new List<PersistentManifold>(); | 33 | private List<PersistentManifold> _manifolds = new List<PersistentManifold>(); |
34 | 34 | ||
35 | //private bool _useIslands; | 35 | //private bool _useIslands; |
36 | private NearCallback _nearCallback; | 36 | private NearCallback _nearCallback; |
37 | 37 | ||
38 | //private ManifoldResult _defaultManifoldResult; | 38 | //private ManifoldResult _defaultManifoldResult; |
39 | 39 | ||
40 | private CollisionAlgorithmCreateFunction[,] _doubleDispatch = new CollisionAlgorithmCreateFunction[(int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes, (int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes]; | 40 | private CollisionAlgorithmCreateFunction[,] _doubleDispatch = new CollisionAlgorithmCreateFunction[(int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes, (int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes]; |
41 | 41 | ||
42 | //default CreationFunctions, filling the m_doubleDispatch table | 42 | //default CreationFunctions, filling the m_doubleDispatch table |
43 | private CollisionAlgorithmCreateFunction _convexConvexCreateFunc; | 43 | private CollisionAlgorithmCreateFunction _convexConvexCreateFunc; |
44 | private CollisionAlgorithmCreateFunction _convexConcaveCreateFunc; | 44 | private CollisionAlgorithmCreateFunction _convexConcaveCreateFunc; |
45 | private CollisionAlgorithmCreateFunction _swappedConvexConcaveCreateFunc; | 45 | private CollisionAlgorithmCreateFunction _swappedConvexConcaveCreateFunc; |
46 | private CollisionAlgorithmCreateFunction _compoundCreateFunc; | 46 | private CollisionAlgorithmCreateFunction _compoundCreateFunc; |
47 | private CollisionAlgorithmCreateFunction _swappedCompoundCreateFunc; | 47 | private CollisionAlgorithmCreateFunction _swappedCompoundCreateFunc; |
48 | private CollisionAlgorithmCreateFunction _emptyCreateFunc; | 48 | private CollisionAlgorithmCreateFunction _emptyCreateFunc; |
49 | 49 | ||
50 | private int _count; | 50 | private int _count; |
51 | private static int _manifoldCount = 0; | 51 | private static int _manifoldCount = 0; |
52 | 52 | ||
53 | public CollisionDispatcher() | 53 | public CollisionDispatcher() |
54 | { | 54 | { |
55 | NearCallback = DefaultNearCallback; | 55 | NearCallback = DefaultNearCallback; |
56 | //_useIslands = true; | 56 | //_useIslands = true; |
57 | //default CreationFunctions, filling the m_doubleDispatch table | 57 | //default CreationFunctions, filling the m_doubleDispatch table |
58 | _convexConvexCreateFunc = new ConvexConvexAlgorithm.CreateFunc(); | 58 | _convexConvexCreateFunc = new ConvexConvexAlgorithm.CreateFunc(); |
59 | _convexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.CreateFunc(); | 59 | _convexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.CreateFunc(); |
60 | _swappedConvexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.SwappedCreateFunc(); | 60 | _swappedConvexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.SwappedCreateFunc(); |
61 | _compoundCreateFunc = new CompoundCollisionAlgorithm.CreateFunc(); | 61 | _compoundCreateFunc = new CompoundCollisionAlgorithm.CreateFunc(); |
62 | _swappedCompoundCreateFunc = new CompoundCollisionAlgorithm.SwappedCreateFunc(); | 62 | _swappedCompoundCreateFunc = new CompoundCollisionAlgorithm.SwappedCreateFunc(); |
63 | _emptyCreateFunc = new EmptyAlgorithm.CreateFunc(); | 63 | _emptyCreateFunc = new EmptyAlgorithm.CreateFunc(); |
64 | 64 | ||
65 | for (BroadphaseNativeTypes i = BroadphaseNativeTypes.Box; i < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; i++) | 65 | for (BroadphaseNativeTypes i = BroadphaseNativeTypes.Box; i < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; i++) |
66 | { | 66 | { |
67 | for (BroadphaseNativeTypes j = BroadphaseNativeTypes.Box; j < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; j++) | 67 | for (BroadphaseNativeTypes j = BroadphaseNativeTypes.Box; j < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; j++) |
68 | { | 68 | { |
69 | _doubleDispatch[(int)i, (int)j] = FindCreateFunction(i, j); | 69 | _doubleDispatch[(int)i, (int)j] = FindCreateFunction(i, j); |
70 | if (_doubleDispatch[(int)i, (int)j] == null) | 70 | if (_doubleDispatch[(int)i, (int)j] == null) |
71 | throw new BulletException(); | 71 | throw new BulletException(); |
72 | } | 72 | } |
73 | } | 73 | } |
74 | } | 74 | } |
75 | 75 | ||
76 | public int Count { get { return _count; } set { _count = value; } } | 76 | public int Count { get { return _count; } set { _count = value; } } |
77 | public int ManifoldCount { get { return _manifolds.Count; } } | 77 | public int ManifoldCount { get { return _manifolds.Count; } } |
78 | public List<PersistentManifold> Manifolds { get { return _manifolds; } } | 78 | public List<PersistentManifold> Manifolds { get { return _manifolds; } } |
79 | 79 | ||
80 | public static int GlobalManifoldCount { get { return _manifoldCount; } set { _manifoldCount = value; } } | 80 | public static int GlobalManifoldCount { get { return _manifoldCount; } set { _manifoldCount = value; } } |
81 | 81 | ||
82 | public PersistentManifold GetManifoldByIndex(int index) | 82 | public PersistentManifold GetManifoldByIndex(int index) |
83 | { | 83 | { |
84 | return _manifolds[index]; | 84 | return _manifolds[index]; |
85 | } | 85 | } |
86 | 86 | ||
87 | //registerCollisionCreateFunc allows registration of custom/alternative collision create functions | 87 | //registerCollisionCreateFunc allows registration of custom/alternative collision create functions |
88 | public void RegisterCollisionCreateFunc(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB, CollisionAlgorithmCreateFunction createFunc) | 88 | public void RegisterCollisionCreateFunc(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB, CollisionAlgorithmCreateFunction createFunc) |
89 | { | 89 | { |
90 | _doubleDispatch[(int)proxyTypeA, (int)proxyTypeB] = createFunc; | 90 | _doubleDispatch[(int)proxyTypeA, (int)proxyTypeB] = createFunc; |
91 | } | 91 | } |
92 | 92 | ||
93 | public virtual PersistentManifold GetNewManifold(object bodyA, object bodyB) | 93 | public virtual PersistentManifold GetNewManifold(object bodyA, object bodyB) |
94 | { | 94 | { |
95 | _manifoldCount++; | 95 | _manifoldCount++; |
96 | 96 | ||
97 | CollisionObject body0 = bodyA as CollisionObject; | 97 | CollisionObject body0 = bodyA as CollisionObject; |
98 | CollisionObject body1 = bodyB as CollisionObject; | 98 | CollisionObject body1 = bodyB as CollisionObject; |
99 | 99 | ||
100 | PersistentManifold manifold = new PersistentManifold(body0, body1); | 100 | PersistentManifold manifold = new PersistentManifold(body0, body1); |
101 | _manifolds.Add(manifold); | 101 | _manifolds.Add(manifold); |
102 | 102 | ||
103 | return manifold; | 103 | return manifold; |
104 | } | 104 | } |
105 | 105 | ||
106 | public virtual void ReleaseManifold(PersistentManifold manifold) | 106 | public virtual void ReleaseManifold(PersistentManifold manifold) |
107 | { | 107 | { |
108 | _manifoldCount--; | 108 | _manifoldCount--; |
109 | 109 | ||
110 | ClearManifold(manifold); | 110 | ClearManifold(manifold); |
111 | _manifolds.Remove(manifold); | 111 | _manifolds.Remove(manifold); |
112 | } | 112 | } |
113 | 113 | ||
114 | 114 | ||
115 | public virtual void ClearManifold(PersistentManifold manifold) | 115 | public virtual void ClearManifold(PersistentManifold manifold) |
116 | { | 116 | { |
117 | manifold.ClearManifold(); | 117 | manifold.ClearManifold(); |
118 | } | 118 | } |
119 | 119 | ||
120 | public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB) | 120 | public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB) |
121 | { | 121 | { |
122 | return FindAlgorithm(bodyA, bodyB, null); | 122 | return FindAlgorithm(bodyA, bodyB, null); |
123 | } | 123 | } |
124 | 124 | ||
125 | public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold) | 125 | public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold) |
126 | { | 126 | { |
127 | CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo(); | 127 | CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo(); |
128 | collisionAlgorithmConstructionInfo.Dispatcher = this; | 128 | collisionAlgorithmConstructionInfo.Dispatcher = this; |
129 | collisionAlgorithmConstructionInfo.Manifold = sharedManifold; | 129 | collisionAlgorithmConstructionInfo.Manifold = sharedManifold; |
130 | CollisionAlgorithm collisionAlgorithm = _doubleDispatch[(int)bodyA.CollisionShape.ShapeType, (int)bodyB.CollisionShape.ShapeType].CreateCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB); | 130 | CollisionAlgorithm collisionAlgorithm = _doubleDispatch[(int)bodyA.CollisionShape.ShapeType, (int)bodyB.CollisionShape.ShapeType].CreateCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB); |
131 | return collisionAlgorithm; | 131 | return collisionAlgorithm; |
132 | } | 132 | } |
133 | 133 | ||
134 | /*public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1) | 134 | /*public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1) |
135 | { | 135 | { |
136 | return internalFindAlgorithm(body0, body1, null); | 136 | return internalFindAlgorithm(body0, body1, null); |
137 | } | 137 | } |
138 | 138 | ||
139 | public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold) | 139 | public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold) |
140 | { | 140 | { |
141 | m_count++; | 141 | m_count++; |
142 | 142 | ||
143 | CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo(); | 143 | CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo(); |
144 | ci.m_dispatcher = this; | 144 | ci.m_dispatcher = this; |
145 | 145 | ||
146 | if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConvex()) | 146 | if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConvex()) |
147 | { | 147 | { |
148 | return new ConvexConvexAlgorithm(sharedManifold, ci, body0, body1); | 148 | return new ConvexConvexAlgorithm(sharedManifold, ci, body0, body1); |
149 | } | 149 | } |
150 | 150 | ||
151 | if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConcave()) | 151 | if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConcave()) |
152 | { | 152 | { |
153 | return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, false); | 153 | return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, false); |
154 | } | 154 | } |
155 | 155 | ||
156 | if (body1.getCollisionShape().isConvex() && body0.getCollisionShape().isConcave()) | 156 | if (body1.getCollisionShape().isConvex() && body0.getCollisionShape().isConcave()) |
157 | { | 157 | { |
158 | return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, true); | 158 | return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, true); |
159 | } | 159 | } |
160 | 160 | ||
161 | if (body0.getCollisionShape().isCompound()) | 161 | if (body0.getCollisionShape().isCompound()) |
162 | { | 162 | { |
163 | return new CompoundCollisionAlgorithm(ci, body0, body1, false); | 163 | return new CompoundCollisionAlgorithm(ci, body0, body1, false); |
164 | } | 164 | } |
165 | else | 165 | else |
166 | { | 166 | { |
167 | if (body1.getCollisionShape().isCompound()) | 167 | if (body1.getCollisionShape().isCompound()) |
168 | { | 168 | { |
169 | return new CompoundCollisionAlgorithm(ci, body0, body1, true); | 169 | return new CompoundCollisionAlgorithm(ci, body0, body1, true); |
170 | } | 170 | } |
171 | } | 171 | } |
172 | 172 | ||
173 | //failed to find an algorithm | 173 | //failed to find an algorithm |
174 | return new EmptyAlgorithm(ci); | 174 | return new EmptyAlgorithm(ci); |
175 | }*/ | 175 | }*/ |
176 | 176 | ||
177 | public virtual bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB) | 177 | public virtual bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB) |
178 | { | 178 | { |
179 | if (bodyA == null || bodyB == null) | 179 | if (bodyA == null || bodyB == null) |
180 | throw new BulletException(); | 180 | throw new BulletException(); |
181 | 181 | ||
182 | bool needsCollision = true; | 182 | bool needsCollision = true; |
183 | 183 | ||
184 | //broadphase filtering already deals with this | 184 | //broadphase filtering already deals with this |
185 | /*if ((body0.isStaticObject() || body0.isKinematicObject()) && | 185 | /*if ((body0.isStaticObject() || body0.isKinematicObject()) && |
186 | (body1.isStaticObject() || body1.isKinematicObject())) | 186 | (body1.isStaticObject() || body1.isKinematicObject())) |
187 | { | 187 | { |
188 | printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); | 188 | printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); |
189 | }*/ | 189 | }*/ |
190 | 190 | ||
191 | if ((!bodyA.IsActive) && (!bodyB.IsActive)) | 191 | if ((!bodyA.IsActive) && (!bodyB.IsActive)) |
192 | needsCollision = false; | 192 | needsCollision = false; |
193 | 193 | ||
194 | return needsCollision; | 194 | return needsCollision; |
195 | } | 195 | } |
196 | 196 | ||
197 | public virtual bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB) | 197 | public virtual bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB) |
198 | { | 198 | { |
199 | //here you can do filtering | 199 | //here you can do filtering |
200 | bool hasResponse = bodyA.HasContactResponse && bodyB.HasContactResponse; | 200 | bool hasResponse = bodyA.HasContactResponse && bodyB.HasContactResponse; |
201 | hasResponse = hasResponse && (!bodyA.IsStaticOrKinematicObject || !bodyB.IsStaticOrKinematicObject); | 201 | hasResponse = hasResponse && (!bodyA.IsStaticOrKinematicObject || !bodyB.IsStaticOrKinematicObject); |
202 | return hasResponse; | 202 | return hasResponse; |
203 | } | 203 | } |
204 | 204 | ||
205 | public virtual void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo) | 205 | public virtual void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo) |
206 | { | 206 | { |
207 | CollisionPairCallback collisionCallback = new CollisionPairCallback(dispatchInfo, this); | 207 | CollisionPairCallback collisionCallback = new CollisionPairCallback(dispatchInfo, this); |
208 | pairCache.ProcessAllOverlappingPairs(collisionCallback); | 208 | pairCache.ProcessAllOverlappingPairs(collisionCallback); |
209 | } | 209 | } |
210 | 210 | ||
211 | private CollisionAlgorithmCreateFunction FindCreateFunction(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB) | 211 | private CollisionAlgorithmCreateFunction FindCreateFunction(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB) |
212 | { | 212 | { |
213 | if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConvex(proxyTypeB)) | 213 | if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConvex(proxyTypeB)) |
214 | { | 214 | { |
215 | return _convexConvexCreateFunc; | 215 | return _convexConvexCreateFunc; |
216 | } | 216 | } |
217 | 217 | ||
218 | if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConcave(proxyTypeB)) | 218 | if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConcave(proxyTypeB)) |
219 | { | 219 | { |
220 | return _convexConcaveCreateFunc; | 220 | return _convexConcaveCreateFunc; |
221 | } | 221 | } |
222 | 222 | ||
223 | if (BroadphaseProxy.IsConvex(proxyTypeB) && BroadphaseProxy.IsConcave(proxyTypeA)) | 223 | if (BroadphaseProxy.IsConvex(proxyTypeB) && BroadphaseProxy.IsConcave(proxyTypeA)) |
224 | { | 224 | { |
225 | return _swappedConvexConcaveCreateFunc; | 225 | return _swappedConvexConcaveCreateFunc; |
226 | } | 226 | } |
227 | 227 | ||
228 | if (BroadphaseProxy.IsCompound(proxyTypeA)) | 228 | if (BroadphaseProxy.IsCompound(proxyTypeA)) |
229 | { | 229 | { |
230 | return _compoundCreateFunc; | 230 | return _compoundCreateFunc; |
231 | } | 231 | } |
232 | else | 232 | else |
233 | { | 233 | { |
234 | if (BroadphaseProxy.IsCompound(proxyTypeB)) | 234 | if (BroadphaseProxy.IsCompound(proxyTypeB)) |
235 | { | 235 | { |
236 | return _swappedCompoundCreateFunc; | 236 | return _swappedCompoundCreateFunc; |
237 | } | 237 | } |
238 | } | 238 | } |
239 | 239 | ||
240 | //failed to find an algorithm | 240 | //failed to find an algorithm |
241 | return _emptyCreateFunc; | 241 | return _emptyCreateFunc; |
242 | } | 242 | } |
243 | 243 | ||
244 | public NearCallback NearCallback { get { return _nearCallback; } set { _nearCallback = value; } } | 244 | public NearCallback NearCallback { get { return _nearCallback; } set { _nearCallback = value; } } |
245 | 245 | ||
246 | //by default, Bullet will use this near callback | 246 | //by default, Bullet will use this near callback |
247 | public static void DefaultNearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo) | 247 | public static void DefaultNearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo) |
248 | { | 248 | { |
249 | CollisionObject collisionObjectA = collisionPair.ProxyA.ClientData as CollisionObject; | 249 | CollisionObject collisionObjectA = collisionPair.ProxyA.ClientData as CollisionObject; |
250 | CollisionObject collisionObjectB = collisionPair.ProxyB.ClientData as CollisionObject; | 250 | CollisionObject collisionObjectB = collisionPair.ProxyB.ClientData as CollisionObject; |
251 | 251 | ||
252 | if (dispatcher.NeedsCollision(collisionObjectA, collisionObjectB)) | 252 | if (dispatcher.NeedsCollision(collisionObjectA, collisionObjectB)) |
253 | { | 253 | { |
254 | //dispatcher will keep algorithms persistent in the collision pair | 254 | //dispatcher will keep algorithms persistent in the collision pair |
255 | if (collisionPair.CollisionAlgorithm == null) | 255 | if (collisionPair.CollisionAlgorithm == null) |
256 | { | 256 | { |
257 | collisionPair.CollisionAlgorithm = dispatcher.FindAlgorithm(collisionObjectA, collisionObjectB); | 257 | collisionPair.CollisionAlgorithm = dispatcher.FindAlgorithm(collisionObjectA, collisionObjectB); |
258 | } | 258 | } |
259 | 259 | ||
260 | if (collisionPair.CollisionAlgorithm != null) | 260 | if (collisionPair.CollisionAlgorithm != null) |
261 | { | 261 | { |
262 | ManifoldResult contactPointResult = new ManifoldResult(collisionObjectA, collisionObjectB); | 262 | ManifoldResult contactPointResult = new ManifoldResult(collisionObjectA, collisionObjectB); |
263 | 263 | ||
264 | if (dispatchInfo.DispatchFunction == DispatchFunction.Discrete) | 264 | if (dispatchInfo.DispatchFunction == DispatchFunction.Discrete) |
265 | { | 265 | { |
266 | //discrete collision detection query | 266 | //discrete collision detection query |
267 | collisionPair.CollisionAlgorithm.ProcessCollision(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult); | 267 | collisionPair.CollisionAlgorithm.ProcessCollision(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult); |
268 | } | 268 | } |
269 | else | 269 | else |
270 | { | 270 | { |
271 | //continuous collision detection query, time of impact (toi) | 271 | //continuous collision detection query, time of impact (toi) |
272 | float timeOfImpact = collisionPair.CollisionAlgorithm.CalculateTimeOfImpact(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult); | 272 | float timeOfImpact = collisionPair.CollisionAlgorithm.CalculateTimeOfImpact(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult); |
273 | if (dispatchInfo.TimeOfImpact > timeOfImpact) | 273 | if (dispatchInfo.TimeOfImpact > timeOfImpact) |
274 | dispatchInfo.TimeOfImpact = timeOfImpact; | 274 | dispatchInfo.TimeOfImpact = timeOfImpact; |
275 | } | 275 | } |
276 | } | 276 | } |
277 | } | 277 | } |
278 | } | 278 | } |
279 | } | 279 | } |
280 | } | 280 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs index 4e9cf58..3eae564 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs | |||
@@ -1,163 +1,163 @@ | |||
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 | public enum ActivationState | 29 | public enum ActivationState |
30 | { | 30 | { |
31 | Nothing = 0, | 31 | Nothing = 0, |
32 | Active, | 32 | Active, |
33 | IslandSleeping, | 33 | IslandSleeping, |
34 | WantsDeactivation, | 34 | WantsDeactivation, |
35 | DisableDeactivation, | 35 | DisableDeactivation, |
36 | DisableSimulation, | 36 | DisableSimulation, |
37 | } | 37 | } |
38 | 38 | ||
39 | public enum CollisionOptions | 39 | public enum CollisionOptions |
40 | { | 40 | { |
41 | StaticObject = 1, | 41 | StaticObject = 1, |
42 | KinematicObject = 2, | 42 | KinematicObject = 2, |
43 | NoContactResponse = 4, | 43 | NoContactResponse = 4, |
44 | CustomMaterialCallback = 8,//this allows per-triangle material (friction/restitution) | 44 | CustomMaterialCallback = 8,//this allows per-triangle material (friction/restitution) |
45 | } | 45 | } |
46 | 46 | ||
47 | /// <summary> | 47 | /// <summary> |
48 | /// btCollisionObject can be used to manage collision detection objects. | 48 | /// btCollisionObject can be used to manage collision detection objects. |
49 | /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy. | 49 | /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy. |
50 | /// They can be added to the btCollisionWorld. | 50 | /// They can be added to the btCollisionWorld. |
51 | /// </summary> | 51 | /// </summary> |
52 | public class CollisionObject | 52 | public class CollisionObject |
53 | { | 53 | { |
54 | protected Matrix _worldTransform; | 54 | protected Matrix _worldTransform; |
55 | private BroadphaseProxy _broadphase; | 55 | private BroadphaseProxy _broadphase; |
56 | private CollisionShape _collisionShape; | 56 | private CollisionShape _collisionShape; |
57 | 57 | ||
58 | //m_interpolationWorldTransform is used for CCD and interpolation | 58 | //m_interpolationWorldTransform is used for CCD and interpolation |
59 | //it can be either previous or future (predicted) transform | 59 | //it can be either previous or future (predicted) transform |
60 | private Matrix _interpolationWorldTransform; | 60 | private Matrix _interpolationWorldTransform; |
61 | 61 | ||
62 | private CollisionOptions _collisionFlags; | 62 | private CollisionOptions _collisionFlags; |
63 | 63 | ||
64 | private int _islandTag; | 64 | private int _islandTag; |
65 | private ActivationState _activationState; | 65 | private ActivationState _activationState; |
66 | private float _deactivationTime; | 66 | private float _deactivationTime; |
67 | 67 | ||
68 | private float _friction; | 68 | private float _friction; |
69 | private float _restitution; | 69 | private float _restitution; |
70 | 70 | ||
71 | //users can point to their objects, m_userPointer is not used by Bullet | 71 | //users can point to their objects, m_userPointer is not used by Bullet |
72 | private object _userData; | 72 | private object _userData; |
73 | 73 | ||
74 | //m_internalOwner one is used by optional Bullet high level interface | 74 | //m_internalOwner one is used by optional Bullet high level interface |
75 | private object _internalOwner; | 75 | private object _internalOwner; |
76 | 76 | ||
77 | //time of impact calculation | 77 | //time of impact calculation |
78 | private float _hitFraction; | 78 | private float _hitFraction; |
79 | 79 | ||
80 | //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: | 80 | //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: |
81 | private float _ccdSweptSphereRadius; | 81 | private float _ccdSweptSphereRadius; |
82 | 82 | ||
83 | // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold | 83 | // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold |
84 | private float _ccdSquareMotionThreshold; | 84 | private float _ccdSquareMotionThreshold; |
85 | //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) | 85 | //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) |
86 | //without destroying the continuous interpolated motion (which uses this interpolation velocities) | 86 | //without destroying the continuous interpolated motion (which uses this interpolation velocities) |
87 | private Vector3 _interpolationLinearVelocity; | 87 | private Vector3 _interpolationLinearVelocity; |
88 | private Vector3 _interpolationAngularVelocity; | 88 | private Vector3 _interpolationAngularVelocity; |
89 | 89 | ||
90 | private int _companionID; | 90 | private int _companionID; |
91 | 91 | ||
92 | public CollisionObject() | 92 | public CollisionObject() |
93 | { | 93 | { |
94 | _activationState = ActivationState.Active; | 94 | _activationState = ActivationState.Active; |
95 | _hitFraction = 1; | 95 | _hitFraction = 1; |
96 | } | 96 | } |
97 | 97 | ||
98 | public bool IsStaticObject { get { return (_collisionFlags & CollisionOptions.StaticObject) != 0; } } | 98 | public bool IsStaticObject { get { return (_collisionFlags & CollisionOptions.StaticObject) != 0; } } |
99 | public bool IsKinematicObject { get { return (_collisionFlags & CollisionOptions.KinematicObject) != 0; } } | 99 | public bool IsKinematicObject { get { return (_collisionFlags & CollisionOptions.KinematicObject) != 0; } } |
100 | public bool IsStaticOrKinematicObject { get { return (_collisionFlags & (CollisionOptions.KinematicObject | CollisionOptions.StaticObject)) != 0; } } | 100 | public bool IsStaticOrKinematicObject { get { return (_collisionFlags & (CollisionOptions.KinematicObject | CollisionOptions.StaticObject)) != 0; } } |
101 | 101 | ||
102 | public bool HasContactResponse { get { return (_collisionFlags & CollisionOptions.NoContactResponse) == 0; } } | 102 | public bool HasContactResponse { get { return (_collisionFlags & CollisionOptions.NoContactResponse) == 0; } } |
103 | public bool MergesSimulationIslands | 103 | public bool MergesSimulationIslands |
104 | { | 104 | { |
105 | get | 105 | get |
106 | { | 106 | { |
107 | //static objects, kinematic and object without contact response don't merge islands | 107 | //static objects, kinematic and object without contact response don't merge islands |
108 | return (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject | CollisionOptions.NoContactResponse)) == 0; | 108 | return (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject | CollisionOptions.NoContactResponse)) == 0; |
109 | } | 109 | } |
110 | } | 110 | } |
111 | 111 | ||
112 | public ActivationState ActivationState | 112 | public ActivationState ActivationState |
113 | { | 113 | { |
114 | get { return _activationState; } | 114 | get { return _activationState; } |
115 | set | 115 | set |
116 | { | 116 | { |
117 | if ((_activationState != ActivationState.DisableDeactivation) && (_activationState != ActivationState.DisableSimulation)) | 117 | if ((_activationState != ActivationState.DisableDeactivation) && (_activationState != ActivationState.DisableSimulation)) |
118 | _activationState = value; | 118 | _activationState = value; |
119 | } | 119 | } |
120 | } | 120 | } |
121 | 121 | ||
122 | public bool IsActive { get { return ((ActivationState != ActivationState.IslandSleeping) && (ActivationState != ActivationState.DisableSimulation)); } } | 122 | public bool IsActive { get { return ((ActivationState != ActivationState.IslandSleeping) && (ActivationState != ActivationState.DisableSimulation)); } } |
123 | public float Restitution { get { return _restitution; } set { _restitution = value; } } | 123 | public float Restitution { get { return _restitution; } set { _restitution = value; } } |
124 | public float Friction { get { return _friction; } set { _friction = value; } } | 124 | public float Friction { get { return _friction; } set { _friction = value; } } |
125 | public CollisionShape CollisionShape { get { return _collisionShape; } set { _collisionShape = value; } } | 125 | public CollisionShape CollisionShape { get { return _collisionShape; } set { _collisionShape = value; } } |
126 | public float DeactivationTime { get { return _deactivationTime; } set { _deactivationTime = value; } } | 126 | public float DeactivationTime { get { return _deactivationTime; } set { _deactivationTime = value; } } |
127 | public object Owner { get { return _internalOwner; } protected set { _internalOwner = value; } } | 127 | public object Owner { get { return _internalOwner; } protected set { _internalOwner = value; } } |
128 | public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } } | 128 | public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } } |
129 | public BroadphaseProxy Broadphase { get { return _broadphase; } set { _broadphase = value; } } | 129 | public BroadphaseProxy Broadphase { get { return _broadphase; } set { _broadphase = value; } } |
130 | public Matrix InterpolationWorldTransform { get { return _interpolationWorldTransform; } set { _interpolationWorldTransform = value; } } | 130 | public Matrix InterpolationWorldTransform { get { return _interpolationWorldTransform; } set { _interpolationWorldTransform = value; } } |
131 | public Vector3 InterpolationLinearVelocity { get { return _interpolationLinearVelocity; } protected set { _interpolationLinearVelocity = value; } } | 131 | public Vector3 InterpolationLinearVelocity { get { return _interpolationLinearVelocity; } protected set { _interpolationLinearVelocity = value; } } |
132 | public Vector3 InterpolationAngularVelocity { get { return _interpolationAngularVelocity; } protected set { _interpolationAngularVelocity = value; } } | 132 | public Vector3 InterpolationAngularVelocity { get { return _interpolationAngularVelocity; } protected set { _interpolationAngularVelocity = value; } } |
133 | public int IslandTag { get { return _islandTag; } set { _islandTag = value; } } | 133 | public int IslandTag { get { return _islandTag; } set { _islandTag = value; } } |
134 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } | 134 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } |
135 | public CollisionOptions CollisionFlags { get { return _collisionFlags; } set { _collisionFlags = value; } } | 135 | public CollisionOptions CollisionFlags { get { return _collisionFlags; } set { _collisionFlags = value; } } |
136 | //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm | 136 | //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm |
137 | public float CcdSweptSphereRadius { get { return _ccdSweptSphereRadius; } set { _ccdSweptSphereRadius = value; } } | 137 | public float CcdSweptSphereRadius { get { return _ccdSweptSphereRadius; } set { _ccdSweptSphereRadius = value; } } |
138 | // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold | 138 | // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold |
139 | public float CcdSquareMotionThreshold { get { return _ccdSquareMotionThreshold; } set { _ccdSquareMotionThreshold = value; } } | 139 | public float CcdSquareMotionThreshold { get { return _ccdSquareMotionThreshold; } set { _ccdSquareMotionThreshold = value; } } |
140 | //users can point to their objects, userPointer is not used by Bullet | 140 | //users can point to their objects, userPointer is not used by Bullet |
141 | public object UserData { get { return _userData; } set { _userData = value; } } | 141 | public object UserData { get { return _userData; } set { _userData = value; } } |
142 | public int CompanionID { get { return _companionID; } set { _companionID = value; } } | 142 | public int CompanionID { get { return _companionID; } set { _companionID = value; } } |
143 | 143 | ||
144 | public void ForceActivationState(ActivationState newState) | 144 | public void ForceActivationState(ActivationState newState) |
145 | { | 145 | { |
146 | _activationState = newState; | 146 | _activationState = newState; |
147 | } | 147 | } |
148 | 148 | ||
149 | public void Activate() | 149 | public void Activate() |
150 | { | 150 | { |
151 | Activate(false); | 151 | Activate(false); |
152 | } | 152 | } |
153 | 153 | ||
154 | public void Activate(bool forceActivation) | 154 | public void Activate(bool forceActivation) |
155 | { | 155 | { |
156 | if (forceActivation || (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject)) == 0) | 156 | if (forceActivation || (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject)) == 0) |
157 | { | 157 | { |
158 | ActivationState = ActivationState.Active; | 158 | ActivationState = ActivationState.Active; |
159 | _deactivationTime = 0; | 159 | _deactivationTime = 0; |
160 | } | 160 | } |
161 | } | 161 | } |
162 | } | 162 | } |
163 | } | 163 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs index 7a85731..6ba1c87 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs | |||
@@ -1,48 +1,48 @@ | |||
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 | public class CollisionPairCallback : IOverlapCallback | 29 | public class CollisionPairCallback : IOverlapCallback |
30 | { | 30 | { |
31 | private DispatcherInfo _dispatchInfo; | 31 | private DispatcherInfo _dispatchInfo; |
32 | private CollisionDispatcher _dispatcher; | 32 | private CollisionDispatcher _dispatcher; |
33 | 33 | ||
34 | public CollisionPairCallback(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher) | 34 | public CollisionPairCallback(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher) |
35 | { | 35 | { |
36 | _dispatchInfo = dispatchInfo; | 36 | _dispatchInfo = dispatchInfo; |
37 | _dispatcher = dispatcher; | 37 | _dispatcher = dispatcher; |
38 | } | 38 | } |
39 | 39 | ||
40 | #region IOverlapCallback Members | 40 | #region IOverlapCallback Members |
41 | public bool ProcessOverlap(ref BroadphasePair pair) | 41 | public bool ProcessOverlap(ref BroadphasePair pair) |
42 | { | 42 | { |
43 | _dispatcher.NearCallback(ref pair, _dispatcher, _dispatchInfo); | 43 | _dispatcher.NearCallback(ref pair, _dispatcher, _dispatchInfo); |
44 | return false; | 44 | return false; |
45 | } | 45 | } |
46 | #endregion | 46 | #endregion |
47 | } | 47 | } |
48 | } | 48 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs index 968e219..cb520bb 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs | |||
@@ -1,358 +1,358 @@ | |||
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 | public class CollisionWorld | 29 | public class CollisionWorld |
30 | { | 30 | { |
31 | private List<CollisionObject> _collisionObjects = new List<CollisionObject>(); | 31 | private List<CollisionObject> _collisionObjects = new List<CollisionObject>(); |
32 | private IDispatcher _dispatcher; | 32 | private IDispatcher _dispatcher; |
33 | private OverlappingPairCache _broadphasePairCache; | 33 | private OverlappingPairCache _broadphasePairCache; |
34 | private bool _ownsDispatcher; | 34 | private bool _ownsDispatcher; |
35 | private bool _ownsBroadphasePairCache; | 35 | private bool _ownsBroadphasePairCache; |
36 | private DispatcherInfo _dispatchInfo = new DispatcherInfo(); | 36 | private DispatcherInfo _dispatchInfo = new DispatcherInfo(); |
37 | 37 | ||
38 | /// <summary> | 38 | /// <summary> |
39 | /// this constructor doesn't own the dispatcher and paircache/broadphase | 39 | /// this constructor doesn't own the dispatcher and paircache/broadphase |
40 | /// </summary> | 40 | /// </summary> |
41 | /// <param name="dispatcher"></param> | 41 | /// <param name="dispatcher"></param> |
42 | /// <param name="pairCache"></param> | 42 | /// <param name="pairCache"></param> |
43 | public CollisionWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) | 43 | public CollisionWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) |
44 | { | 44 | { |
45 | _dispatcher = dispatcher; | 45 | _dispatcher = dispatcher; |
46 | _broadphasePairCache = pairCache; | 46 | _broadphasePairCache = pairCache; |
47 | _ownsDispatcher = false; | 47 | _ownsDispatcher = false; |
48 | _ownsBroadphasePairCache = false; | 48 | _ownsBroadphasePairCache = false; |
49 | } | 49 | } |
50 | 50 | ||
51 | public DispatcherInfo DispatchInfo { get { return _dispatchInfo; } protected set { _dispatchInfo = value; } } | 51 | public DispatcherInfo DispatchInfo { get { return _dispatchInfo; } protected set { _dispatchInfo = value; } } |
52 | public List<CollisionObject> CollisionObjects { get { return _collisionObjects; } protected set { _collisionObjects = value; } } | 52 | public List<CollisionObject> CollisionObjects { get { return _collisionObjects; } protected set { _collisionObjects = value; } } |
53 | public IBroadphase Broadphase { get { return _broadphasePairCache; } } | 53 | public IBroadphase Broadphase { get { return _broadphasePairCache; } } |
54 | public OverlappingPairCache BroadphasePairCache { get { return _broadphasePairCache; } protected set { _broadphasePairCache = value; } } | 54 | public OverlappingPairCache BroadphasePairCache { get { return _broadphasePairCache; } protected set { _broadphasePairCache = value; } } |
55 | public IDispatcher Dispatcher { get { return _dispatcher; } protected set { _dispatcher = value; } } | 55 | public IDispatcher Dispatcher { get { return _dispatcher; } protected set { _dispatcher = value; } } |
56 | public int CollisionObjectsCount { get { return _collisionObjects.Count; } } | 56 | public int CollisionObjectsCount { get { return _collisionObjects.Count; } } |
57 | protected bool OwnsDispatcher { get { return _ownsDispatcher; } set { _ownsDispatcher = value; } } | 57 | protected bool OwnsDispatcher { get { return _ownsDispatcher; } set { _ownsDispatcher = value; } } |
58 | protected bool OwnsBroadphasePairCache { get { return _ownsBroadphasePairCache; } set { _ownsBroadphasePairCache = value; } } | 58 | protected bool OwnsBroadphasePairCache { get { return _ownsBroadphasePairCache; } set { _ownsBroadphasePairCache = value; } } |
59 | 59 | ||
60 | // rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback | 60 | // rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback |
61 | // This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback. | 61 | // This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback. |
62 | public void RayTest(Vector3 rayFromWorld, Vector3 rayToWorld, RayResultCallback resultCallback) | 62 | public void RayTest(Vector3 rayFromWorld, Vector3 rayToWorld, RayResultCallback resultCallback) |
63 | { | 63 | { |
64 | Matrix rayFromTrans, rayToTrans; | 64 | Matrix rayFromTrans, rayToTrans; |
65 | 65 | ||
66 | rayFromTrans = Matrix.Identity; | 66 | rayFromTrans = Matrix.Identity; |
67 | rayFromTrans.Translation = rayFromWorld; | 67 | rayFromTrans.Translation = rayFromWorld; |
68 | 68 | ||
69 | rayToTrans = Matrix.Identity; | 69 | rayToTrans = Matrix.Identity; |
70 | rayToTrans.Translation = rayToWorld; | 70 | rayToTrans.Translation = rayToWorld; |
71 | 71 | ||
72 | // brute force go over all objects. Once there is a broadphase, use that, or | 72 | // brute force go over all objects. Once there is a broadphase, use that, or |
73 | // add a raycast against aabb first. | 73 | // add a raycast against aabb first. |
74 | 74 | ||
75 | foreach (CollisionObject collisionObject in _collisionObjects) | 75 | foreach (CollisionObject collisionObject in _collisionObjects) |
76 | { | 76 | { |
77 | //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); | 77 | //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); |
78 | Vector3 collisionObjectAabbMin, collisionObjectAabbMax; | 78 | Vector3 collisionObjectAabbMin, collisionObjectAabbMax; |
79 | collisionObject.CollisionShape.GetAabb(collisionObject.WorldTransform, out collisionObjectAabbMin, out collisionObjectAabbMax); | 79 | collisionObject.CollisionShape.GetAabb(collisionObject.WorldTransform, out collisionObjectAabbMin, out collisionObjectAabbMax); |
80 | 80 | ||
81 | float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing | 81 | float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing |
82 | Vector3 hitNormal = new Vector3(); | 82 | Vector3 hitNormal = new Vector3(); |
83 | 83 | ||
84 | //if (MathHelper.TestAabbAgainstAabb2(rayAabbMin, rayAabbMax, collisionObjectAabbMin, collisionObjectAabbMax)) | 84 | //if (MathHelper.TestAabbAgainstAabb2(rayAabbMin, rayAabbMax, collisionObjectAabbMin, collisionObjectAabbMax)) |
85 | if (MathHelper.RayAabb(rayFromWorld, rayToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) | 85 | if (MathHelper.RayAabb(rayFromWorld, rayToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) |
86 | { | 86 | { |
87 | RayTestSingle(rayFromTrans, rayToTrans, | 87 | RayTestSingle(rayFromTrans, rayToTrans, |
88 | collisionObject, collisionObject.CollisionShape, | 88 | collisionObject, collisionObject.CollisionShape, |
89 | collisionObject.WorldTransform, resultCallback); | 89 | collisionObject.WorldTransform, resultCallback); |
90 | 90 | ||
91 | } | 91 | } |
92 | } | 92 | } |
93 | } | 93 | } |
94 | 94 | ||
95 | // rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. | 95 | // rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. |
96 | // In a future implementation, we consider moving the ray test as a virtual method in CollisionShape. | 96 | // In a future implementation, we consider moving the ray test as a virtual method in CollisionShape. |
97 | // This allows more customization. | 97 | // This allows more customization. |
98 | public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans, | 98 | public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans, |
99 | CollisionObject collisionObject, | 99 | CollisionObject collisionObject, |
100 | CollisionShape collisionShape, | 100 | CollisionShape collisionShape, |
101 | Matrix colObjWorldTransform, | 101 | Matrix colObjWorldTransform, |
102 | RayResultCallback resultCallback) | 102 | RayResultCallback resultCallback) |
103 | { | 103 | { |
104 | SphereShape pointShape=new SphereShape(0.0f); | 104 | SphereShape pointShape=new SphereShape(0.0f); |
105 | 105 | ||
106 | if (collisionShape.IsConvex) | 106 | if (collisionShape.IsConvex) |
107 | { | 107 | { |
108 | CastResult castResult = new CastResult(); | 108 | CastResult castResult = new CastResult(); |
109 | castResult.Fraction = 1f;//?? | 109 | castResult.Fraction = 1f;//?? |
110 | 110 | ||
111 | ConvexShape convexShape = collisionShape as ConvexShape; | 111 | ConvexShape convexShape = collisionShape as ConvexShape; |
112 | VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); | 112 | VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); |
113 | SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver); | 113 | SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver); |
114 | //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); | 114 | //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); |
115 | //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); | 115 | //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); |
116 | 116 | ||
117 | if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) | 117 | if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) |
118 | { | 118 | { |
119 | //add hit | 119 | //add hit |
120 | if (castResult.Normal.LengthSquared() > 0.0001f) | 120 | if (castResult.Normal.LengthSquared() > 0.0001f) |
121 | { | 121 | { |
122 | castResult.Normal.Normalize(); | 122 | castResult.Normal.Normalize(); |
123 | if (castResult.Fraction < resultCallback.ClosestHitFraction) | 123 | if (castResult.Fraction < resultCallback.ClosestHitFraction) |
124 | { | 124 | { |
125 | 125 | ||
126 | CollisionWorld.LocalRayResult localRayResult = new LocalRayResult | 126 | CollisionWorld.LocalRayResult localRayResult = new LocalRayResult |
127 | ( | 127 | ( |
128 | collisionObject, | 128 | collisionObject, |
129 | new LocalShapeInfo(), | 129 | new LocalShapeInfo(), |
130 | castResult.Normal, | 130 | castResult.Normal, |
131 | castResult.Fraction | 131 | castResult.Fraction |
132 | ); | 132 | ); |
133 | 133 | ||
134 | resultCallback.AddSingleResult(localRayResult); | 134 | resultCallback.AddSingleResult(localRayResult); |
135 | } | 135 | } |
136 | } | 136 | } |
137 | } | 137 | } |
138 | else | 138 | else |
139 | { | 139 | { |
140 | if (collisionShape.IsConcave) | 140 | if (collisionShape.IsConcave) |
141 | { | 141 | { |
142 | 142 | ||
143 | TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape; | 143 | TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape; |
144 | 144 | ||
145 | Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform); | 145 | Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform); |
146 | 146 | ||
147 | Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject); | 147 | Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject); |
148 | Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject); | 148 | Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject); |
149 | 149 | ||
150 | BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh); | 150 | BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh); |
151 | rcb.HitFraction = resultCallback.ClosestHitFraction; | 151 | rcb.HitFraction = resultCallback.ClosestHitFraction; |
152 | 152 | ||
153 | Vector3 rayAabbMinLocal = rayFromLocal; | 153 | Vector3 rayAabbMinLocal = rayFromLocal; |
154 | MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal); | 154 | MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal); |
155 | Vector3 rayAabbMaxLocal = rayFromLocal; | 155 | Vector3 rayAabbMaxLocal = rayFromLocal; |
156 | MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal); | 156 | MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal); |
157 | 157 | ||
158 | triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal); | 158 | triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal); |
159 | } | 159 | } |
160 | else | 160 | else |
161 | { | 161 | { |
162 | //todo: use AABB tree or other BVH acceleration structure! | 162 | //todo: use AABB tree or other BVH acceleration structure! |
163 | if (collisionShape.IsCompound) | 163 | if (collisionShape.IsCompound) |
164 | { | 164 | { |
165 | CompoundShape compoundShape = collisionShape as CompoundShape; | 165 | CompoundShape compoundShape = collisionShape as CompoundShape; |
166 | for (int i = 0; i < compoundShape.ChildShapeCount; i++) | 166 | for (int i = 0; i < compoundShape.ChildShapeCount; i++) |
167 | { | 167 | { |
168 | Matrix childTrans = compoundShape.GetChildTransform(i); | 168 | Matrix childTrans = compoundShape.GetChildTransform(i); |
169 | CollisionShape childCollisionShape = compoundShape.GetChildShape(i); | 169 | CollisionShape childCollisionShape = compoundShape.GetChildShape(i); |
170 | Matrix childWorldTrans = colObjWorldTransform * childTrans; | 170 | Matrix childWorldTrans = colObjWorldTransform * childTrans; |
171 | RayTestSingle(rayFromTrans, rayToTrans, | 171 | RayTestSingle(rayFromTrans, rayToTrans, |
172 | collisionObject, | 172 | collisionObject, |
173 | childCollisionShape, | 173 | childCollisionShape, |
174 | childWorldTrans, | 174 | childWorldTrans, |
175 | resultCallback); | 175 | resultCallback); |
176 | } | 176 | } |
177 | } | 177 | } |
178 | } | 178 | } |
179 | } | 179 | } |
180 | } | 180 | } |
181 | } | 181 | } |
182 | 182 | ||
183 | public void AddCollisionObject(CollisionObject collisionObject, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) | 183 | public void AddCollisionObject(CollisionObject collisionObject, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) |
184 | { | 184 | { |
185 | //check that the object isn't already added | 185 | //check that the object isn't already added |
186 | if (!_collisionObjects.Contains(collisionObject)) | 186 | if (!_collisionObjects.Contains(collisionObject)) |
187 | { | 187 | { |
188 | _collisionObjects.Add(collisionObject); | 188 | _collisionObjects.Add(collisionObject); |
189 | 189 | ||
190 | //calculate new AABB | 190 | //calculate new AABB |
191 | Matrix trans = collisionObject.WorldTransform; | 191 | Matrix trans = collisionObject.WorldTransform; |
192 | 192 | ||
193 | Vector3 minAabb; | 193 | Vector3 minAabb; |
194 | Vector3 maxAabb; | 194 | Vector3 maxAabb; |
195 | collisionObject.CollisionShape.GetAabb(trans, out minAabb, out maxAabb); | 195 | collisionObject.CollisionShape.GetAabb(trans, out minAabb, out maxAabb); |
196 | 196 | ||
197 | BroadphaseNativeTypes type = collisionObject.CollisionShape.ShapeType; | 197 | BroadphaseNativeTypes type = collisionObject.CollisionShape.ShapeType; |
198 | collisionObject.Broadphase = Broadphase.CreateProxy( | 198 | collisionObject.Broadphase = Broadphase.CreateProxy( |
199 | minAabb, | 199 | minAabb, |
200 | maxAabb, | 200 | maxAabb, |
201 | type, | 201 | type, |
202 | collisionObject, | 202 | collisionObject, |
203 | collisionFilterGroup, | 203 | collisionFilterGroup, |
204 | collisionFilterMask | 204 | collisionFilterMask |
205 | ); | 205 | ); |
206 | } | 206 | } |
207 | } | 207 | } |
208 | 208 | ||
209 | public void AddCollisionObject(CollisionObject collisionObject) | 209 | public void AddCollisionObject(CollisionObject collisionObject) |
210 | { | 210 | { |
211 | AddCollisionObject(collisionObject, BroadphaseProxy.CollisionFilterGroups.Default, BroadphaseProxy.CollisionFilterGroups.Default); | 211 | AddCollisionObject(collisionObject, BroadphaseProxy.CollisionFilterGroups.Default, BroadphaseProxy.CollisionFilterGroups.Default); |
212 | } | 212 | } |
213 | 213 | ||
214 | public void RemoveCollisionObject(CollisionObject collisionObject) | 214 | public void RemoveCollisionObject(CollisionObject collisionObject) |
215 | { | 215 | { |
216 | BroadphaseProxy bp = collisionObject.Broadphase; | 216 | BroadphaseProxy bp = collisionObject.Broadphase; |
217 | if (bp != null) | 217 | if (bp != null) |
218 | { | 218 | { |
219 | // | 219 | // |
220 | // only clear the cached algorithms | 220 | // only clear the cached algorithms |
221 | // | 221 | // |
222 | Broadphase.CleanProxyFromPairs(bp); | 222 | Broadphase.CleanProxyFromPairs(bp); |
223 | Broadphase.DestroyProxy(bp); | 223 | Broadphase.DestroyProxy(bp); |
224 | collisionObject.Broadphase = null; | 224 | collisionObject.Broadphase = null; |
225 | } | 225 | } |
226 | 226 | ||
227 | _collisionObjects.Remove(collisionObject); | 227 | _collisionObjects.Remove(collisionObject); |
228 | } | 228 | } |
229 | 229 | ||
230 | public virtual void PerformDiscreteCollisionDetection() | 230 | public virtual void PerformDiscreteCollisionDetection() |
231 | { | 231 | { |
232 | DispatcherInfo dispatchInfo = DispatchInfo; | 232 | DispatcherInfo dispatchInfo = DispatchInfo; |
233 | //update aabb (of all moved objects) | 233 | //update aabb (of all moved objects) |
234 | 234 | ||
235 | Vector3 aabbMin, aabbMax; | 235 | Vector3 aabbMin, aabbMax; |
236 | for (int i = 0; i < _collisionObjects.Count; i++) | 236 | for (int i = 0; i < _collisionObjects.Count; i++) |
237 | { | 237 | { |
238 | _collisionObjects[i].CollisionShape.GetAabb(_collisionObjects[i].WorldTransform, out aabbMin, out aabbMax); | 238 | _collisionObjects[i].CollisionShape.GetAabb(_collisionObjects[i].WorldTransform, out aabbMin, out aabbMax); |
239 | _broadphasePairCache.SetAabb(_collisionObjects[i].Broadphase, aabbMin, aabbMax); | 239 | _broadphasePairCache.SetAabb(_collisionObjects[i].Broadphase, aabbMin, aabbMax); |
240 | } | 240 | } |
241 | 241 | ||
242 | _broadphasePairCache.RefreshOverlappingPairs(); | 242 | _broadphasePairCache.RefreshOverlappingPairs(); |
243 | 243 | ||
244 | IDispatcher dispatcher = Dispatcher; | 244 | IDispatcher dispatcher = Dispatcher; |
245 | if (dispatcher != null) | 245 | if (dispatcher != null) |
246 | dispatcher.DispatchAllCollisionPairs(_broadphasePairCache, dispatchInfo); | 246 | dispatcher.DispatchAllCollisionPairs(_broadphasePairCache, dispatchInfo); |
247 | } | 247 | } |
248 | 248 | ||
249 | public void Dispose(bool disposing) | 249 | public void Dispose(bool disposing) |
250 | { | 250 | { |
251 | if (disposing) | 251 | if (disposing) |
252 | { | 252 | { |
253 | //clean up remaining objects | 253 | //clean up remaining objects |
254 | foreach (CollisionObject collisionObject in _collisionObjects) | 254 | foreach (CollisionObject collisionObject in _collisionObjects) |
255 | { | 255 | { |
256 | BroadphaseProxy bp = collisionObject.Broadphase; | 256 | BroadphaseProxy bp = collisionObject.Broadphase; |
257 | if (bp != null) | 257 | if (bp != null) |
258 | { | 258 | { |
259 | // | 259 | // |
260 | // only clear the cached algorithms | 260 | // only clear the cached algorithms |
261 | // | 261 | // |
262 | Broadphase.CleanProxyFromPairs(bp); | 262 | Broadphase.CleanProxyFromPairs(bp); |
263 | Broadphase.DestroyProxy(bp); | 263 | Broadphase.DestroyProxy(bp); |
264 | } | 264 | } |
265 | } | 265 | } |
266 | } | 266 | } |
267 | } | 267 | } |
268 | 268 | ||
269 | /// <summary> | 269 | /// <summary> |
270 | /// LocalShapeInfo gives extra information for complex shapes | 270 | /// LocalShapeInfo gives extra information for complex shapes |
271 | /// Currently, only TriangleMeshShape is available, so it just contains triangleIndex and subpart | 271 | /// Currently, only TriangleMeshShape is available, so it just contains triangleIndex and subpart |
272 | /// </summary> | 272 | /// </summary> |
273 | public struct LocalShapeInfo | 273 | public struct LocalShapeInfo |
274 | { | 274 | { |
275 | private int _shapePart; | 275 | private int _shapePart; |
276 | private int _triangleIndex; | 276 | private int _triangleIndex; |
277 | 277 | ||
278 | public int ShapePart { get { return _shapePart; } set { _shapePart = value; } } | 278 | public int ShapePart { get { return _shapePart; } set { _shapePart = value; } } |
279 | public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } | 279 | public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } |
280 | } | 280 | } |
281 | 281 | ||
282 | public struct LocalRayResult | 282 | public struct LocalRayResult |
283 | { | 283 | { |
284 | private CollisionObject _collisionObject; | 284 | private CollisionObject _collisionObject; |
285 | private LocalShapeInfo _localShapeInfo; | 285 | private LocalShapeInfo _localShapeInfo; |
286 | private Vector3 _hitNormalLocal; | 286 | private Vector3 _hitNormalLocal; |
287 | private float _hitFraction; | 287 | private float _hitFraction; |
288 | 288 | ||
289 | public LocalRayResult(CollisionObject collisionObject, | 289 | public LocalRayResult(CollisionObject collisionObject, |
290 | LocalShapeInfo localShapeInfo, | 290 | LocalShapeInfo localShapeInfo, |
291 | Vector3 hitNormalLocal, | 291 | Vector3 hitNormalLocal, |
292 | float hitFraction) | 292 | float hitFraction) |
293 | { | 293 | { |
294 | _collisionObject = collisionObject; | 294 | _collisionObject = collisionObject; |
295 | _localShapeInfo = localShapeInfo; | 295 | _localShapeInfo = localShapeInfo; |
296 | _hitNormalLocal = hitNormalLocal; | 296 | _hitNormalLocal = hitNormalLocal; |
297 | _hitFraction = hitFraction; | 297 | _hitFraction = hitFraction; |
298 | } | 298 | } |
299 | 299 | ||
300 | public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } } | 300 | public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } } |
301 | public LocalShapeInfo LocalShapeInfo { get { return _localShapeInfo; } set { _localShapeInfo = value; } } | 301 | public LocalShapeInfo LocalShapeInfo { get { return _localShapeInfo; } set { _localShapeInfo = value; } } |
302 | public Vector3 HitNormalLocal { get { return _hitNormalLocal; } set { _hitNormalLocal = value; } } | 302 | public Vector3 HitNormalLocal { get { return _hitNormalLocal; } set { _hitNormalLocal = value; } } |
303 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } | 303 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } |
304 | } | 304 | } |
305 | 305 | ||
306 | /// <summary> | 306 | /// <summary> |
307 | /// RayResultCallback is used to report new raycast results | 307 | /// RayResultCallback is used to report new raycast results |
308 | /// </summary> | 308 | /// </summary> |
309 | public abstract class RayResultCallback | 309 | public abstract class RayResultCallback |
310 | { | 310 | { |
311 | private float _closestHitFraction; | 311 | private float _closestHitFraction; |
312 | 312 | ||
313 | public RayResultCallback() | 313 | public RayResultCallback() |
314 | { | 314 | { |
315 | _closestHitFraction = 1; | 315 | _closestHitFraction = 1; |
316 | } | 316 | } |
317 | 317 | ||
318 | public float ClosestHitFraction { get { return _closestHitFraction; } set { _closestHitFraction = value; } } | 318 | public float ClosestHitFraction { get { return _closestHitFraction; } set { _closestHitFraction = value; } } |
319 | public bool HasHit { get { return _closestHitFraction < 1; } } | 319 | public bool HasHit { get { return _closestHitFraction < 1; } } |
320 | 320 | ||
321 | public abstract float AddSingleResult(LocalRayResult rayResult); | 321 | public abstract float AddSingleResult(LocalRayResult rayResult); |
322 | } | 322 | } |
323 | 323 | ||
324 | public class ClosestRayResultCallback : RayResultCallback | 324 | public class ClosestRayResultCallback : RayResultCallback |
325 | { | 325 | { |
326 | private Vector3 _rayFromWorld;//used to calculate hitPointWorld from hitFraction | 326 | private Vector3 _rayFromWorld;//used to calculate hitPointWorld from hitFraction |
327 | private Vector3 _rayToWorld; | 327 | private Vector3 _rayToWorld; |
328 | 328 | ||
329 | private Vector3 _hitNormalWorld; | 329 | private Vector3 _hitNormalWorld; |
330 | private Vector3 _hitPointWorld; | 330 | private Vector3 _hitPointWorld; |
331 | private CollisionObject _collisionObject; | 331 | private CollisionObject _collisionObject; |
332 | 332 | ||
333 | public ClosestRayResultCallback(Vector3 rayFromWorld, Vector3 rayToWorld) | 333 | public ClosestRayResultCallback(Vector3 rayFromWorld, Vector3 rayToWorld) |
334 | { | 334 | { |
335 | _rayFromWorld = rayFromWorld; | 335 | _rayFromWorld = rayFromWorld; |
336 | _rayToWorld = rayToWorld; | 336 | _rayToWorld = rayToWorld; |
337 | _collisionObject = null; | 337 | _collisionObject = null; |
338 | } | 338 | } |
339 | 339 | ||
340 | public Vector3 RayFromWorld { get { return _rayFromWorld; } set { _rayFromWorld = value; } } | 340 | public Vector3 RayFromWorld { get { return _rayFromWorld; } set { _rayFromWorld = value; } } |
341 | public Vector3 RayToWorld { get { return _rayToWorld; } set { _rayToWorld = value; } } | 341 | public Vector3 RayToWorld { get { return _rayToWorld; } set { _rayToWorld = value; } } |
342 | public Vector3 HitNormalWorld { get { return _hitNormalWorld; } set { _hitNormalWorld = value; } } | 342 | public Vector3 HitNormalWorld { get { return _hitNormalWorld; } set { _hitNormalWorld = value; } } |
343 | public Vector3 HitPointWorld { get { return _hitPointWorld; } set { _hitPointWorld = value; } } | 343 | public Vector3 HitPointWorld { get { return _hitPointWorld; } set { _hitPointWorld = value; } } |
344 | public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } } | 344 | public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } } |
345 | 345 | ||
346 | public override float AddSingleResult(LocalRayResult rayResult) | 346 | public override float AddSingleResult(LocalRayResult rayResult) |
347 | { | 347 | { |
348 | //caller already does the filter on the m_closestHitFraction | 348 | //caller already does the filter on the m_closestHitFraction |
349 | //assert(rayResult.m_hitFraction <= m_closestHitFraction); | 349 | //assert(rayResult.m_hitFraction <= m_closestHitFraction); |
350 | ClosestHitFraction = rayResult.HitFraction; | 350 | ClosestHitFraction = rayResult.HitFraction; |
351 | _collisionObject = rayResult.CollisionObject; | 351 | _collisionObject = rayResult.CollisionObject; |
352 | _hitNormalWorld = Vector3.TransformNormal(rayResult.HitNormalLocal, _collisionObject.WorldTransform); | 352 | _hitNormalWorld = Vector3.TransformNormal(rayResult.HitNormalLocal, _collisionObject.WorldTransform); |
353 | MathHelper.SetInterpolate3(_rayFromWorld, _rayToWorld, rayResult.HitFraction, ref _hitPointWorld); | 353 | MathHelper.SetInterpolate3(_rayFromWorld, _rayToWorld, rayResult.HitFraction, ref _hitPointWorld); |
354 | return rayResult.HitFraction; | 354 | return rayResult.HitFraction; |
355 | } | 355 | } |
356 | } | 356 | } |
357 | } | 357 | } |
358 | } | 358 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs index 3fec7d0..6f2278a 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs | |||
@@ -1,157 +1,157 @@ | |||
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 System.Diagnostics; | 25 | using System.Diagnostics; |
26 | using MonoXnaCompactMaths; | 26 | using MonoXnaCompactMaths; |
27 | 27 | ||
28 | namespace XnaDevRu.BulletX | 28 | namespace XnaDevRu.BulletX |
29 | { | 29 | { |
30 | public class CompoundCollisionAlgorithm : CollisionAlgorithm | 30 | public class CompoundCollisionAlgorithm : CollisionAlgorithm |
31 | { | 31 | { |
32 | private List<CollisionAlgorithm> _childCollisionAlgorithms; | 32 | private List<CollisionAlgorithm> _childCollisionAlgorithms; |
33 | private bool _isSwapped; | 33 | private bool _isSwapped; |
34 | 34 | ||
35 | public CompoundCollisionAlgorithm( | 35 | public CompoundCollisionAlgorithm( |
36 | CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, | 36 | CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, |
37 | CollisionObject bodyA, | 37 | CollisionObject bodyA, |
38 | CollisionObject bodyB, bool isSwapped) | 38 | CollisionObject bodyB, bool isSwapped) |
39 | : base(collisionAlgorithmConstructionInfo) | 39 | : base(collisionAlgorithmConstructionInfo) |
40 | { | 40 | { |
41 | //Begin | 41 | //Begin |
42 | _isSwapped = isSwapped; | 42 | _isSwapped = isSwapped; |
43 | 43 | ||
44 | CollisionObject collisionObject = isSwapped ? bodyB : bodyA; | 44 | CollisionObject collisionObject = isSwapped ? bodyB : bodyA; |
45 | CollisionObject otherObject = isSwapped ? bodyA : bodyB; | 45 | CollisionObject otherObject = isSwapped ? bodyA : bodyB; |
46 | 46 | ||
47 | BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); | 47 | BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); |
48 | 48 | ||
49 | CompoundShape compoundShape = collisionObject.CollisionShape as CompoundShape; | 49 | CompoundShape compoundShape = collisionObject.CollisionShape as CompoundShape; |
50 | int childrenNumber = compoundShape.ChildShapeCount; | 50 | int childrenNumber = compoundShape.ChildShapeCount; |
51 | int index = 0; | 51 | int index = 0; |
52 | 52 | ||
53 | _childCollisionAlgorithms = new List<CollisionAlgorithm>(childrenNumber); | 53 | _childCollisionAlgorithms = new List<CollisionAlgorithm>(childrenNumber); |
54 | 54 | ||
55 | for (index = 0; index < childrenNumber; index++) | 55 | for (index = 0; index < childrenNumber; index++) |
56 | { | 56 | { |
57 | CollisionShape childShape = compoundShape.GetChildShape(index); | 57 | CollisionShape childShape = compoundShape.GetChildShape(index); |
58 | CollisionShape orgShape = collisionObject.CollisionShape; | 58 | CollisionShape orgShape = collisionObject.CollisionShape; |
59 | 59 | ||
60 | collisionObject.CollisionShape = childShape; | 60 | collisionObject.CollisionShape = childShape; |
61 | _childCollisionAlgorithms[index] = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(collisionObject, otherObject); | 61 | _childCollisionAlgorithms[index] = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(collisionObject, otherObject); |
62 | collisionObject.CollisionShape = orgShape; | 62 | collisionObject.CollisionShape = orgShape; |
63 | } | 63 | } |
64 | } | 64 | } |
65 | 65 | ||
66 | public override void ProcessCollision( | 66 | public override void ProcessCollision( |
67 | CollisionObject bodyA, | 67 | CollisionObject bodyA, |
68 | CollisionObject bodyB, | 68 | CollisionObject bodyB, |
69 | DispatcherInfo dispatchInfo, ManifoldResult resultOut) | 69 | DispatcherInfo dispatchInfo, ManifoldResult resultOut) |
70 | { | 70 | { |
71 | //Begin | 71 | //Begin |
72 | 72 | ||
73 | CollisionObject collisionObject = _isSwapped ? bodyB : bodyB; | 73 | CollisionObject collisionObject = _isSwapped ? bodyB : bodyB; |
74 | CollisionObject otherObject = _isSwapped ? bodyA : bodyB; | 74 | CollisionObject otherObject = _isSwapped ? bodyA : bodyB; |
75 | 75 | ||
76 | //Debug.Assert(collisionObject.getCollisionShape().isCompound()); | 76 | //Debug.Assert(collisionObject.getCollisionShape().isCompound()); |
77 | BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); | 77 | BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); |
78 | 78 | ||
79 | CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape; | 79 | CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape; |
80 | 80 | ||
81 | int childrenNumber = _childCollisionAlgorithms.Count; | 81 | int childrenNumber = _childCollisionAlgorithms.Count; |
82 | 82 | ||
83 | for (int i = 0; i < childrenNumber; i++) | 83 | for (int i = 0; i < childrenNumber; i++) |
84 | { | 84 | { |
85 | CompoundShape childShape = compoundShape.GetChildShape(i) as CompoundShape; | 85 | CompoundShape childShape = compoundShape.GetChildShape(i) as CompoundShape; |
86 | 86 | ||
87 | Matrix orgTransform = collisionObject.WorldTransform; | 87 | Matrix orgTransform = collisionObject.WorldTransform; |
88 | CollisionShape orgShape = collisionObject.CollisionShape; | 88 | CollisionShape orgShape = collisionObject.CollisionShape; |
89 | 89 | ||
90 | Matrix childTransform = compoundShape.GetChildTransform(i); | 90 | Matrix childTransform = compoundShape.GetChildTransform(i); |
91 | Matrix newChildWorld = orgTransform * childTransform; | 91 | Matrix newChildWorld = orgTransform * childTransform; |
92 | 92 | ||
93 | collisionObject.WorldTransform = newChildWorld; | 93 | collisionObject.WorldTransform = newChildWorld; |
94 | collisionObject.CollisionShape = childShape; | 94 | collisionObject.CollisionShape = childShape; |
95 | _childCollisionAlgorithms[i].ProcessCollision(collisionObject, otherObject, dispatchInfo, resultOut); | 95 | _childCollisionAlgorithms[i].ProcessCollision(collisionObject, otherObject, dispatchInfo, resultOut); |
96 | 96 | ||
97 | collisionObject.CollisionShape = orgShape; | 97 | collisionObject.CollisionShape = orgShape; |
98 | collisionObject.WorldTransform = orgTransform; | 98 | collisionObject.WorldTransform = orgTransform; |
99 | } | 99 | } |
100 | } | 100 | } |
101 | 101 | ||
102 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | 102 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) |
103 | { | 103 | { |
104 | CollisionObject collisionObject = _isSwapped ? bodyB : bodyA; | 104 | CollisionObject collisionObject = _isSwapped ? bodyB : bodyA; |
105 | CollisionObject otherObject = _isSwapped ? bodyA : bodyB; | 105 | CollisionObject otherObject = _isSwapped ? bodyA : bodyB; |
106 | 106 | ||
107 | BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); | 107 | BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); |
108 | 108 | ||
109 | CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape; | 109 | CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape; |
110 | 110 | ||
111 | float hitFraction = 1.0f; | 111 | float hitFraction = 1.0f; |
112 | 112 | ||
113 | for (int i = 0; i < _childCollisionAlgorithms.Count; i++) | 113 | for (int i = 0; i < _childCollisionAlgorithms.Count; i++) |
114 | { | 114 | { |
115 | CollisionShape childShape = compoundShape.GetChildShape(i); | 115 | CollisionShape childShape = compoundShape.GetChildShape(i); |
116 | 116 | ||
117 | Matrix orgTransform = collisionObject.WorldTransform; | 117 | Matrix orgTransform = collisionObject.WorldTransform; |
118 | CollisionShape orgShape = collisionObject.CollisionShape; | 118 | CollisionShape orgShape = collisionObject.CollisionShape; |
119 | 119 | ||
120 | Matrix childTransform = compoundShape.GetChildTransform(i); | 120 | Matrix childTransform = compoundShape.GetChildTransform(i); |
121 | Matrix newChildWorld = orgTransform * childTransform; | 121 | Matrix newChildWorld = orgTransform * childTransform; |
122 | collisionObject.WorldTransform = newChildWorld; | 122 | collisionObject.WorldTransform = newChildWorld; |
123 | 123 | ||
124 | collisionObject.CollisionShape = childShape; | 124 | collisionObject.CollisionShape = childShape; |
125 | float frac = _childCollisionAlgorithms[i].CalculateTimeOfImpact( | 125 | float frac = _childCollisionAlgorithms[i].CalculateTimeOfImpact( |
126 | collisionObject, otherObject, dispatchInfo, resultOut | 126 | collisionObject, otherObject, dispatchInfo, resultOut |
127 | ); | 127 | ); |
128 | 128 | ||
129 | if (frac < hitFraction) | 129 | if (frac < hitFraction) |
130 | { | 130 | { |
131 | hitFraction = frac; | 131 | hitFraction = frac; |
132 | } | 132 | } |
133 | 133 | ||
134 | collisionObject.CollisionShape = orgShape; | 134 | collisionObject.CollisionShape = orgShape; |
135 | collisionObject.WorldTransform = orgTransform; | 135 | collisionObject.WorldTransform = orgTransform; |
136 | } | 136 | } |
137 | 137 | ||
138 | return hitFraction; | 138 | return hitFraction; |
139 | } | 139 | } |
140 | 140 | ||
141 | public class CreateFunc : CollisionAlgorithmCreateFunction | 141 | public class CreateFunc : CollisionAlgorithmCreateFunction |
142 | { | 142 | { |
143 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | 143 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) |
144 | { | 144 | { |
145 | return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false); | 145 | return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false); |
146 | } | 146 | } |
147 | }; | 147 | }; |
148 | 148 | ||
149 | public class SwappedCreateFunc : CollisionAlgorithmCreateFunction | 149 | public class SwappedCreateFunc : CollisionAlgorithmCreateFunction |
150 | { | 150 | { |
151 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | 151 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) |
152 | { | 152 | { |
153 | return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true); | 153 | return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true); |
154 | } | 154 | } |
155 | }; | 155 | }; |
156 | } | 156 | } |
157 | } | 157 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs index c4949f0..39438e4 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs | |||
@@ -1,189 +1,189 @@ | |||
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 | public class ConvexConcaveCollisionAlgorithm : CollisionAlgorithm | 29 | public class ConvexConcaveCollisionAlgorithm : CollisionAlgorithm |
30 | { | 30 | { |
31 | private bool _isSwapped; | 31 | private bool _isSwapped; |
32 | private ConvexTriangleCallback _convexTriangleCallback; | 32 | private ConvexTriangleCallback _convexTriangleCallback; |
33 | 33 | ||
34 | public ConvexConcaveCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) | 34 | public ConvexConcaveCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) |
35 | : base(collisionAlgorithmConstructionInfo) | 35 | : base(collisionAlgorithmConstructionInfo) |
36 | { | 36 | { |
37 | _isSwapped = isSwapped; | 37 | _isSwapped = isSwapped; |
38 | _convexTriangleCallback = new ConvexTriangleCallback(collisionAlgorithmConstructionInfo.Dispatcher, bodyA, bodyB, isSwapped); | 38 | _convexTriangleCallback = new ConvexTriangleCallback(collisionAlgorithmConstructionInfo.Dispatcher, bodyA, bodyB, isSwapped); |
39 | } | 39 | } |
40 | 40 | ||
41 | public void ClearCache() | 41 | public void ClearCache() |
42 | { | 42 | { |
43 | _convexTriangleCallback.ClearCache(); | 43 | _convexTriangleCallback.ClearCache(); |
44 | } | 44 | } |
45 | 45 | ||
46 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | 46 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) |
47 | { | 47 | { |
48 | CollisionObject convexBody = _isSwapped ? bodyB : bodyA; | 48 | CollisionObject convexBody = _isSwapped ? bodyB : bodyA; |
49 | CollisionObject triBody = _isSwapped ? bodyA : bodyB; | 49 | CollisionObject triBody = _isSwapped ? bodyA : bodyB; |
50 | 50 | ||
51 | if (triBody.CollisionShape.IsConcave) | 51 | if (triBody.CollisionShape.IsConcave) |
52 | { | 52 | { |
53 | CollisionObject triOb = triBody; | 53 | CollisionObject triOb = triBody; |
54 | ConcaveShape concaveShape = triOb.CollisionShape as ConcaveShape; | 54 | ConcaveShape concaveShape = triOb.CollisionShape as ConcaveShape; |
55 | 55 | ||
56 | if (convexBody.CollisionShape.IsConvex) | 56 | if (convexBody.CollisionShape.IsConvex) |
57 | { | 57 | { |
58 | float collisionMarginTriangle = concaveShape.Margin; | 58 | float collisionMarginTriangle = concaveShape.Margin; |
59 | 59 | ||
60 | resultOut.SetPersistentManifold(_convexTriangleCallback.Manifold); | 60 | resultOut.SetPersistentManifold(_convexTriangleCallback.Manifold); |
61 | _convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut); | 61 | _convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut); |
62 | 62 | ||
63 | //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. | 63 | //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. |
64 | //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr); | 64 | //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr); |
65 | 65 | ||
66 | _convexTriangleCallback.Manifold.SetBodies(convexBody, triBody); | 66 | _convexTriangleCallback.Manifold.SetBodies(convexBody, triBody); |
67 | concaveShape.ProcessAllTriangles(_convexTriangleCallback, _convexTriangleCallback.AabbMin, _convexTriangleCallback.AabbMax); | 67 | concaveShape.ProcessAllTriangles(_convexTriangleCallback, _convexTriangleCallback.AabbMin, _convexTriangleCallback.AabbMax); |
68 | } | 68 | } |
69 | } | 69 | } |
70 | } | 70 | } |
71 | 71 | ||
72 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | 72 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) |
73 | { | 73 | { |
74 | CollisionObject convexbody = _isSwapped ? bodyB : bodyA; | 74 | CollisionObject convexbody = _isSwapped ? bodyB : bodyA; |
75 | CollisionObject triBody = _isSwapped ? bodyA : bodyB; | 75 | CollisionObject triBody = _isSwapped ? bodyA : bodyB; |
76 | 76 | ||
77 | 77 | ||
78 | //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) | 78 | //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) |
79 | 79 | ||
80 | //only perform CCD above a certain threshold, this prevents blocking on the long run | 80 | //only perform CCD above a certain threshold, this prevents blocking on the long run |
81 | //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... | 81 | //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... |
82 | float squareMot0 = (convexbody.InterpolationWorldTransform.Translation - convexbody.WorldTransform.Translation).LengthSquared(); | 82 | float squareMot0 = (convexbody.InterpolationWorldTransform.Translation - convexbody.WorldTransform.Translation).LengthSquared(); |
83 | if (squareMot0 < convexbody.CcdSquareMotionThreshold) | 83 | if (squareMot0 < convexbody.CcdSquareMotionThreshold) |
84 | { | 84 | { |
85 | return 1; | 85 | return 1; |
86 | } | 86 | } |
87 | 87 | ||
88 | Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform); | 88 | Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform); |
89 | Matrix convexFromLocal = triInv * convexbody.WorldTransform; | 89 | Matrix convexFromLocal = triInv * convexbody.WorldTransform; |
90 | Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform; | 90 | Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform; |
91 | 91 | ||
92 | if (triBody.CollisionShape.IsConcave) | 92 | if (triBody.CollisionShape.IsConcave) |
93 | { | 93 | { |
94 | Vector3 rayAabbMin = convexFromLocal.Translation; | 94 | Vector3 rayAabbMin = convexFromLocal.Translation; |
95 | MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation); | 95 | MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation); |
96 | Vector3 rayAabbMax = convexFromLocal.Translation; | 96 | Vector3 rayAabbMax = convexFromLocal.Translation; |
97 | MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation); | 97 | MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation); |
98 | float ccdRadius0 = convexbody.CcdSweptSphereRadius; | 98 | float ccdRadius0 = convexbody.CcdSweptSphereRadius; |
99 | rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); | 99 | rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); |
100 | rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); | 100 | rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); |
101 | 101 | ||
102 | float curHitFraction = 1f; //is this available? | 102 | float curHitFraction = 1f; //is this available? |
103 | LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal, | 103 | LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal, |
104 | convexbody.CcdSweptSphereRadius, curHitFraction); | 104 | convexbody.CcdSweptSphereRadius, curHitFraction); |
105 | 105 | ||
106 | raycastCallback.HitFraction = convexbody.HitFraction; | 106 | raycastCallback.HitFraction = convexbody.HitFraction; |
107 | 107 | ||
108 | CollisionObject concavebody = triBody; | 108 | CollisionObject concavebody = triBody; |
109 | 109 | ||
110 | ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape; | 110 | ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape; |
111 | 111 | ||
112 | if (triangleMesh != null) | 112 | if (triangleMesh != null) |
113 | { | 113 | { |
114 | triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax); | 114 | triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax); |
115 | } | 115 | } |
116 | 116 | ||
117 | if (raycastCallback.HitFraction < convexbody.HitFraction) | 117 | if (raycastCallback.HitFraction < convexbody.HitFraction) |
118 | { | 118 | { |
119 | convexbody.HitFraction = raycastCallback.HitFraction; | 119 | convexbody.HitFraction = raycastCallback.HitFraction; |
120 | return raycastCallback.HitFraction; | 120 | return raycastCallback.HitFraction; |
121 | } | 121 | } |
122 | } | 122 | } |
123 | 123 | ||
124 | return 1; | 124 | return 1; |
125 | } | 125 | } |
126 | 126 | ||
127 | public class CreateFunc : CollisionAlgorithmCreateFunction | 127 | public class CreateFunc : CollisionAlgorithmCreateFunction |
128 | { | 128 | { |
129 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | 129 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) |
130 | { | 130 | { |
131 | return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false); | 131 | return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false); |
132 | } | 132 | } |
133 | } | 133 | } |
134 | 134 | ||
135 | public class SwappedCreateFunc : CollisionAlgorithmCreateFunction | 135 | public class SwappedCreateFunc : CollisionAlgorithmCreateFunction |
136 | { | 136 | { |
137 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | 137 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) |
138 | { | 138 | { |
139 | return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true); | 139 | return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true); |
140 | } | 140 | } |
141 | } | 141 | } |
142 | 142 | ||
143 | private class LocalTriangleSphereCastCallback : ITriangleCallback | 143 | private class LocalTriangleSphereCastCallback : ITriangleCallback |
144 | { | 144 | { |
145 | private Matrix _ccdSphereFromTrans; | 145 | private Matrix _ccdSphereFromTrans; |
146 | private Matrix _ccdSphereToTrans; | 146 | private Matrix _ccdSphereToTrans; |
147 | private Matrix _meshTransform; | 147 | private Matrix _meshTransform; |
148 | 148 | ||
149 | private float _ccdSphereRadius; | 149 | private float _ccdSphereRadius; |
150 | private float _hitFraction; | 150 | private float _hitFraction; |
151 | 151 | ||
152 | public LocalTriangleSphereCastCallback(Matrix from, Matrix to, float ccdSphereRadius, float hitFraction) | 152 | public LocalTriangleSphereCastCallback(Matrix from, Matrix to, float ccdSphereRadius, float hitFraction) |
153 | { | 153 | { |
154 | _ccdSphereFromTrans = from; | 154 | _ccdSphereFromTrans = from; |
155 | _ccdSphereToTrans = to; | 155 | _ccdSphereToTrans = to; |
156 | _ccdSphereRadius = ccdSphereRadius; | 156 | _ccdSphereRadius = ccdSphereRadius; |
157 | _hitFraction = hitFraction; | 157 | _hitFraction = hitFraction; |
158 | } | 158 | } |
159 | 159 | ||
160 | public Matrix CcdSphereFromTrans { get { return _ccdSphereFromTrans; } set { _ccdSphereFromTrans = value; } } | 160 | public Matrix CcdSphereFromTrans { get { return _ccdSphereFromTrans; } set { _ccdSphereFromTrans = value; } } |
161 | public Matrix CcdSphereToTrans { get { return _ccdSphereToTrans; } set { _ccdSphereToTrans = value; } } | 161 | public Matrix CcdSphereToTrans { get { return _ccdSphereToTrans; } set { _ccdSphereToTrans = value; } } |
162 | public Matrix MeshTransform { get { return _meshTransform; } set { _meshTransform = value; } } | 162 | public Matrix MeshTransform { get { return _meshTransform; } set { _meshTransform = value; } } |
163 | public float CcdSphereRadius { get { return _ccdSphereRadius; } set { _ccdSphereRadius = value; } } | 163 | public float CcdSphereRadius { get { return _ccdSphereRadius; } set { _ccdSphereRadius = value; } } |
164 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } | 164 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } |
165 | 165 | ||
166 | public void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex) | 166 | public void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex) |
167 | { | 167 | { |
168 | //do a swept sphere for now | 168 | //do a swept sphere for now |
169 | Matrix ident = Matrix.Identity; | 169 | Matrix ident = Matrix.Identity; |
170 | CastResult castResult = new CastResult(); | 170 | CastResult castResult = new CastResult(); |
171 | castResult.Fraction = _hitFraction; | 171 | castResult.Fraction = _hitFraction; |
172 | SphereShape pointShape = new SphereShape(_ccdSphereRadius); | 172 | SphereShape pointShape = new SphereShape(_ccdSphereRadius); |
173 | TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); | 173 | TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); |
174 | VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); | 174 | VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); |
175 | SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver); | 175 | SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver); |
176 | //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); | 176 | //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); |
177 | //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); | 177 | //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); |
178 | //local space? | 178 | //local space? |
179 | 179 | ||
180 | if (convexCaster.CalcTimeOfImpact(_ccdSphereFromTrans, _ccdSphereToTrans, | 180 | if (convexCaster.CalcTimeOfImpact(_ccdSphereFromTrans, _ccdSphereToTrans, |
181 | ident, ident, castResult)) | 181 | ident, ident, castResult)) |
182 | { | 182 | { |
183 | if (_hitFraction > castResult.Fraction) | 183 | if (_hitFraction > castResult.Fraction) |
184 | _hitFraction = castResult.Fraction; | 184 | _hitFraction = castResult.Fraction; |
185 | } | 185 | } |
186 | } | 186 | } |
187 | } | 187 | } |
188 | } | 188 | } |
189 | } | 189 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs index 8fa4837..01c8fe0 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs | |||
@@ -1,193 +1,193 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX | 26 | namespace XnaDevRu.BulletX |
27 | { | 27 | { |
28 | public class ConvexConvexAlgorithm : CollisionAlgorithm, IDisposable | 28 | public class ConvexConvexAlgorithm : CollisionAlgorithm, IDisposable |
29 | { | 29 | { |
30 | private const bool DisableCcd = false; | 30 | private const bool DisableCcd = false; |
31 | private GjkPairDetector _gjkPairDetector; | 31 | private GjkPairDetector _gjkPairDetector; |
32 | private bool _ownManifold; | 32 | private bool _ownManifold; |
33 | private PersistentManifold _manifold; | 33 | private PersistentManifold _manifold; |
34 | private bool _lowLevelOfDetail; | 34 | private bool _lowLevelOfDetail; |
35 | 35 | ||
36 | public ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) | 36 | public ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) |
37 | : base(collisionAlgorithmConstructionInfo) | 37 | : base(collisionAlgorithmConstructionInfo) |
38 | { | 38 | { |
39 | _gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver); | 39 | _gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver); |
40 | _ownManifold = false; | 40 | _ownManifold = false; |
41 | _manifold = manifold; | 41 | _manifold = manifold; |
42 | _lowLevelOfDetail = false; | 42 | _lowLevelOfDetail = false; |
43 | } | 43 | } |
44 | 44 | ||
45 | public bool LowLevelOfDetail { get { return _lowLevelOfDetail; } set { _lowLevelOfDetail = value; } } | 45 | public bool LowLevelOfDetail { get { return _lowLevelOfDetail; } set { _lowLevelOfDetail = value; } } |
46 | public bool OwnManifold { get { return _ownManifold; } set { _ownManifold = value; } } | 46 | public bool OwnManifold { get { return _ownManifold; } set { _ownManifold = value; } } |
47 | public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } | 47 | public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } |
48 | 48 | ||
49 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | 49 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) |
50 | { | 50 | { |
51 | if (_manifold == null) | 51 | if (_manifold == null) |
52 | { | 52 | { |
53 | //swapped? | 53 | //swapped? |
54 | _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); | 54 | _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); |
55 | _ownManifold = true; | 55 | _ownManifold = true; |
56 | } | 56 | } |
57 | resultOut.SetPersistentManifold(_manifold); | 57 | resultOut.SetPersistentManifold(_manifold); |
58 | 58 | ||
59 | ConvexShape min0 = bodyA.CollisionShape as ConvexShape; | 59 | ConvexShape min0 = bodyA.CollisionShape as ConvexShape; |
60 | ConvexShape min1 = bodyB.CollisionShape as ConvexShape; | 60 | ConvexShape min1 = bodyB.CollisionShape as ConvexShape; |
61 | 61 | ||
62 | GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | 62 | GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); |
63 | 63 | ||
64 | //TODO: if (dispatchInfo.m_useContinuous) | 64 | //TODO: if (dispatchInfo.m_useContinuous) |
65 | _gjkPairDetector.setMinkowskiA(min0); | 65 | _gjkPairDetector.setMinkowskiA(min0); |
66 | _gjkPairDetector.setMinkowskiB(min1); | 66 | _gjkPairDetector.setMinkowskiB(min1); |
67 | input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold; | 67 | input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold; |
68 | input.MaximumDistanceSquared *= input.MaximumDistanceSquared; | 68 | input.MaximumDistanceSquared *= input.MaximumDistanceSquared; |
69 | 69 | ||
70 | // input.m_maximumDistanceSquared = 1e30f; | 70 | // input.m_maximumDistanceSquared = 1e30f; |
71 | 71 | ||
72 | input.TransformA = bodyA.WorldTransform; | 72 | input.TransformA = bodyA.WorldTransform; |
73 | input.TransformB = bodyB.WorldTransform; | 73 | input.TransformB = bodyB.WorldTransform; |
74 | 74 | ||
75 | _gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw); | 75 | _gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw); |
76 | } | 76 | } |
77 | 77 | ||
78 | public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | 78 | public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) |
79 | { | 79 | { |
80 | //Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold | 80 | //Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold |
81 | 81 | ||
82 | //Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold | 82 | //Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold |
83 | //col0->m_worldTransform, | 83 | //col0->m_worldTransform, |
84 | float resultFraction = 1f; | 84 | float resultFraction = 1f; |
85 | 85 | ||
86 | float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared(); | 86 | float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared(); |
87 | float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared(); | 87 | float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared(); |
88 | 88 | ||
89 | if (squareMotA < colA.CcdSquareMotionThreshold && | 89 | if (squareMotA < colA.CcdSquareMotionThreshold && |
90 | squareMotB < colB.CcdSquareMotionThreshold) | 90 | squareMotB < colB.CcdSquareMotionThreshold) |
91 | return resultFraction; | 91 | return resultFraction; |
92 | 92 | ||
93 | if (DisableCcd) | 93 | if (DisableCcd) |
94 | return 1f; | 94 | return 1f; |
95 | 95 | ||
96 | //An adhoc way of testing the Continuous Collision Detection algorithms | 96 | //An adhoc way of testing the Continuous Collision Detection algorithms |
97 | //One object is approximated as a sphere, to simplify things | 97 | //One object is approximated as a sphere, to simplify things |
98 | //Starting in penetration should report no time of impact | 98 | //Starting in penetration should report no time of impact |
99 | //For proper CCD, better accuracy and handling of 'allowed' penetration should be added | 99 | //For proper CCD, better accuracy and handling of 'allowed' penetration should be added |
100 | //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) | 100 | //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) |
101 | 101 | ||
102 | // Convex0 against sphere for Convex1 | 102 | // Convex0 against sphere for Convex1 |
103 | { | 103 | { |
104 | ConvexShape convexA = colA.CollisionShape as ConvexShape; | 104 | ConvexShape convexA = colA.CollisionShape as ConvexShape; |
105 | 105 | ||
106 | SphereShape sphereB = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation | 106 | SphereShape sphereB = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation |
107 | CastResult result = new CastResult(); | 107 | CastResult result = new CastResult(); |
108 | VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); | 108 | VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); |
109 | //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); | 109 | //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); |
110 | //Simplification, one object is simplified as a sphere | 110 | //Simplification, one object is simplified as a sphere |
111 | GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex); | 111 | GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex); |
112 | //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); | 112 | //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); |
113 | if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform, | 113 | if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform, |
114 | colB.WorldTransform, colB.InterpolationWorldTransform, result)) | 114 | colB.WorldTransform, colB.InterpolationWorldTransform, result)) |
115 | { | 115 | { |
116 | //store result.m_fraction in both bodies | 116 | //store result.m_fraction in both bodies |
117 | if (colA.HitFraction > result.Fraction) | 117 | if (colA.HitFraction > result.Fraction) |
118 | colA.HitFraction = result.Fraction; | 118 | colA.HitFraction = result.Fraction; |
119 | 119 | ||
120 | if (colB.HitFraction > result.Fraction) | 120 | if (colB.HitFraction > result.Fraction) |
121 | colB.HitFraction = result.Fraction; | 121 | colB.HitFraction = result.Fraction; |
122 | 122 | ||
123 | if (resultFraction > result.Fraction) | 123 | if (resultFraction > result.Fraction) |
124 | resultFraction = result.Fraction; | 124 | resultFraction = result.Fraction; |
125 | } | 125 | } |
126 | } | 126 | } |
127 | 127 | ||
128 | // Sphere (for convex0) against Convex1 | 128 | // Sphere (for convex0) against Convex1 |
129 | { | 129 | { |
130 | ConvexShape convexB = colB.CollisionShape as ConvexShape; | 130 | ConvexShape convexB = colB.CollisionShape as ConvexShape; |
131 | 131 | ||
132 | SphereShape sphereA = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation | 132 | SphereShape sphereA = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation |
133 | CastResult result = new CastResult(); | 133 | CastResult result = new CastResult(); |
134 | VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); | 134 | VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); |
135 | //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); | 135 | //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); |
136 | ///Simplification, one object is simplified as a sphere | 136 | ///Simplification, one object is simplified as a sphere |
137 | GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex); | 137 | GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex); |
138 | //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); | 138 | //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); |
139 | if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform, | 139 | if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform, |
140 | colB.WorldTransform, colB.InterpolationWorldTransform, result)) | 140 | colB.WorldTransform, colB.InterpolationWorldTransform, result)) |
141 | { | 141 | { |
142 | //store result.m_fraction in both bodies | 142 | //store result.m_fraction in both bodies |
143 | if (colA.HitFraction > result.Fraction) | 143 | if (colA.HitFraction > result.Fraction) |
144 | colA.HitFraction = result.Fraction; | 144 | colA.HitFraction = result.Fraction; |
145 | 145 | ||
146 | if (colB.HitFraction > result.Fraction) | 146 | if (colB.HitFraction > result.Fraction) |
147 | colB.HitFraction = result.Fraction; | 147 | colB.HitFraction = result.Fraction; |
148 | 148 | ||
149 | if (resultFraction > result.Fraction) | 149 | if (resultFraction > result.Fraction) |
150 | resultFraction = result.Fraction; | 150 | resultFraction = result.Fraction; |
151 | } | 151 | } |
152 | } | 152 | } |
153 | return resultFraction; | 153 | return resultFraction; |
154 | } | 154 | } |
155 | 155 | ||
156 | public class CreateFunc : CollisionAlgorithmCreateFunction | 156 | public class CreateFunc : CollisionAlgorithmCreateFunction |
157 | { | 157 | { |
158 | private IConvexPenetrationDepthSolver _penetrationDepthSolver; | 158 | private IConvexPenetrationDepthSolver _penetrationDepthSolver; |
159 | private ISimplexSolver _simplexSolver; | 159 | private ISimplexSolver _simplexSolver; |
160 | //private bool _ownsSolvers; | 160 | //private bool _ownsSolvers; |
161 | 161 | ||
162 | public CreateFunc() | 162 | public CreateFunc() |
163 | { | 163 | { |
164 | //_ownsSolvers = true; | 164 | //_ownsSolvers = true; |
165 | _simplexSolver = new VoronoiSimplexSolver(); | 165 | _simplexSolver = new VoronoiSimplexSolver(); |
166 | _penetrationDepthSolver = new GjkEpaPenetrationDepthSolver(); | 166 | _penetrationDepthSolver = new GjkEpaPenetrationDepthSolver(); |
167 | } | 167 | } |
168 | 168 | ||
169 | public CreateFunc(ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) | 169 | public CreateFunc(ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) |
170 | { | 170 | { |
171 | //_ownsSolvers = false; | 171 | //_ownsSolvers = false; |
172 | _simplexSolver = simplexSolver; | 172 | _simplexSolver = simplexSolver; |
173 | _penetrationDepthSolver = penetrationDepthSolver; | 173 | _penetrationDepthSolver = penetrationDepthSolver; |
174 | } | 174 | } |
175 | 175 | ||
176 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | 176 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) |
177 | { | 177 | { |
178 | return new ConvexConvexAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, _simplexSolver, _penetrationDepthSolver); | 178 | return new ConvexConvexAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, _simplexSolver, _penetrationDepthSolver); |
179 | } | 179 | } |
180 | } | 180 | } |
181 | 181 | ||
182 | #region IDisposable Members | 182 | #region IDisposable Members |
183 | public void Dispose() | 183 | public void Dispose() |
184 | { | 184 | { |
185 | if (_ownManifold) | 185 | if (_ownManifold) |
186 | { | 186 | { |
187 | if (_manifold != null) | 187 | if (_manifold != null) |
188 | Dispatcher.ReleaseManifold(_manifold); | 188 | Dispatcher.ReleaseManifold(_manifold); |
189 | } | 189 | } |
190 | } | 190 | } |
191 | #endregion | 191 | #endregion |
192 | } | 192 | } |
193 | } | 193 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs index 5355817..ba21c72 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs | |||
@@ -1,130 +1,130 @@ | |||
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 | public class ConvexTriangleCallback : ITriangleCallback, IDisposable | 29 | public class ConvexTriangleCallback : ITriangleCallback, IDisposable |
30 | { | 30 | { |
31 | private CollisionObject _convexBody; | 31 | private CollisionObject _convexBody; |
32 | private CollisionObject _triBody; | 32 | private CollisionObject _triBody; |
33 | 33 | ||
34 | private Vector3 _aabbMin; | 34 | private Vector3 _aabbMin; |
35 | private Vector3 _aabbMax; | 35 | private Vector3 _aabbMax; |
36 | 36 | ||
37 | private ManifoldResult _resultOut; | 37 | private ManifoldResult _resultOut; |
38 | 38 | ||
39 | private IDispatcher _dispatcher; | 39 | private IDispatcher _dispatcher; |
40 | private DispatcherInfo _dispatchInfo; | 40 | private DispatcherInfo _dispatchInfo; |
41 | private float _collisionMarginTriangle; | 41 | private float _collisionMarginTriangle; |
42 | 42 | ||
43 | private int _triangleCount; | 43 | private int _triangleCount; |
44 | 44 | ||
45 | private PersistentManifold _manifold; | 45 | private PersistentManifold _manifold; |
46 | 46 | ||
47 | public ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) | 47 | public ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) |
48 | { | 48 | { |
49 | _dispatcher = dispatcher; | 49 | _dispatcher = dispatcher; |
50 | _dispatchInfo = null; | 50 | _dispatchInfo = null; |
51 | _convexBody = isSwapped ? bodyB : bodyA; | 51 | _convexBody = isSwapped ? bodyB : bodyA; |
52 | _triBody = isSwapped ? bodyA : bodyB; | 52 | _triBody = isSwapped ? bodyA : bodyB; |
53 | 53 | ||
54 | // create the manifold from the dispatcher 'manifold pool' | 54 | // create the manifold from the dispatcher 'manifold pool' |
55 | _manifold = _dispatcher.GetNewManifold(_convexBody, _triBody); | 55 | _manifold = _dispatcher.GetNewManifold(_convexBody, _triBody); |
56 | ClearCache(); | 56 | ClearCache(); |
57 | } | 57 | } |
58 | 58 | ||
59 | public Vector3 AabbMin { get { return _aabbMin; } } | 59 | public Vector3 AabbMin { get { return _aabbMin; } } |
60 | public Vector3 AabbMax { get { return _aabbMax; } } | 60 | public Vector3 AabbMax { get { return _aabbMax; } } |
61 | public int TriangleCount { get { return _triangleCount; } set { _triangleCount = value; } } | 61 | public int TriangleCount { get { return _triangleCount; } set { _triangleCount = value; } } |
62 | public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } | 62 | public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } |
63 | 63 | ||
64 | public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | 64 | public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut) |
65 | { | 65 | { |
66 | _dispatchInfo = dispatchInfo; | 66 | _dispatchInfo = dispatchInfo; |
67 | _collisionMarginTriangle = collisionMarginTriangle; | 67 | _collisionMarginTriangle = collisionMarginTriangle; |
68 | _resultOut = resultOut; | 68 | _resultOut = resultOut; |
69 | 69 | ||
70 | //recalc aabbs | 70 | //recalc aabbs |
71 | Matrix convexInTriangleSpace = MathHelper.InvertMatrix(_triBody.WorldTransform) * _convexBody.WorldTransform; | 71 | Matrix convexInTriangleSpace = MathHelper.InvertMatrix(_triBody.WorldTransform) * _convexBody.WorldTransform; |
72 | CollisionShape convexShape = _convexBody.CollisionShape; | 72 | CollisionShape convexShape = _convexBody.CollisionShape; |
73 | //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape); | 73 | //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape); |
74 | convexShape.GetAabb(convexInTriangleSpace, out _aabbMin, out _aabbMax); | 74 | convexShape.GetAabb(convexInTriangleSpace, out _aabbMin, out _aabbMax); |
75 | float extraMargin = collisionMarginTriangle; | 75 | float extraMargin = collisionMarginTriangle; |
76 | Vector3 extra = new Vector3(extraMargin, extraMargin, extraMargin); | 76 | Vector3 extra = new Vector3(extraMargin, extraMargin, extraMargin); |
77 | 77 | ||
78 | _aabbMax += extra; | 78 | _aabbMax += extra; |
79 | _aabbMin -= extra; | 79 | _aabbMin -= extra; |
80 | } | 80 | } |
81 | 81 | ||
82 | public void ClearCache() | 82 | public void ClearCache() |
83 | { | 83 | { |
84 | _dispatcher.ClearManifold(_manifold); | 84 | _dispatcher.ClearManifold(_manifold); |
85 | } | 85 | } |
86 | 86 | ||
87 | #region ITriangleCallback Members | 87 | #region ITriangleCallback Members |
88 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) | 88 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) |
89 | { | 89 | { |
90 | //aabb filter is already applied! | 90 | //aabb filter is already applied! |
91 | CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo(); | 91 | CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo(); |
92 | collisionAlgorithmConstructionInfo.Dispatcher = _dispatcher; | 92 | collisionAlgorithmConstructionInfo.Dispatcher = _dispatcher; |
93 | 93 | ||
94 | CollisionObject collisionObject = _triBody; | 94 | CollisionObject collisionObject = _triBody; |
95 | 95 | ||
96 | //debug drawing of the overlapping triangles | 96 | //debug drawing of the overlapping triangles |
97 | /*if (m_dispatchInfoPtr && m_dispatchInfoPtr.m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) | 97 | /*if (m_dispatchInfoPtr && m_dispatchInfoPtr.m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) |
98 | { | 98 | { |
99 | Vector3 color = new Vector3(255, 255, 0); | 99 | Vector3 color = new Vector3(255, 255, 0); |
100 | btTransform & tr = ob->WorldTransform; | 100 | btTransform & tr = ob->WorldTransform; |
101 | m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]), tr(triangle[1]), color); | 101 | m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]), tr(triangle[1]), color); |
102 | m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]), tr(triangle[2]), color); | 102 | m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]), tr(triangle[2]), color); |
103 | m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]), tr(triangle[0]), color); | 103 | m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]), tr(triangle[0]), color); |
104 | }*/ | 104 | }*/ |
105 | 105 | ||
106 | if (_convexBody.CollisionShape.IsConvex) | 106 | if (_convexBody.CollisionShape.IsConvex) |
107 | { | 107 | { |
108 | TriangleShape triangleShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); | 108 | TriangleShape triangleShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); |
109 | triangleShape.Margin=_collisionMarginTriangle; | 109 | triangleShape.Margin=_collisionMarginTriangle; |
110 | 110 | ||
111 | CollisionShape tempShape = collisionObject.CollisionShape; | 111 | CollisionShape tempShape = collisionObject.CollisionShape; |
112 | collisionObject.CollisionShape = triangleShape; | 112 | collisionObject.CollisionShape = triangleShape; |
113 | 113 | ||
114 | CollisionAlgorithm collisionAlgorithm = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(_convexBody, _triBody, _manifold); | 114 | CollisionAlgorithm collisionAlgorithm = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(_convexBody, _triBody, _manifold); |
115 | 115 | ||
116 | _resultOut.SetShapeIdentifiers(-1, -1, partID, triangleIndex); | 116 | _resultOut.SetShapeIdentifiers(-1, -1, partID, triangleIndex); |
117 | collisionAlgorithm.ProcessCollision(_convexBody, _triBody, _dispatchInfo, _resultOut); | 117 | collisionAlgorithm.ProcessCollision(_convexBody, _triBody, _dispatchInfo, _resultOut); |
118 | collisionObject.CollisionShape = tempShape; | 118 | collisionObject.CollisionShape = tempShape; |
119 | } | 119 | } |
120 | } | 120 | } |
121 | #endregion | 121 | #endregion |
122 | #region IDisposable Members | 122 | #region IDisposable Members |
123 | public void Dispose() | 123 | public void Dispose() |
124 | { | 124 | { |
125 | ClearCache(); | 125 | ClearCache(); |
126 | _dispatcher.ReleaseManifold(_manifold); | 126 | _dispatcher.ReleaseManifold(_manifold); |
127 | } | 127 | } |
128 | #endregion | 128 | #endregion |
129 | } | 129 | } |
130 | } | 130 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs index 13cb3d9..5995e2c 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs | |||
@@ -1,52 +1,52 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX | 26 | namespace XnaDevRu.BulletX |
27 | { | 27 | { |
28 | /// <summary> | 28 | /// <summary> |
29 | /// EmptyAlgorithm is a stub for unsupported collision pairs. | 29 | /// EmptyAlgorithm is a stub for unsupported collision pairs. |
30 | /// The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame. | 30 | /// The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame. |
31 | /// </summary> | 31 | /// </summary> |
32 | public class EmptyAlgorithm : CollisionAlgorithm | 32 | public class EmptyAlgorithm : CollisionAlgorithm |
33 | { | 33 | { |
34 | public EmptyAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) | 34 | public EmptyAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) |
35 | : base(collisionAlgorithmConstructionInfo) { } | 35 | : base(collisionAlgorithmConstructionInfo) { } |
36 | 36 | ||
37 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { } | 37 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { } |
38 | 38 | ||
39 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | 39 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) |
40 | { | 40 | { |
41 | return 1f; | 41 | return 1f; |
42 | } | 42 | } |
43 | 43 | ||
44 | public class CreateFunc : CollisionAlgorithmCreateFunction | 44 | public class CreateFunc : CollisionAlgorithmCreateFunction |
45 | { | 45 | { |
46 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | 46 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) |
47 | { | 47 | { |
48 | return new EmptyAlgorithm(collisionAlgorithmConstructionInfo); | 48 | return new EmptyAlgorithm(collisionAlgorithmConstructionInfo); |
49 | } | 49 | } |
50 | }; | 50 | }; |
51 | } | 51 | } |
52 | } | 52 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs index 63f37ea..69143a1 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs | |||
@@ -1,147 +1,147 @@ | |||
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 | public delegate bool ContactAddedCallback(ManifoldPoint contactPoint, CollisionObject collisionObjectA, int partIdA, int indexA, CollisionObject collisionObjectB, int partIdB, int indexB); | 29 | public delegate bool ContactAddedCallback(ManifoldPoint contactPoint, CollisionObject collisionObjectA, int partIdA, int indexA, CollisionObject collisionObjectB, int partIdB, int indexB); |
30 | 30 | ||
31 | public class ManifoldResult : DiscreteCollisionDetectorInterface.Result | 31 | public class ManifoldResult : DiscreteCollisionDetectorInterface.Result |
32 | { | 32 | { |
33 | private PersistentManifold _manifold; | 33 | private PersistentManifold _manifold; |
34 | private static ContactAddedCallback _contactAddedCallback = null; | 34 | private static ContactAddedCallback _contactAddedCallback = null; |
35 | 35 | ||
36 | //we need this for compounds | 36 | //we need this for compounds |
37 | private Matrix _rootTransA; | 37 | private Matrix _rootTransA; |
38 | private Matrix _rootTransB; | 38 | private Matrix _rootTransB; |
39 | 39 | ||
40 | private CollisionObject _bodyA; | 40 | private CollisionObject _bodyA; |
41 | private CollisionObject _bodyB; | 41 | private CollisionObject _bodyB; |
42 | private int _partIdA; | 42 | private int _partIdA; |
43 | private int _partIdB; | 43 | private int _partIdB; |
44 | private int _indexA; | 44 | private int _indexA; |
45 | private int _indexB; | 45 | private int _indexB; |
46 | 46 | ||
47 | public ManifoldResult() | 47 | public ManifoldResult() |
48 | { | 48 | { |
49 | } | 49 | } |
50 | 50 | ||
51 | public ManifoldResult(CollisionObject bodyA, CollisionObject bodyB) | 51 | public ManifoldResult(CollisionObject bodyA, CollisionObject bodyB) |
52 | { | 52 | { |
53 | _bodyA = bodyA; | 53 | _bodyA = bodyA; |
54 | _bodyB = bodyB; | 54 | _bodyB = bodyB; |
55 | _rootTransA = bodyA.WorldTransform; | 55 | _rootTransA = bodyA.WorldTransform; |
56 | _rootTransB = bodyB.WorldTransform; | 56 | _rootTransB = bodyB.WorldTransform; |
57 | } | 57 | } |
58 | 58 | ||
59 | public static ContactAddedCallback ContactAddedCallback { get { return _contactAddedCallback; } set { _contactAddedCallback = value; } } | 59 | public static ContactAddedCallback ContactAddedCallback { get { return _contactAddedCallback; } set { _contactAddedCallback = value; } } |
60 | 60 | ||
61 | public void SetPersistentManifold(PersistentManifold manifold) | 61 | public void SetPersistentManifold(PersistentManifold manifold) |
62 | { | 62 | { |
63 | _manifold = manifold; | 63 | _manifold = manifold; |
64 | } | 64 | } |
65 | 65 | ||
66 | public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB) | 66 | public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB) |
67 | { | 67 | { |
68 | _partIdA = partIdA; | 68 | _partIdA = partIdA; |
69 | _partIdB = partIdB; | 69 | _partIdB = partIdB; |
70 | _indexA = indexA; | 70 | _indexA = indexA; |
71 | _indexB = indexB; | 71 | _indexB = indexB; |
72 | } | 72 | } |
73 | 73 | ||
74 | public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) | 74 | public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) |
75 | { | 75 | { |
76 | if (_manifold == null) | 76 | if (_manifold == null) |
77 | throw new BulletException("Manifold Pointer is null."); | 77 | throw new BulletException("Manifold Pointer is null."); |
78 | 78 | ||
79 | //order in manifold needs to match | 79 | //order in manifold needs to match |
80 | 80 | ||
81 | if (depth > PersistentManifold.ContactBreakingThreshold) | 81 | if (depth > PersistentManifold.ContactBreakingThreshold) |
82 | return; | 82 | return; |
83 | 83 | ||
84 | bool isSwapped = _manifold.BodyA != _bodyA; | 84 | bool isSwapped = _manifold.BodyA != _bodyA; |
85 | 85 | ||
86 | Vector3 pointA = pointInWorld + normalOnBInWorld * depth; | 86 | Vector3 pointA = pointInWorld + normalOnBInWorld * depth; |
87 | Vector3 localA; | 87 | Vector3 localA; |
88 | Vector3 localB; | 88 | Vector3 localB; |
89 | 89 | ||
90 | if (isSwapped) | 90 | if (isSwapped) |
91 | { | 91 | { |
92 | localA = MathHelper.InvXForm(_rootTransB, pointA); | 92 | localA = MathHelper.InvXForm(_rootTransB, pointA); |
93 | localB = MathHelper.InvXForm(_rootTransA, pointInWorld); | 93 | localB = MathHelper.InvXForm(_rootTransA, pointInWorld); |
94 | } | 94 | } |
95 | else | 95 | else |
96 | { | 96 | { |
97 | localA = MathHelper.InvXForm(_rootTransA, pointA); | 97 | localA = MathHelper.InvXForm(_rootTransA, pointA); |
98 | localB = MathHelper.InvXForm(_rootTransB, pointInWorld); | 98 | localB = MathHelper.InvXForm(_rootTransB, pointInWorld); |
99 | } | 99 | } |
100 | 100 | ||
101 | ManifoldPoint newPt = new ManifoldPoint(localA, localB, normalOnBInWorld, depth); | 101 | ManifoldPoint newPt = new ManifoldPoint(localA, localB, normalOnBInWorld, depth); |
102 | 102 | ||
103 | int insertIndex = _manifold.GetCacheEntry(newPt); | 103 | int insertIndex = _manifold.GetCacheEntry(newPt); |
104 | 104 | ||
105 | newPt.CombinedFriction = CalculateCombinedFriction(_bodyA, _bodyB); | 105 | newPt.CombinedFriction = CalculateCombinedFriction(_bodyA, _bodyB); |
106 | newPt.CombinedRestitution = CalculateCombinedRestitution(_bodyA, _bodyB); | 106 | newPt.CombinedRestitution = CalculateCombinedRestitution(_bodyA, _bodyB); |
107 | 107 | ||
108 | //User can override friction and/or restitution | 108 | //User can override friction and/or restitution |
109 | if (_contactAddedCallback != null && | 109 | if (_contactAddedCallback != null && |
110 | //and if either of the two bodies requires custom material | 110 | //and if either of the two bodies requires custom material |
111 | ((_bodyA.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0 || | 111 | ((_bodyA.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0 || |
112 | (_bodyB.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0)) | 112 | (_bodyB.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0)) |
113 | { | 113 | { |
114 | //experimental feature info, for per-triangle material etc. | 114 | //experimental feature info, for per-triangle material etc. |
115 | CollisionObject obj0 = isSwapped ? _bodyB : _bodyA; | 115 | CollisionObject obj0 = isSwapped ? _bodyB : _bodyA; |
116 | CollisionObject obj1 = isSwapped ? _bodyA : _bodyB; | 116 | CollisionObject obj1 = isSwapped ? _bodyA : _bodyB; |
117 | _contactAddedCallback(newPt, obj0, _partIdA, _indexA, obj1, _partIdB, _indexB); | 117 | _contactAddedCallback(newPt, obj0, _partIdA, _indexA, obj1, _partIdB, _indexB); |
118 | } | 118 | } |
119 | 119 | ||
120 | if (insertIndex >= 0) | 120 | if (insertIndex >= 0) |
121 | { | 121 | { |
122 | _manifold.ReplaceContactPoint(newPt, insertIndex); | 122 | _manifold.ReplaceContactPoint(newPt, insertIndex); |
123 | } | 123 | } |
124 | else | 124 | else |
125 | { | 125 | { |
126 | _manifold.AddManifoldPoint(newPt); | 126 | _manifold.AddManifoldPoint(newPt); |
127 | } | 127 | } |
128 | } | 128 | } |
129 | 129 | ||
130 | private float CalculateCombinedFriction(CollisionObject bodyA, CollisionObject bodyB) | 130 | private float CalculateCombinedFriction(CollisionObject bodyA, CollisionObject bodyB) |
131 | { | 131 | { |
132 | float friction = bodyA.Friction * bodyB.Friction; | 132 | float friction = bodyA.Friction * bodyB.Friction; |
133 | 133 | ||
134 | float MaxFriction = 10; | 134 | float MaxFriction = 10; |
135 | if (friction < -MaxFriction) | 135 | if (friction < -MaxFriction) |
136 | friction = -MaxFriction; | 136 | friction = -MaxFriction; |
137 | if (friction > MaxFriction) | 137 | if (friction > MaxFriction) |
138 | friction = MaxFriction; | 138 | friction = MaxFriction; |
139 | return friction; | 139 | return friction; |
140 | } | 140 | } |
141 | 141 | ||
142 | private float CalculateCombinedRestitution(CollisionObject bodyA, CollisionObject bodyB) | 142 | private float CalculateCombinedRestitution(CollisionObject bodyA, CollisionObject bodyB) |
143 | { | 143 | { |
144 | return bodyA.Restitution * bodyB.Restitution; | 144 | return bodyA.Restitution * bodyB.Restitution; |
145 | } | 145 | } |
146 | } | 146 | } |
147 | } | 147 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs index 957843b..39eb53a 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs | |||
@@ -1,304 +1,304 @@ | |||
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 | public class SimulationIslandManager | 29 | public class SimulationIslandManager |
30 | { | 30 | { |
31 | private UnionFind _unionFind = new UnionFind(); | 31 | private UnionFind _unionFind = new UnionFind(); |
32 | 32 | ||
33 | public void InitUnionFind(int n) | 33 | public void InitUnionFind(int n) |
34 | { | 34 | { |
35 | _unionFind.Reset(n); | 35 | _unionFind.Reset(n); |
36 | } | 36 | } |
37 | 37 | ||
38 | public UnionFind UnionFind { get { return _unionFind; } } | 38 | public UnionFind UnionFind { get { return _unionFind; } } |
39 | 39 | ||
40 | public virtual void UpdateActivationState(CollisionWorld world, IDispatcher dispatcher) | 40 | public virtual void UpdateActivationState(CollisionWorld world, IDispatcher dispatcher) |
41 | { | 41 | { |
42 | InitUnionFind(world.CollisionObjectsCount); | 42 | InitUnionFind(world.CollisionObjectsCount); |
43 | 43 | ||
44 | // put the index into m_controllers into m_tag | 44 | // put the index into m_controllers into m_tag |
45 | int index = 0; | 45 | int index = 0; |
46 | for (int i = 0; i < world.CollisionObjects.Count; i++) | 46 | for (int i = 0; i < world.CollisionObjects.Count; i++) |
47 | { | 47 | { |
48 | world.CollisionObjects[i].IslandTag = index; | 48 | world.CollisionObjects[i].IslandTag = index; |
49 | world.CollisionObjects[i].HitFraction = 1; | 49 | world.CollisionObjects[i].HitFraction = 1; |
50 | world.CollisionObjects[i].CompanionID = -1; | 50 | world.CollisionObjects[i].CompanionID = -1; |
51 | index++; | 51 | index++; |
52 | } | 52 | } |
53 | // do the union find | 53 | // do the union find |
54 | FindUnions(dispatcher); | 54 | FindUnions(dispatcher); |
55 | } | 55 | } |
56 | 56 | ||
57 | public virtual void StoreIslandActivationState(CollisionWorld world) | 57 | public virtual void StoreIslandActivationState(CollisionWorld world) |
58 | { | 58 | { |
59 | // put the islandId ('find' value) into m_tag | 59 | // put the islandId ('find' value) into m_tag |
60 | int index = 0; | 60 | int index = 0; |
61 | for (int i = 0; i < world.CollisionObjects.Count; i++) | 61 | for (int i = 0; i < world.CollisionObjects.Count; i++) |
62 | { | 62 | { |
63 | if (world.CollisionObjects[i].MergesSimulationIslands) | 63 | if (world.CollisionObjects[i].MergesSimulationIslands) |
64 | { | 64 | { |
65 | world.CollisionObjects[i].IslandTag = _unionFind.Find(index); | 65 | world.CollisionObjects[i].IslandTag = _unionFind.Find(index); |
66 | world.CollisionObjects[i].CompanionID = -1; | 66 | world.CollisionObjects[i].CompanionID = -1; |
67 | } | 67 | } |
68 | else | 68 | else |
69 | { | 69 | { |
70 | world.CollisionObjects[i].IslandTag = -1; | 70 | world.CollisionObjects[i].IslandTag = -1; |
71 | world.CollisionObjects[i].CompanionID = -2; | 71 | world.CollisionObjects[i].CompanionID = -2; |
72 | } | 72 | } |
73 | index++; | 73 | index++; |
74 | } | 74 | } |
75 | } | 75 | } |
76 | 76 | ||
77 | public void FindUnions(IDispatcher dispatcher) | 77 | public void FindUnions(IDispatcher dispatcher) |
78 | { | 78 | { |
79 | for (int i = 0; i < dispatcher.ManifoldCount; i++) | 79 | for (int i = 0; i < dispatcher.ManifoldCount; i++) |
80 | { | 80 | { |
81 | PersistentManifold manifold = dispatcher.GetManifoldByIndex(i); | 81 | PersistentManifold manifold = dispatcher.GetManifoldByIndex(i); |
82 | //static objects (invmass 0.f) don't merge ! | 82 | //static objects (invmass 0.f) don't merge ! |
83 | 83 | ||
84 | CollisionObject colObjA = manifold.BodyA as CollisionObject; | 84 | CollisionObject colObjA = manifold.BodyA as CollisionObject; |
85 | CollisionObject colObjB = manifold.BodyB as CollisionObject; | 85 | CollisionObject colObjB = manifold.BodyB as CollisionObject; |
86 | 86 | ||
87 | if (((colObjA != null) && (colObjA.MergesSimulationIslands)) && | 87 | if (((colObjA != null) && (colObjA.MergesSimulationIslands)) && |
88 | ((colObjB != null) && (colObjB.MergesSimulationIslands))) | 88 | ((colObjB != null) && (colObjB.MergesSimulationIslands))) |
89 | { | 89 | { |
90 | _unionFind.Unite(colObjA.IslandTag, colObjB.IslandTag); | 90 | _unionFind.Unite(colObjA.IslandTag, colObjB.IslandTag); |
91 | } | 91 | } |
92 | } | 92 | } |
93 | } | 93 | } |
94 | 94 | ||
95 | public void BuildAndProcessIslands(IDispatcher dispatcher, List<CollisionObject> collisionObjects, IIslandCallback callback) | 95 | public void BuildAndProcessIslands(IDispatcher dispatcher, List<CollisionObject> collisionObjects, IIslandCallback callback) |
96 | { | 96 | { |
97 | //we are going to sort the unionfind array, and store the element id in the size | 97 | //we are going to sort the unionfind array, and store the element id in the size |
98 | //afterwards, we clean unionfind, to make sure no-one uses it anymore | 98 | //afterwards, we clean unionfind, to make sure no-one uses it anymore |
99 | UnionFind.SortIslands(); | 99 | UnionFind.SortIslands(); |
100 | int numElem = UnionFind.ElementCount; | 100 | int numElem = UnionFind.ElementCount; |
101 | 101 | ||
102 | int endIslandIndex = 1; | 102 | int endIslandIndex = 1; |
103 | int startIslandIndex; | 103 | int startIslandIndex; |
104 | 104 | ||
105 | //update the sleeping state for bodies, if all are sleeping | 105 | //update the sleeping state for bodies, if all are sleeping |
106 | for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) | 106 | for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) |
107 | { | 107 | { |
108 | int islandId = UnionFind[startIslandIndex].ID; | 108 | int islandId = UnionFind[startIslandIndex].ID; |
109 | for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++) | 109 | for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++) |
110 | { | 110 | { |
111 | } | 111 | } |
112 | 112 | ||
113 | //int numSleeping = 0; | 113 | //int numSleeping = 0; |
114 | 114 | ||
115 | bool allSleeping = true; | 115 | bool allSleeping = true; |
116 | 116 | ||
117 | int idx; | 117 | int idx; |
118 | for (idx = startIslandIndex; idx < endIslandIndex; idx++) | 118 | for (idx = startIslandIndex; idx < endIslandIndex; idx++) |
119 | { | 119 | { |
120 | int i = UnionFind[idx].Size; | 120 | int i = UnionFind[idx].Size; |
121 | 121 | ||
122 | CollisionObject colObjA = collisionObjects[i]; | 122 | CollisionObject colObjA = collisionObjects[i]; |
123 | if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) | 123 | if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) |
124 | { | 124 | { |
125 | Console.WriteLine("error in island management"); | 125 | Console.WriteLine("error in island management"); |
126 | } | 126 | } |
127 | 127 | ||
128 | BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); | 128 | BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); |
129 | if (colObjA.IslandTag == islandId) | 129 | if (colObjA.IslandTag == islandId) |
130 | { | 130 | { |
131 | if (colObjA.ActivationState == ActivationState.Active) | 131 | if (colObjA.ActivationState == ActivationState.Active) |
132 | { | 132 | { |
133 | allSleeping = false; | 133 | allSleeping = false; |
134 | } | 134 | } |
135 | if (colObjA.ActivationState == ActivationState.DisableDeactivation) | 135 | if (colObjA.ActivationState == ActivationState.DisableDeactivation) |
136 | { | 136 | { |
137 | allSleeping = false; | 137 | allSleeping = false; |
138 | } | 138 | } |
139 | } | 139 | } |
140 | } | 140 | } |
141 | 141 | ||
142 | 142 | ||
143 | if (allSleeping) | 143 | if (allSleeping) |
144 | { | 144 | { |
145 | for (idx = startIslandIndex; idx < endIslandIndex; idx++) | 145 | for (idx = startIslandIndex; idx < endIslandIndex; idx++) |
146 | { | 146 | { |
147 | int i = UnionFind[idx].Size; | 147 | int i = UnionFind[idx].Size; |
148 | CollisionObject colObjA = collisionObjects[i]; | 148 | CollisionObject colObjA = collisionObjects[i]; |
149 | if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) | 149 | if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) |
150 | { | 150 | { |
151 | Console.WriteLine("error in island management"); | 151 | Console.WriteLine("error in island management"); |
152 | } | 152 | } |
153 | 153 | ||
154 | BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); | 154 | BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); |
155 | 155 | ||
156 | if (colObjA.IslandTag == islandId) | 156 | if (colObjA.IslandTag == islandId) |
157 | { | 157 | { |
158 | colObjA.ActivationState =ActivationState.IslandSleeping; | 158 | colObjA.ActivationState =ActivationState.IslandSleeping; |
159 | } | 159 | } |
160 | } | 160 | } |
161 | } | 161 | } |
162 | else | 162 | else |
163 | { | 163 | { |
164 | for (idx = startIslandIndex; idx < endIslandIndex; idx++) | 164 | for (idx = startIslandIndex; idx < endIslandIndex; idx++) |
165 | { | 165 | { |
166 | int i = UnionFind[idx].Size; | 166 | int i = UnionFind[idx].Size; |
167 | 167 | ||
168 | CollisionObject colObjA = collisionObjects[i]; | 168 | CollisionObject colObjA = collisionObjects[i]; |
169 | if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) | 169 | if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) |
170 | { | 170 | { |
171 | Console.WriteLine("error in island management"); | 171 | Console.WriteLine("error in island management"); |
172 | } | 172 | } |
173 | 173 | ||
174 | BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); | 174 | BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); |
175 | 175 | ||
176 | if (colObjA.IslandTag == islandId) | 176 | if (colObjA.IslandTag == islandId) |
177 | { | 177 | { |
178 | if (colObjA.ActivationState == ActivationState.IslandSleeping) | 178 | if (colObjA.ActivationState == ActivationState.IslandSleeping) |
179 | { | 179 | { |
180 | colObjA.ActivationState = ActivationState.WantsDeactivation; | 180 | colObjA.ActivationState = ActivationState.WantsDeactivation; |
181 | } | 181 | } |
182 | } | 182 | } |
183 | } | 183 | } |
184 | } | 184 | } |
185 | } | 185 | } |
186 | 186 | ||
187 | //int maxNumManifolds = dispatcher.ManifoldCount; | 187 | //int maxNumManifolds = dispatcher.ManifoldCount; |
188 | List<PersistentManifold> islandmanifold = new List<PersistentManifold>(dispatcher.ManifoldCount); | 188 | List<PersistentManifold> islandmanifold = new List<PersistentManifold>(dispatcher.ManifoldCount); |
189 | 189 | ||
190 | for (int i = 0; i < dispatcher.ManifoldCount; i++) | 190 | for (int i = 0; i < dispatcher.ManifoldCount; i++) |
191 | { | 191 | { |
192 | PersistentManifold manifold = dispatcher.GetManifoldByIndex(i); | 192 | PersistentManifold manifold = dispatcher.GetManifoldByIndex(i); |
193 | 193 | ||
194 | CollisionObject colObjA = manifold.BodyA as CollisionObject; | 194 | CollisionObject colObjA = manifold.BodyA as CollisionObject; |
195 | CollisionObject colObjB = manifold.BodyB as CollisionObject; | 195 | CollisionObject colObjB = manifold.BodyB as CollisionObject; |
196 | 196 | ||
197 | //todo: check sleeping conditions! | 197 | //todo: check sleeping conditions! |
198 | if (((colObjA != null) && colObjA.ActivationState != ActivationState.IslandSleeping) || | 198 | if (((colObjA != null) && colObjA.ActivationState != ActivationState.IslandSleeping) || |
199 | ((colObjB != null) && colObjB.ActivationState != ActivationState.IslandSleeping)) | 199 | ((colObjB != null) && colObjB.ActivationState != ActivationState.IslandSleeping)) |
200 | { | 200 | { |
201 | 201 | ||
202 | //kinematic objects don't merge islands, but wake up all connected objects | 202 | //kinematic objects don't merge islands, but wake up all connected objects |
203 | if (colObjA.IsStaticOrKinematicObject && colObjA.ActivationState != ActivationState.IslandSleeping) | 203 | if (colObjA.IsStaticOrKinematicObject && colObjA.ActivationState != ActivationState.IslandSleeping) |
204 | { | 204 | { |
205 | colObjB.Activate(); | 205 | colObjB.Activate(); |
206 | } | 206 | } |
207 | if (colObjB.IsStaticOrKinematicObject && colObjB.ActivationState != ActivationState.IslandSleeping) | 207 | if (colObjB.IsStaticOrKinematicObject && colObjB.ActivationState != ActivationState.IslandSleeping) |
208 | { | 208 | { |
209 | colObjA.Activate(); | 209 | colObjA.Activate(); |
210 | } | 210 | } |
211 | 211 | ||
212 | //filtering for response | 212 | //filtering for response |
213 | if (dispatcher.NeedsResponse(colObjA, colObjB)) | 213 | if (dispatcher.NeedsResponse(colObjA, colObjB)) |
214 | islandmanifold.Add(manifold); | 214 | islandmanifold.Add(manifold); |
215 | } | 215 | } |
216 | } | 216 | } |
217 | 217 | ||
218 | int numManifolds = islandmanifold.Count; | 218 | int numManifolds = islandmanifold.Count; |
219 | 219 | ||
220 | // Sort manifolds, based on islands | 220 | // Sort manifolds, based on islands |
221 | // Sort the vector using predicate and std::sort | 221 | // Sort the vector using predicate and std::sort |
222 | islandmanifold.Sort(new Comparison<PersistentManifold>(PersistentManifoldSortPredicate)); | 222 | islandmanifold.Sort(new Comparison<PersistentManifold>(PersistentManifoldSortPredicate)); |
223 | 223 | ||
224 | //now process all active islands (sets of manifolds for now) | 224 | //now process all active islands (sets of manifolds for now) |
225 | int startManifoldIndex = 0; | 225 | int startManifoldIndex = 0; |
226 | int endManifoldIndex = 1; | 226 | int endManifoldIndex = 1; |
227 | 227 | ||
228 | List<CollisionObject> islandBodies = new List<CollisionObject>(); | 228 | List<CollisionObject> islandBodies = new List<CollisionObject>(); |
229 | 229 | ||
230 | for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) | 230 | for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) |
231 | { | 231 | { |
232 | int islandId = UnionFind[startIslandIndex].ID; | 232 | int islandId = UnionFind[startIslandIndex].ID; |
233 | bool islandSleeping = false; | 233 | bool islandSleeping = false; |
234 | for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++) | 234 | for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++) |
235 | { | 235 | { |
236 | int i = UnionFind[endIslandIndex].Size; | 236 | int i = UnionFind[endIslandIndex].Size; |
237 | CollisionObject colObjA = collisionObjects[i]; | 237 | CollisionObject colObjA = collisionObjects[i]; |
238 | islandBodies.Add(colObjA); | 238 | islandBodies.Add(colObjA); |
239 | if (!colObjA.IsActive) | 239 | if (!colObjA.IsActive) |
240 | islandSleeping = true; | 240 | islandSleeping = true; |
241 | } | 241 | } |
242 | 242 | ||
243 | //find the accompanying contact manifold for this islandId | 243 | //find the accompanying contact manifold for this islandId |
244 | int numIslandManifolds = 0; | 244 | int numIslandManifolds = 0; |
245 | List<PersistentManifold> startManifold = new List<PersistentManifold>(numIslandManifolds); | 245 | List<PersistentManifold> startManifold = new List<PersistentManifold>(numIslandManifolds); |
246 | 246 | ||
247 | if (startManifoldIndex < numManifolds) | 247 | if (startManifoldIndex < numManifolds) |
248 | { | 248 | { |
249 | int curIslandID = GetIslandId(islandmanifold[startManifoldIndex]); | 249 | int curIslandID = GetIslandId(islandmanifold[startManifoldIndex]); |
250 | if (curIslandID == islandId) | 250 | if (curIslandID == islandId) |
251 | { | 251 | { |
252 | for (int k = startManifoldIndex; k < islandmanifold.Count; k++) | 252 | for (int k = startManifoldIndex; k < islandmanifold.Count; k++) |
253 | { | 253 | { |
254 | startManifold.Add(islandmanifold[k]); | 254 | startManifold.Add(islandmanifold[k]); |
255 | } | 255 | } |
256 | for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(islandmanifold[endManifoldIndex])); endManifoldIndex++) { } | 256 | for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(islandmanifold[endManifoldIndex])); endManifoldIndex++) { } |
257 | 257 | ||
258 | // Process the actual simulation, only if not sleeping/deactivated | 258 | // Process the actual simulation, only if not sleeping/deactivated |
259 | numIslandManifolds = endManifoldIndex - startManifoldIndex; | 259 | numIslandManifolds = endManifoldIndex - startManifoldIndex; |
260 | } | 260 | } |
261 | } | 261 | } |
262 | 262 | ||
263 | if (!islandSleeping) | 263 | if (!islandSleeping) |
264 | { | 264 | { |
265 | callback.ProcessIsland(islandBodies, startManifold, numIslandManifolds, islandId); | 265 | callback.ProcessIsland(islandBodies, startManifold, numIslandManifolds, islandId); |
266 | } | 266 | } |
267 | 267 | ||
268 | if (numIslandManifolds != 0) | 268 | if (numIslandManifolds != 0) |
269 | { | 269 | { |
270 | startManifoldIndex = endManifoldIndex; | 270 | startManifoldIndex = endManifoldIndex; |
271 | } | 271 | } |
272 | 272 | ||
273 | islandBodies.Clear(); | 273 | islandBodies.Clear(); |
274 | } | 274 | } |
275 | } | 275 | } |
276 | 276 | ||
277 | private static int GetIslandId(PersistentManifold lhs) | 277 | private static int GetIslandId(PersistentManifold lhs) |
278 | { | 278 | { |
279 | int islandId; | 279 | int islandId; |
280 | CollisionObject rcolObjA = lhs.BodyA as CollisionObject; | 280 | CollisionObject rcolObjA = lhs.BodyA as CollisionObject; |
281 | CollisionObject rcolObjB = lhs.BodyB as CollisionObject; | 281 | CollisionObject rcolObjB = lhs.BodyB as CollisionObject; |
282 | islandId = rcolObjA.IslandTag >= 0 ? rcolObjA.IslandTag : rcolObjB.IslandTag; | 282 | islandId = rcolObjA.IslandTag >= 0 ? rcolObjA.IslandTag : rcolObjB.IslandTag; |
283 | return islandId; | 283 | return islandId; |
284 | } | 284 | } |
285 | 285 | ||
286 | private static int PersistentManifoldSortPredicate(PersistentManifold lhs, PersistentManifold rhs) | 286 | private static int PersistentManifoldSortPredicate(PersistentManifold lhs, PersistentManifold rhs) |
287 | { | 287 | { |
288 | int rIslandIdA, lIslandIdB; | 288 | int rIslandIdA, lIslandIdB; |
289 | rIslandIdA = GetIslandId(rhs); | 289 | rIslandIdA = GetIslandId(rhs); |
290 | lIslandIdB = GetIslandId(lhs); | 290 | lIslandIdB = GetIslandId(lhs); |
291 | //return lIslandId0 < rIslandId0; | 291 | //return lIslandId0 < rIslandId0; |
292 | if (lIslandIdB < rIslandIdA) | 292 | if (lIslandIdB < rIslandIdA) |
293 | return -1; | 293 | return -1; |
294 | //else if (lIslandIdB > rIslandIdA) | 294 | //else if (lIslandIdB > rIslandIdA) |
295 | // return 1; | 295 | // return 1; |
296 | return 1; | 296 | return 1; |
297 | } | 297 | } |
298 | 298 | ||
299 | public interface IIslandCallback | 299 | public interface IIslandCallback |
300 | { | 300 | { |
301 | void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID); | 301 | void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID); |
302 | } | 302 | } |
303 | } | 303 | } |
304 | } | 304 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs index 8933638..2b4e801 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs | |||
@@ -1,270 +1,270 @@ | |||
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 | /// SphereBoxCollisionAlgorithm provides sphere-box collision detection. | 30 | /// SphereBoxCollisionAlgorithm provides sphere-box collision detection. |
31 | /// Other features are frame-coherency (persistent data) and collision response. | 31 | /// Other features are frame-coherency (persistent data) and collision response. |
32 | /// </summary> | 32 | /// </summary> |
33 | public class SphereBoxCollisionAlgorithm : CollisionAlgorithm, IDisposable | 33 | public class SphereBoxCollisionAlgorithm : CollisionAlgorithm, IDisposable |
34 | { | 34 | { |
35 | private bool _ownManifold; | 35 | private bool _ownManifold; |
36 | private PersistentManifold _manifold; | 36 | private PersistentManifold _manifold; |
37 | private bool _isSwapped; | 37 | private bool _isSwapped; |
38 | 38 | ||
39 | public SphereBoxCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject collisionObjectA, CollisionObject collisionObjectB, bool isSwapped) | 39 | public SphereBoxCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject collisionObjectA, CollisionObject collisionObjectB, bool isSwapped) |
40 | : base(collisionAlgorithmConstructionInfo) | 40 | : base(collisionAlgorithmConstructionInfo) |
41 | { | 41 | { |
42 | _ownManifold = false; | 42 | _ownManifold = false; |
43 | _manifold = manifold; | 43 | _manifold = manifold; |
44 | _isSwapped = isSwapped; | 44 | _isSwapped = isSwapped; |
45 | 45 | ||
46 | CollisionObject sphereObject = _isSwapped ? collisionObjectB : collisionObjectA; | 46 | CollisionObject sphereObject = _isSwapped ? collisionObjectB : collisionObjectA; |
47 | CollisionObject boxObject = _isSwapped ? collisionObjectA : collisionObjectB; | 47 | CollisionObject boxObject = _isSwapped ? collisionObjectA : collisionObjectB; |
48 | 48 | ||
49 | if (_manifold == null && Dispatcher.NeedsCollision(sphereObject, boxObject)) | 49 | if (_manifold == null && Dispatcher.NeedsCollision(sphereObject, boxObject)) |
50 | { | 50 | { |
51 | _manifold = Dispatcher.GetNewManifold(sphereObject, boxObject); | 51 | _manifold = Dispatcher.GetNewManifold(sphereObject, boxObject); |
52 | _ownManifold = true; | 52 | _ownManifold = true; |
53 | } | 53 | } |
54 | } | 54 | } |
55 | 55 | ||
56 | public float GetSphereDistance(CollisionObject boxObject, out Vector3 pointOnBox, out Vector3 pointOnSphere, Vector3 sphereCenter, float radius) | 56 | public float GetSphereDistance(CollisionObject boxObject, out Vector3 pointOnBox, out Vector3 pointOnSphere, Vector3 sphereCenter, float radius) |
57 | { | 57 | { |
58 | pointOnBox = new Vector3(); | 58 | pointOnBox = new Vector3(); |
59 | pointOnSphere = new Vector3(); | 59 | pointOnSphere = new Vector3(); |
60 | 60 | ||
61 | float margins; | 61 | float margins; |
62 | Vector3[] bounds = new Vector3[2]; | 62 | Vector3[] bounds = new Vector3[2]; |
63 | BoxShape boxShape = boxObject.CollisionShape as BoxShape; | 63 | BoxShape boxShape = boxObject.CollisionShape as BoxShape; |
64 | 64 | ||
65 | bounds[0] = -boxShape.HalfExtents; | 65 | bounds[0] = -boxShape.HalfExtents; |
66 | bounds[1] = boxShape.HalfExtents; | 66 | bounds[1] = boxShape.HalfExtents; |
67 | 67 | ||
68 | margins = boxShape.Margin; //also add sphereShape margin? | 68 | margins = boxShape.Margin; //also add sphereShape margin? |
69 | 69 | ||
70 | Matrix m44T = boxObject.WorldTransform; | 70 | Matrix m44T = boxObject.WorldTransform; |
71 | 71 | ||
72 | Vector3[] boundsVec = new Vector3[2]; | 72 | Vector3[] boundsVec = new Vector3[2]; |
73 | float penetration; | 73 | float penetration; |
74 | 74 | ||
75 | boundsVec[0] = bounds[0]; | 75 | boundsVec[0] = bounds[0]; |
76 | boundsVec[1] = bounds[1]; | 76 | boundsVec[1] = bounds[1]; |
77 | 77 | ||
78 | Vector3 marginsVec = new Vector3(margins, margins, margins); | 78 | Vector3 marginsVec = new Vector3(margins, margins, margins); |
79 | 79 | ||
80 | // add margins | 80 | // add margins |
81 | bounds[0] += marginsVec; | 81 | bounds[0] += marginsVec; |
82 | bounds[1] -= marginsVec; | 82 | bounds[1] -= marginsVec; |
83 | 83 | ||
84 | ///////////////////////////////////////////////// | 84 | ///////////////////////////////////////////////// |
85 | 85 | ||
86 | Vector3 tmp, prel, normal, v3P; | 86 | Vector3 tmp, prel, normal, v3P; |
87 | Vector3[] n = new Vector3[6]; | 87 | Vector3[] n = new Vector3[6]; |
88 | float sep = 10000000.0f, sepThis; | 88 | float sep = 10000000.0f, sepThis; |
89 | 89 | ||
90 | n[0] = new Vector3(-1.0f, 0.0f, 0.0f); | 90 | n[0] = new Vector3(-1.0f, 0.0f, 0.0f); |
91 | n[1] = new Vector3(0.0f, -1.0f, 0.0f); | 91 | n[1] = new Vector3(0.0f, -1.0f, 0.0f); |
92 | n[2] = new Vector3(0.0f, 0.0f, -1.0f); | 92 | n[2] = new Vector3(0.0f, 0.0f, -1.0f); |
93 | n[3] = new Vector3(1.0f, 0.0f, 0.0f); | 93 | n[3] = new Vector3(1.0f, 0.0f, 0.0f); |
94 | n[4] = new Vector3(0.0f, 1.0f, 0.0f); | 94 | n[4] = new Vector3(0.0f, 1.0f, 0.0f); |
95 | n[5] = new Vector3(0.0f, 0.0f, 1.0f); | 95 | n[5] = new Vector3(0.0f, 0.0f, 1.0f); |
96 | 96 | ||
97 | // convert point in local space | 97 | // convert point in local space |
98 | prel = MathHelper.InvXForm(m44T, sphereCenter); | 98 | prel = MathHelper.InvXForm(m44T, sphereCenter); |
99 | 99 | ||
100 | bool found = false; | 100 | bool found = false; |
101 | 101 | ||
102 | v3P = prel; | 102 | v3P = prel; |
103 | 103 | ||
104 | for (int i = 0; i < 6; i++) | 104 | for (int i = 0; i < 6; i++) |
105 | { | 105 | { |
106 | int j = i < 3 ? 0 : 1; | 106 | int j = i < 3 ? 0 : 1; |
107 | if ((sepThis = (Vector3.Dot(v3P - bounds[j], n[i]))) > 0.0f) | 107 | if ((sepThis = (Vector3.Dot(v3P - bounds[j], n[i]))) > 0.0f) |
108 | { | 108 | { |
109 | v3P = v3P - n[i] * sepThis; | 109 | v3P = v3P - n[i] * sepThis; |
110 | found = true; | 110 | found = true; |
111 | } | 111 | } |
112 | } | 112 | } |
113 | 113 | ||
114 | // | 114 | // |
115 | 115 | ||
116 | if (found) | 116 | if (found) |
117 | { | 117 | { |
118 | bounds[0] = boundsVec[0]; | 118 | bounds[0] = boundsVec[0]; |
119 | bounds[1] = boundsVec[1]; | 119 | bounds[1] = boundsVec[1]; |
120 | 120 | ||
121 | normal = Vector3.Normalize(prel - v3P); | 121 | normal = Vector3.Normalize(prel - v3P); |
122 | pointOnBox = v3P + normal * margins; | 122 | pointOnBox = v3P + normal * margins; |
123 | pointOnSphere = prel - normal * radius; | 123 | pointOnSphere = prel - normal * radius; |
124 | 124 | ||
125 | if ((Vector3.Dot(pointOnSphere - pointOnBox, normal)) > 0.0f) | 125 | if ((Vector3.Dot(pointOnSphere - pointOnBox, normal)) > 0.0f) |
126 | { | 126 | { |
127 | return 1.0f; | 127 | return 1.0f; |
128 | } | 128 | } |
129 | 129 | ||
130 | // transform back in world space | 130 | // transform back in world space |
131 | tmp = MathHelper.MatrixToVector(m44T, pointOnBox); | 131 | tmp = MathHelper.MatrixToVector(m44T, pointOnBox); |
132 | pointOnBox = tmp; | 132 | pointOnBox = tmp; |
133 | tmp = MathHelper.MatrixToVector(m44T, pointOnSphere); | 133 | tmp = MathHelper.MatrixToVector(m44T, pointOnSphere); |
134 | pointOnSphere = tmp; | 134 | pointOnSphere = tmp; |
135 | float seps2 = (pointOnBox - pointOnSphere).LengthSquared(); | 135 | float seps2 = (pointOnBox - pointOnSphere).LengthSquared(); |
136 | 136 | ||
137 | //if this fails, fallback into deeper penetration case, below | 137 | //if this fails, fallback into deeper penetration case, below |
138 | if (seps2 > MathHelper.Epsilon) | 138 | if (seps2 > MathHelper.Epsilon) |
139 | { | 139 | { |
140 | sep = -(float)Math.Sqrt(seps2); | 140 | sep = -(float)Math.Sqrt(seps2); |
141 | normal = (pointOnBox - pointOnSphere); | 141 | normal = (pointOnBox - pointOnSphere); |
142 | normal *= 1f / sep; | 142 | normal *= 1f / sep; |
143 | } | 143 | } |
144 | return sep; | 144 | return sep; |
145 | } | 145 | } |
146 | 146 | ||
147 | ////////////////////////////////////////////////// | 147 | ////////////////////////////////////////////////// |
148 | // Deep penetration case | 148 | // Deep penetration case |
149 | 149 | ||
150 | penetration = GetSpherePenetration(boxObject, ref pointOnBox, ref pointOnSphere, sphereCenter, radius, bounds[0], bounds[1]); | 150 | penetration = GetSpherePenetration(boxObject, ref pointOnBox, ref pointOnSphere, sphereCenter, radius, bounds[0], bounds[1]); |
151 | 151 | ||
152 | bounds[0] = boundsVec[0]; | 152 | bounds[0] = boundsVec[0]; |
153 | bounds[1] = boundsVec[1]; | 153 | bounds[1] = boundsVec[1]; |
154 | 154 | ||
155 | if (penetration <= 0.0f) | 155 | if (penetration <= 0.0f) |
156 | return (penetration - margins); | 156 | return (penetration - margins); |
157 | else | 157 | else |
158 | return 1.0f; | 158 | return 1.0f; |
159 | } | 159 | } |
160 | 160 | ||
161 | public float GetSpherePenetration(CollisionObject boxObject, ref Vector3 pointOnBox, ref Vector3 pointOnSphere, Vector3 sphereCenter, float radius, Vector3 aabbMin, Vector3 aabbMax) | 161 | public float GetSpherePenetration(CollisionObject boxObject, ref Vector3 pointOnBox, ref Vector3 pointOnSphere, Vector3 sphereCenter, float radius, Vector3 aabbMin, Vector3 aabbMax) |
162 | { | 162 | { |
163 | Vector3[] bounds = new Vector3[2]; | 163 | Vector3[] bounds = new Vector3[2]; |
164 | 164 | ||
165 | bounds[0] = aabbMin; | 165 | bounds[0] = aabbMin; |
166 | bounds[1] = aabbMax; | 166 | bounds[1] = aabbMax; |
167 | 167 | ||
168 | Vector3 p0 = new Vector3(), tmp, prel, normal = new Vector3(); | 168 | Vector3 p0 = new Vector3(), tmp, prel, normal = new Vector3(); |
169 | Vector3[] n = new Vector3[6]; | 169 | Vector3[] n = new Vector3[6]; |
170 | float sep = -10000000.0f, sepThis; | 170 | float sep = -10000000.0f, sepThis; |
171 | 171 | ||
172 | n[0] = new Vector3(-1.0f, 0.0f, 0.0f); | 172 | n[0] = new Vector3(-1.0f, 0.0f, 0.0f); |
173 | n[1] = new Vector3(0.0f, -1.0f, 0.0f); | 173 | n[1] = new Vector3(0.0f, -1.0f, 0.0f); |
174 | n[2] = new Vector3(0.0f, 0.0f, -1.0f); | 174 | n[2] = new Vector3(0.0f, 0.0f, -1.0f); |
175 | n[3] = new Vector3(1.0f, 0.0f, 0.0f); | 175 | n[3] = new Vector3(1.0f, 0.0f, 0.0f); |
176 | n[4] = new Vector3(0.0f, 1.0f, 0.0f); | 176 | n[4] = new Vector3(0.0f, 1.0f, 0.0f); |
177 | n[5] = new Vector3(0.0f, 0.0f, 1.0f); | 177 | n[5] = new Vector3(0.0f, 0.0f, 1.0f); |
178 | 178 | ||
179 | Matrix m44T = boxObject.WorldTransform; | 179 | Matrix m44T = boxObject.WorldTransform; |
180 | 180 | ||
181 | // convert point in local space | 181 | // convert point in local space |
182 | prel = MathHelper.InvXForm(m44T, sphereCenter); | 182 | prel = MathHelper.InvXForm(m44T, sphereCenter); |
183 | 183 | ||
184 | /////////// | 184 | /////////// |
185 | 185 | ||
186 | for (int i = 0; i < 6; i++) | 186 | for (int i = 0; i < 6; i++) |
187 | { | 187 | { |
188 | int j = i < 3 ? 0 : 1; | 188 | int j = i < 3 ? 0 : 1; |
189 | if ((sepThis = (Vector3.Dot(prel - bounds[j], n[i])) - radius) > 0.0f) return 1.0f; | 189 | if ((sepThis = (Vector3.Dot(prel - bounds[j], n[i])) - radius) > 0.0f) return 1.0f; |
190 | if (sepThis > sep) | 190 | if (sepThis > sep) |
191 | { | 191 | { |
192 | p0 = bounds[j]; | 192 | p0 = bounds[j]; |
193 | normal = n[i]; | 193 | normal = n[i]; |
194 | sep = sepThis; | 194 | sep = sepThis; |
195 | } | 195 | } |
196 | } | 196 | } |
197 | 197 | ||
198 | pointOnBox = prel - normal * (Vector3.Dot(normal, (prel - p0))); | 198 | pointOnBox = prel - normal * (Vector3.Dot(normal, (prel - p0))); |
199 | pointOnSphere = pointOnBox + normal * sep; | 199 | pointOnSphere = pointOnBox + normal * sep; |
200 | 200 | ||
201 | // transform back in world space | 201 | // transform back in world space |
202 | tmp = MathHelper.MatrixToVector(m44T, pointOnBox); | 202 | tmp = MathHelper.MatrixToVector(m44T, pointOnBox); |
203 | pointOnBox = tmp; | 203 | pointOnBox = tmp; |
204 | tmp = MathHelper.MatrixToVector(m44T, pointOnSphere); | 204 | tmp = MathHelper.MatrixToVector(m44T, pointOnSphere); |
205 | pointOnSphere = tmp; | 205 | pointOnSphere = tmp; |
206 | normal = Vector3.Normalize(pointOnBox - pointOnSphere); | 206 | normal = Vector3.Normalize(pointOnBox - pointOnSphere); |
207 | 207 | ||
208 | return sep; | 208 | return sep; |
209 | } | 209 | } |
210 | 210 | ||
211 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | 211 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) |
212 | { | 212 | { |
213 | if (_manifold == null) | 213 | if (_manifold == null) |
214 | return; | 214 | return; |
215 | 215 | ||
216 | CollisionObject sphereObject = _isSwapped ? bodyB : bodyA; | 216 | CollisionObject sphereObject = _isSwapped ? bodyB : bodyA; |
217 | CollisionObject boxObject = _isSwapped ? bodyA : bodyB; | 217 | CollisionObject boxObject = _isSwapped ? bodyA : bodyB; |
218 | 218 | ||
219 | SphereShape sphereA = sphereObject.CollisionShape as SphereShape; | 219 | SphereShape sphereA = sphereObject.CollisionShape as SphereShape; |
220 | 220 | ||
221 | Vector3 pOnBox, pOnSphere; | 221 | Vector3 pOnBox, pOnSphere; |
222 | Vector3 sphereCenter = sphereObject.WorldTransform.Translation; | 222 | Vector3 sphereCenter = sphereObject.WorldTransform.Translation; |
223 | float radius = sphereA.Radius; | 223 | float radius = sphereA.Radius; |
224 | 224 | ||
225 | float dist = GetSphereDistance(boxObject, out pOnBox, out pOnSphere, sphereCenter, radius); | 225 | float dist = GetSphereDistance(boxObject, out pOnBox, out pOnSphere, sphereCenter, radius); |
226 | 226 | ||
227 | if (dist < MathHelper.Epsilon) | 227 | if (dist < MathHelper.Epsilon) |
228 | { | 228 | { |
229 | Vector3 normalOnSurfaceB = Vector3.Normalize(pOnBox - pOnSphere); | 229 | Vector3 normalOnSurfaceB = Vector3.Normalize(pOnBox - pOnSphere); |
230 | 230 | ||
231 | // report a contact. internally this will be kept persistent, and contact reduction is done | 231 | // report a contact. internally this will be kept persistent, and contact reduction is done |
232 | resultOut.SetPersistentManifold(_manifold); | 232 | resultOut.SetPersistentManifold(_manifold); |
233 | resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, dist); | 233 | resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, dist); |
234 | } | 234 | } |
235 | } | 235 | } |
236 | 236 | ||
237 | public override float CalculateTimeOfImpact(CollisionObject collisionObjectA, CollisionObject collisionObjectB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | 237 | public override float CalculateTimeOfImpact(CollisionObject collisionObjectA, CollisionObject collisionObjectB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) |
238 | { | 238 | { |
239 | //not yet | 239 | //not yet |
240 | return 1; | 240 | return 1; |
241 | } | 241 | } |
242 | 242 | ||
243 | public class CreateFunc : CollisionAlgorithmCreateFunction | 243 | public class CreateFunc : CollisionAlgorithmCreateFunction |
244 | { | 244 | { |
245 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | 245 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) |
246 | { | 246 | { |
247 | if (!IsSwapped) | 247 | if (!IsSwapped) |
248 | return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, false); | 248 | return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, false); |
249 | else | 249 | else |
250 | return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, true); | 250 | return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, true); |
251 | } | 251 | } |
252 | } | 252 | } |
253 | 253 | ||
254 | #region IDisposable Members | 254 | #region IDisposable Members |
255 | public void Dispose() | 255 | public void Dispose() |
256 | { | 256 | { |
257 | Dispose(true); | 257 | Dispose(true); |
258 | } | 258 | } |
259 | 259 | ||
260 | public void Dispose(bool disposing) | 260 | public void Dispose(bool disposing) |
261 | { | 261 | { |
262 | if (disposing && _ownManifold) | 262 | if (disposing && _ownManifold) |
263 | { | 263 | { |
264 | if (_manifold != null) | 264 | if (_manifold != null) |
265 | Dispatcher.ReleaseManifold(_manifold); | 265 | Dispatcher.ReleaseManifold(_manifold); |
266 | } | 266 | } |
267 | } | 267 | } |
268 | #endregion | 268 | #endregion |
269 | } | 269 | } |
270 | } | 270 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs index 7a76d24..3a780c1 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs | |||
@@ -1,104 +1,104 @@ | |||
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 | public class SphereSphereCollisionAlgorithm : CollisionAlgorithm | 29 | public class SphereSphereCollisionAlgorithm : CollisionAlgorithm |
30 | { | 30 | { |
31 | private bool _ownManifold; | 31 | private bool _ownManifold; |
32 | private PersistentManifold _manifold; | 32 | private PersistentManifold _manifold; |
33 | 33 | ||
34 | public SphereSphereCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | 34 | public SphereSphereCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) |
35 | : base(collisionAlgorithmConstructionInfo) | 35 | : base(collisionAlgorithmConstructionInfo) |
36 | { | 36 | { |
37 | _ownManifold = false; | 37 | _ownManifold = false; |
38 | _manifold = manifold; | 38 | _manifold = manifold; |
39 | 39 | ||
40 | if (_manifold == null) | 40 | if (_manifold == null) |
41 | { | 41 | { |
42 | _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); | 42 | _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); |
43 | _ownManifold = true; | 43 | _ownManifold = true; |
44 | } | 44 | } |
45 | } | 45 | } |
46 | 46 | ||
47 | public SphereSphereCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) | 47 | public SphereSphereCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) |
48 | : base(collisionAlgorithmConstructionInfo) { } | 48 | : base(collisionAlgorithmConstructionInfo) { } |
49 | 49 | ||
50 | ~SphereSphereCollisionAlgorithm() | 50 | ~SphereSphereCollisionAlgorithm() |
51 | { | 51 | { |
52 | if (_ownManifold) | 52 | if (_ownManifold) |
53 | { | 53 | { |
54 | if (_manifold != null) | 54 | if (_manifold != null) |
55 | Dispatcher.ReleaseManifold(_manifold); | 55 | Dispatcher.ReleaseManifold(_manifold); |
56 | } | 56 | } |
57 | } | 57 | } |
58 | 58 | ||
59 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | 59 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) |
60 | { | 60 | { |
61 | if (_manifold == null) | 61 | if (_manifold == null) |
62 | return; | 62 | return; |
63 | 63 | ||
64 | SphereShape sphereA = bodyA.CollisionShape as SphereShape; | 64 | SphereShape sphereA = bodyA.CollisionShape as SphereShape; |
65 | SphereShape sphereB = bodyB.CollisionShape as SphereShape; | 65 | SphereShape sphereB = bodyB.CollisionShape as SphereShape; |
66 | 66 | ||
67 | Vector3 diff = bodyA.WorldTransform.Translation - bodyB.WorldTransform.Translation; | 67 | Vector3 diff = bodyA.WorldTransform.Translation - bodyB.WorldTransform.Translation; |
68 | float len = diff.Length(); | 68 | float len = diff.Length(); |
69 | float radiusA = sphereA.Radius; | 69 | float radiusA = sphereA.Radius; |
70 | float radiusB = sphereB.Radius; | 70 | float radiusB = sphereB.Radius; |
71 | 71 | ||
72 | //if distance positive, don't generate a new contact | 72 | //if distance positive, don't generate a new contact |
73 | if (len > (radiusA + radiusB)) | 73 | if (len > (radiusA + radiusB)) |
74 | return; | 74 | return; |
75 | 75 | ||
76 | //distance (negative means penetration) | 76 | //distance (negative means penetration) |
77 | float dist = len - (radiusA + radiusB); | 77 | float dist = len - (radiusA + radiusB); |
78 | 78 | ||
79 | Vector3 normalOnSurfaceB = diff / len; | 79 | Vector3 normalOnSurfaceB = diff / len; |
80 | //point on A (worldspace) | 80 | //point on A (worldspace) |
81 | Vector3 posA = bodyA.WorldTransform.Translation - radiusA * normalOnSurfaceB; | 81 | Vector3 posA = bodyA.WorldTransform.Translation - radiusA * normalOnSurfaceB; |
82 | //point on B (worldspace) | 82 | //point on B (worldspace) |
83 | Vector3 posB = bodyB.WorldTransform.Translation + radiusB * normalOnSurfaceB; | 83 | Vector3 posB = bodyB.WorldTransform.Translation + radiusB * normalOnSurfaceB; |
84 | 84 | ||
85 | // report a contact. internally this will be kept persistent, and contact reduction is done | 85 | // report a contact. internally this will be kept persistent, and contact reduction is done |
86 | resultOut.SetPersistentManifold(_manifold); | 86 | resultOut.SetPersistentManifold(_manifold); |
87 | resultOut.AddContactPoint(normalOnSurfaceB, posB, dist); | 87 | resultOut.AddContactPoint(normalOnSurfaceB, posB, dist); |
88 | } | 88 | } |
89 | 89 | ||
90 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | 90 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) |
91 | { | 91 | { |
92 | //not yet | 92 | //not yet |
93 | return 1f; | 93 | return 1f; |
94 | } | 94 | } |
95 | 95 | ||
96 | public class CreateFunc : CollisionAlgorithmCreateFunction | 96 | public class CreateFunc : CollisionAlgorithmCreateFunction |
97 | { | 97 | { |
98 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | 98 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) |
99 | { | 99 | { |
100 | return new SphereSphereCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB); | 100 | return new SphereSphereCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB); |
101 | } | 101 | } |
102 | } | 102 | } |
103 | } | 103 | } |
104 | } | 104 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs index 1ca5cfb..00dfaf1 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs | |||
@@ -1,100 +1,100 @@ | |||
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 | /// SphereSphereCollisionAlgorithm provides sphere-sphere collision detection. | 30 | /// SphereSphereCollisionAlgorithm provides sphere-sphere collision detection. |
31 | /// Other features are frame-coherency (persistent data) and collision response. | 31 | /// Other features are frame-coherency (persistent data) and collision response. |
32 | /// Also provides the most basic sample for custom/user btCollisionAlgorithm | 32 | /// Also provides the most basic sample for custom/user btCollisionAlgorithm |
33 | /// </summary> | 33 | /// </summary> |
34 | public class SphereTriangleCollisionAlgorithm : CollisionAlgorithm, IDisposable | 34 | public class SphereTriangleCollisionAlgorithm : CollisionAlgorithm, IDisposable |
35 | { | 35 | { |
36 | private bool _ownManifold; | 36 | private bool _ownManifold; |
37 | private PersistentManifold _manifold; | 37 | private PersistentManifold _manifold; |
38 | private bool _isSwapped; | 38 | private bool _isSwapped; |
39 | 39 | ||
40 | public SphereTriangleCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) | 40 | public SphereTriangleCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) |
41 | : base(collisionAlgorithmConstructionInfo) | 41 | : base(collisionAlgorithmConstructionInfo) |
42 | { | 42 | { |
43 | _ownManifold = false; | 43 | _ownManifold = false; |
44 | _manifold = manifold; | 44 | _manifold = manifold; |
45 | _isSwapped = isSwapped; | 45 | _isSwapped = isSwapped; |
46 | 46 | ||
47 | if (_manifold == null) | 47 | if (_manifold == null) |
48 | { | 48 | { |
49 | _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); | 49 | _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); |
50 | _ownManifold = true; | 50 | _ownManifold = true; |
51 | } | 51 | } |
52 | } | 52 | } |
53 | 53 | ||
54 | public SphereTriangleCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) | 54 | public SphereTriangleCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) |
55 | : base(collisionAlgorithmConstructionInfo) { } | 55 | : base(collisionAlgorithmConstructionInfo) { } |
56 | 56 | ||
57 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | 57 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) |
58 | { | 58 | { |
59 | if (_manifold == null) | 59 | if (_manifold == null) |
60 | return; | 60 | return; |
61 | 61 | ||
62 | SphereShape sphere = bodyA.CollisionShape as SphereShape; | 62 | SphereShape sphere = bodyA.CollisionShape as SphereShape; |
63 | TriangleShape triangle = bodyB.CollisionShape as TriangleShape; | 63 | TriangleShape triangle = bodyB.CollisionShape as TriangleShape; |
64 | 64 | ||
65 | /// report a contact. internally this will be kept persistent, and contact reduction is done | 65 | /// report a contact. internally this will be kept persistent, and contact reduction is done |
66 | resultOut.SetPersistentManifold(_manifold); | 66 | resultOut.SetPersistentManifold(_manifold); |
67 | SphereTriangleDetector detector = new SphereTriangleDetector(sphere, triangle); | 67 | SphereTriangleDetector detector = new SphereTriangleDetector(sphere, triangle); |
68 | 68 | ||
69 | DiscreteCollisionDetectorInterface.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | 69 | DiscreteCollisionDetectorInterface.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); |
70 | input.MaximumDistanceSquared = 1e30f;//todo: tighter bounds | 70 | input.MaximumDistanceSquared = 1e30f;//todo: tighter bounds |
71 | input.TransformA = bodyA.WorldTransform; | 71 | input.TransformA = bodyA.WorldTransform; |
72 | input.TransformB = bodyB.WorldTransform; | 72 | input.TransformB = bodyB.WorldTransform; |
73 | 73 | ||
74 | detector.GetClosestPoints(input, resultOut, null); | 74 | detector.GetClosestPoints(input, resultOut, null); |
75 | } | 75 | } |
76 | 76 | ||
77 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | 77 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) |
78 | { | 78 | { |
79 | //not yet | 79 | //not yet |
80 | return 1f; | 80 | return 1f; |
81 | } | 81 | } |
82 | 82 | ||
83 | public class CreateFunc : CollisionAlgorithmCreateFunction | 83 | public class CreateFunc : CollisionAlgorithmCreateFunction |
84 | { | 84 | { |
85 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | 85 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) |
86 | { | 86 | { |
87 | return new SphereTriangleCollisionAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, IsSwapped); | 87 | return new SphereTriangleCollisionAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, IsSwapped); |
88 | } | 88 | } |
89 | } | 89 | } |
90 | 90 | ||
91 | #region IDisposable Members | 91 | #region IDisposable Members |
92 | public void Dispose() | 92 | public void Dispose() |
93 | { | 93 | { |
94 | if (_ownManifold) | 94 | if (_ownManifold) |
95 | if (_manifold != null) | 95 | if (_manifold != null) |
96 | Dispatcher.ReleaseManifold(_manifold); | 96 | Dispatcher.ReleaseManifold(_manifold); |
97 | } | 97 | } |
98 | #endregion | 98 | #endregion |
99 | } | 99 | } |
100 | } | 100 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs index 865754a..4d578d8 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs | |||
@@ -1,214 +1,214 @@ | |||
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 | public class SphereTriangleDetector : DiscreteCollisionDetectorInterface | 29 | public class SphereTriangleDetector : DiscreteCollisionDetectorInterface |
30 | { | 30 | { |
31 | private SphereShape _sphere; | 31 | private SphereShape _sphere; |
32 | private TriangleShape _triangle; | 32 | private TriangleShape _triangle; |
33 | private const int MaxOverlap = 0; | 33 | private const int MaxOverlap = 0; |
34 | 34 | ||
35 | public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle) | 35 | public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle) |
36 | { | 36 | { |
37 | this._sphere = sphere; | 37 | this._sphere = sphere; |
38 | this._triangle = triangle; | 38 | this._triangle = triangle; |
39 | } | 39 | } |
40 | 40 | ||
41 | public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw) | 41 | public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw) |
42 | { | 42 | { |
43 | Matrix transformA = input.TransformA; | 43 | Matrix transformA = input.TransformA; |
44 | Matrix transformB = input.TransformB; | 44 | Matrix transformB = input.TransformB; |
45 | 45 | ||
46 | Vector3 point = new Vector3(); | 46 | Vector3 point = new Vector3(); |
47 | Vector3 normal = new Vector3(); | 47 | Vector3 normal = new Vector3(); |
48 | Single timeOfImpact = 1.0f; | 48 | Single timeOfImpact = 1.0f; |
49 | Single depth = 0.0f; | 49 | Single depth = 0.0f; |
50 | 50 | ||
51 | //move sphere into triangle space | 51 | //move sphere into triangle space |
52 | Matrix sphereInTr = MathHelper.InverseTimes(transformB, transformA); | 52 | Matrix sphereInTr = MathHelper.InverseTimes(transformB, transformA); |
53 | 53 | ||
54 | if (Collide(sphereInTr.Translation, point, normal, depth, timeOfImpact)) | 54 | if (Collide(sphereInTr.Translation, point, normal, depth, timeOfImpact)) |
55 | output.AddContactPoint(Vector3.TransformNormal(normal, transformB), Vector3.TransformNormal(point, transformB), depth); | 55 | output.AddContactPoint(Vector3.TransformNormal(normal, transformB), Vector3.TransformNormal(point, transformB), depth); |
56 | } | 56 | } |
57 | 57 | ||
58 | /// <summary> | 58 | /// <summary> |
59 | /// See also geometrictools.com | 59 | /// See also geometrictools.com |
60 | /// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv | 60 | /// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv |
61 | /// </summary> | 61 | /// </summary> |
62 | /// <param name="from"></param> | 62 | /// <param name="from"></param> |
63 | /// <param name="to"></param> | 63 | /// <param name="to"></param> |
64 | /// <param name="p"></param> | 64 | /// <param name="p"></param> |
65 | /// <param name="nearest"></param> | 65 | /// <param name="nearest"></param> |
66 | /// <returns></returns> | 66 | /// <returns></returns> |
67 | private float SegmentSquareDistance(Vector3 from, Vector3 to, Vector3 point, Vector3 nearest) | 67 | private float SegmentSquareDistance(Vector3 from, Vector3 to, Vector3 point, Vector3 nearest) |
68 | { | 68 | { |
69 | Vector3 diff = point - from; | 69 | Vector3 diff = point - from; |
70 | Vector3 v = to - from; | 70 | Vector3 v = to - from; |
71 | float t = Vector3.Dot(v, diff); | 71 | float t = Vector3.Dot(v, diff); |
72 | 72 | ||
73 | if (t > 0) | 73 | if (t > 0) |
74 | { | 74 | { |
75 | float dotVV = Vector3.Dot(v, v); | 75 | float dotVV = Vector3.Dot(v, v); |
76 | if (t < dotVV) | 76 | if (t < dotVV) |
77 | { | 77 | { |
78 | t /= dotVV; | 78 | t /= dotVV; |
79 | diff -= t * v; | 79 | diff -= t * v; |
80 | } | 80 | } |
81 | else | 81 | else |
82 | { | 82 | { |
83 | t = 1; | 83 | t = 1; |
84 | diff -= v; | 84 | diff -= v; |
85 | } | 85 | } |
86 | } | 86 | } |
87 | else | 87 | else |
88 | t = 0; | 88 | t = 0; |
89 | 89 | ||
90 | nearest = from + t * v; | 90 | nearest = from + t * v; |
91 | return Vector3.Dot(diff, diff); | 91 | return Vector3.Dot(diff, diff); |
92 | } | 92 | } |
93 | 93 | ||
94 | private bool Collide(Vector3 sphereCenter, Vector3 point, Vector3 resultNormal, float depth, float timeOfImpact) | 94 | private bool Collide(Vector3 sphereCenter, Vector3 point, Vector3 resultNormal, float depth, float timeOfImpact) |
95 | { | 95 | { |
96 | Vector3[] vertices = _triangle.Vertices; | 96 | Vector3[] vertices = _triangle.Vertices; |
97 | Vector3 c = sphereCenter; | 97 | Vector3 c = sphereCenter; |
98 | float r = _sphere.Radius; | 98 | float r = _sphere.Radius; |
99 | 99 | ||
100 | Vector3 delta = new Vector3(); | 100 | Vector3 delta = new Vector3(); |
101 | 101 | ||
102 | Vector3 normal = Vector3.Cross(vertices[1] - vertices[0], vertices[2] - vertices[0]); | 102 | Vector3 normal = Vector3.Cross(vertices[1] - vertices[0], vertices[2] - vertices[0]); |
103 | normal = Vector3.Normalize(normal); | 103 | normal = Vector3.Normalize(normal); |
104 | Vector3 p1ToCentre = c - vertices[0]; | 104 | Vector3 p1ToCentre = c - vertices[0]; |
105 | float distanceFromPlane = Vector3.Dot(p1ToCentre, normal); | 105 | float distanceFromPlane = Vector3.Dot(p1ToCentre, normal); |
106 | 106 | ||
107 | if (distanceFromPlane < 0) | 107 | if (distanceFromPlane < 0) |
108 | { | 108 | { |
109 | //triangle facing the other way | 109 | //triangle facing the other way |
110 | distanceFromPlane *= -1; | 110 | distanceFromPlane *= -1; |
111 | normal *= -1; | 111 | normal *= -1; |
112 | } | 112 | } |
113 | 113 | ||
114 | float contactMargin = PersistentManifold.ContactBreakingThreshold; | 114 | float contactMargin = PersistentManifold.ContactBreakingThreshold; |
115 | bool isInsideContactPlane = distanceFromPlane < r + contactMargin; | 115 | bool isInsideContactPlane = distanceFromPlane < r + contactMargin; |
116 | bool isInsideShellPlane = distanceFromPlane < r; | 116 | bool isInsideShellPlane = distanceFromPlane < r; |
117 | 117 | ||
118 | float deltaDotNormal = Vector3.Dot(delta, normal); | 118 | float deltaDotNormal = Vector3.Dot(delta, normal); |
119 | if (!isInsideShellPlane && deltaDotNormal >= 0.0f) | 119 | if (!isInsideShellPlane && deltaDotNormal >= 0.0f) |
120 | return false; | 120 | return false; |
121 | 121 | ||
122 | // Check for contact / intersection | 122 | // Check for contact / intersection |
123 | bool hasContact = false; | 123 | bool hasContact = false; |
124 | Vector3 contactPoint = new Vector3(); | 124 | Vector3 contactPoint = new Vector3(); |
125 | if (isInsideContactPlane) | 125 | if (isInsideContactPlane) |
126 | { | 126 | { |
127 | if (FaceContains(c, vertices, normal)) | 127 | if (FaceContains(c, vertices, normal)) |
128 | { | 128 | { |
129 | // Inside the contact wedge - touches a point on the shell plane | 129 | // Inside the contact wedge - touches a point on the shell plane |
130 | hasContact = true; | 130 | hasContact = true; |
131 | contactPoint = c - normal * distanceFromPlane; | 131 | contactPoint = c - normal * distanceFromPlane; |
132 | } | 132 | } |
133 | else | 133 | else |
134 | { | 134 | { |
135 | // Could be inside one of the contact capsules | 135 | // Could be inside one of the contact capsules |
136 | float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); | 136 | float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); |
137 | Vector3 nearestOnEdge = new Vector3(); | 137 | Vector3 nearestOnEdge = new Vector3(); |
138 | for (int i = 0; i < _triangle.EdgeCount; i++) | 138 | for (int i = 0; i < _triangle.EdgeCount; i++) |
139 | { | 139 | { |
140 | Vector3 pa, pb; | 140 | Vector3 pa, pb; |
141 | _triangle.GetEdge(i, out pa, out pb); | 141 | _triangle.GetEdge(i, out pa, out pb); |
142 | 142 | ||
143 | float distanceSqr = SegmentSquareDistance(pa, pb, c, nearestOnEdge); | 143 | float distanceSqr = SegmentSquareDistance(pa, pb, c, nearestOnEdge); |
144 | if (distanceSqr < contactCapsuleRadiusSqr) | 144 | if (distanceSqr < contactCapsuleRadiusSqr) |
145 | { | 145 | { |
146 | // Yep, we're inside a capsule | 146 | // Yep, we're inside a capsule |
147 | hasContact = true; | 147 | hasContact = true; |
148 | contactPoint = nearestOnEdge; | 148 | contactPoint = nearestOnEdge; |
149 | } | 149 | } |
150 | } | 150 | } |
151 | } | 151 | } |
152 | } | 152 | } |
153 | 153 | ||
154 | if (hasContact) | 154 | if (hasContact) |
155 | { | 155 | { |
156 | Vector3 contactToCentre = c - contactPoint; | 156 | Vector3 contactToCentre = c - contactPoint; |
157 | float distanceSqr = contactToCentre.LengthSquared(); | 157 | float distanceSqr = contactToCentre.LengthSquared(); |
158 | if (distanceSqr < (r - MaxOverlap) * (r - MaxOverlap)) | 158 | if (distanceSqr < (r - MaxOverlap) * (r - MaxOverlap)) |
159 | { | 159 | { |
160 | float distance = (float)Math.Sqrt(distanceSqr); | 160 | float distance = (float)Math.Sqrt(distanceSqr); |
161 | resultNormal = contactToCentre; | 161 | resultNormal = contactToCentre; |
162 | resultNormal = Vector3.Normalize(resultNormal); | 162 | resultNormal = Vector3.Normalize(resultNormal); |
163 | point = contactPoint; | 163 | point = contactPoint; |
164 | depth = -(r - distance); | 164 | depth = -(r - distance); |
165 | return true; | 165 | return true; |
166 | } | 166 | } |
167 | 167 | ||
168 | if (Vector3.Dot(delta, contactToCentre) >= 0.0f) | 168 | if (Vector3.Dot(delta, contactToCentre) >= 0.0f) |
169 | return false; | 169 | return false; |
170 | 170 | ||
171 | // Moving towards the contact point -> collision | 171 | // Moving towards the contact point -> collision |
172 | point = contactPoint; | 172 | point = contactPoint; |
173 | timeOfImpact = 0.0f; | 173 | timeOfImpact = 0.0f; |
174 | return true; | 174 | return true; |
175 | } | 175 | } |
176 | return false; | 176 | return false; |
177 | } | 177 | } |
178 | 178 | ||
179 | private bool PointInTriangle(Vector3[] vertices, Vector3 normal, Vector3 p) | 179 | private bool PointInTriangle(Vector3[] vertices, Vector3 normal, Vector3 p) |
180 | { | 180 | { |
181 | Vector3 p1 = vertices[0]; | 181 | Vector3 p1 = vertices[0]; |
182 | Vector3 p2 = vertices[1]; | 182 | Vector3 p2 = vertices[1]; |
183 | Vector3 p3 = vertices[2]; | 183 | Vector3 p3 = vertices[2]; |
184 | 184 | ||
185 | Vector3 edge1 = p2 - p1; | 185 | Vector3 edge1 = p2 - p1; |
186 | Vector3 edge2 = p3 - p2; | 186 | Vector3 edge2 = p3 - p2; |
187 | Vector3 edge3 = p1 - p3; | 187 | Vector3 edge3 = p1 - p3; |
188 | 188 | ||
189 | Vector3 p1ToP = p - p1; | 189 | Vector3 p1ToP = p - p1; |
190 | Vector3 p2ToP = p - p2; | 190 | Vector3 p2ToP = p - p2; |
191 | Vector3 p3ToP = p - p3; | 191 | Vector3 p3ToP = p - p3; |
192 | 192 | ||
193 | Vector3 edge1Normal = Vector3.Cross(edge1, normal); | 193 | Vector3 edge1Normal = Vector3.Cross(edge1, normal); |
194 | Vector3 edge2Normal = Vector3.Cross(edge2, normal); | 194 | Vector3 edge2Normal = Vector3.Cross(edge2, normal); |
195 | Vector3 edge3Normal = Vector3.Cross(edge3, normal); | 195 | Vector3 edge3Normal = Vector3.Cross(edge3, normal); |
196 | 196 | ||
197 | float r1, r2, r3; | 197 | float r1, r2, r3; |
198 | r1 = Vector3.Dot(edge1Normal, p1ToP); | 198 | r1 = Vector3.Dot(edge1Normal, p1ToP); |
199 | r2 = Vector3.Dot(edge2Normal, p2ToP); | 199 | r2 = Vector3.Dot(edge2Normal, p2ToP); |
200 | r3 = Vector3.Dot(edge3Normal, p3ToP); | 200 | r3 = Vector3.Dot(edge3Normal, p3ToP); |
201 | if ((r1 > 0 && r2 > 0 && r3 > 0) || | 201 | if ((r1 > 0 && r2 > 0 && r3 > 0) || |
202 | (r1 <= 0 && r2 <= 0 && r3 <= 0)) | 202 | (r1 <= 0 && r2 <= 0 && r3 <= 0)) |
203 | return true; | 203 | return true; |
204 | return false; | 204 | return false; |
205 | } | 205 | } |
206 | 206 | ||
207 | private bool FaceContains(Vector3 p, Vector3[] vertices, Vector3 normal) | 207 | private bool FaceContains(Vector3 p, Vector3[] vertices, Vector3 normal) |
208 | { | 208 | { |
209 | Vector3 lp = p; | 209 | Vector3 lp = p; |
210 | Vector3 lnormal = normal; | 210 | Vector3 lnormal = normal; |
211 | return PointInTriangle(vertices, lnormal, lp); | 211 | return PointInTriangle(vertices, lnormal, lp); |
212 | } | 212 | } |
213 | } | 213 | } |
214 | } \ No newline at end of file | 214 | } \ No newline at end of file |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs index a825fad..3c7daea 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs | |||
@@ -1,151 +1,151 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX | 26 | namespace XnaDevRu.BulletX |
27 | { | 27 | { |
28 | public class UnionFind : IDisposable | 28 | public class UnionFind : IDisposable |
29 | { | 29 | { |
30 | private List<Element> _elements = new List<Element>(); | 30 | private List<Element> _elements = new List<Element>(); |
31 | 31 | ||
32 | public int ElementCount | 32 | public int ElementCount |
33 | { | 33 | { |
34 | get { return _elements.Count; } | 34 | get { return _elements.Count; } |
35 | } | 35 | } |
36 | 36 | ||
37 | public void SortIslands() | 37 | public void SortIslands() |
38 | { | 38 | { |
39 | for (int i = 0; i < _elements.Count; i++) | 39 | for (int i = 0; i < _elements.Count; i++) |
40 | { | 40 | { |
41 | _elements[i].ID = Find(i); | 41 | _elements[i].ID = Find(i); |
42 | _elements[i].Size = i; | 42 | _elements[i].Size = i; |
43 | } | 43 | } |
44 | 44 | ||
45 | _elements.Sort(Sort); | 45 | _elements.Sort(Sort); |
46 | } | 46 | } |
47 | 47 | ||
48 | private static int Sort(Element x, Element y) | 48 | private static int Sort(Element x, Element y) |
49 | { | 49 | { |
50 | if (x.ID < y.ID) return -1; | 50 | if (x.ID < y.ID) return -1; |
51 | //else if (x.ID > y.ID) return 1; | 51 | //else if (x.ID > y.ID) return 1; |
52 | else return 0; | 52 | else return 0; |
53 | } | 53 | } |
54 | 54 | ||
55 | public void Reset(int number) | 55 | public void Reset(int number) |
56 | { | 56 | { |
57 | Allocate(number); | 57 | Allocate(number); |
58 | 58 | ||
59 | for (int i = 0; i < number; i++) | 59 | for (int i = 0; i < number; i++) |
60 | { | 60 | { |
61 | Element element = new Element(); | 61 | Element element = new Element(); |
62 | element.ID = i; | 62 | element.ID = i; |
63 | element.Size = 1; | 63 | element.Size = 1; |
64 | _elements.Insert(i, element); | 64 | _elements.Insert(i, element); |
65 | } | 65 | } |
66 | } | 66 | } |
67 | 67 | ||
68 | public bool IsRoot(int index) | 68 | public bool IsRoot(int index) |
69 | { | 69 | { |
70 | return (_elements[index].Size == index); | 70 | return (_elements[index].Size == index); |
71 | } | 71 | } |
72 | 72 | ||
73 | public Element this[int index] | 73 | public Element this[int index] |
74 | { | 74 | { |
75 | get { return _elements[index]; } | 75 | get { return _elements[index]; } |
76 | } | 76 | } |
77 | 77 | ||
78 | public void Allocate(int number) | 78 | public void Allocate(int number) |
79 | { | 79 | { |
80 | //Does nothing | 80 | //Does nothing |
81 | _elements = new List<Element>(number); | 81 | _elements = new List<Element>(number); |
82 | } | 82 | } |
83 | 83 | ||
84 | public bool Find(int i, int j) | 84 | public bool Find(int i, int j) |
85 | { | 85 | { |
86 | return (Find(i) == Find(j)); | 86 | return (Find(i) == Find(j)); |
87 | } | 87 | } |
88 | 88 | ||
89 | public int Find(int i) | 89 | public int Find(int i) |
90 | { | 90 | { |
91 | while (i != _elements[i].ID) | 91 | while (i != _elements[i].ID) |
92 | { | 92 | { |
93 | //Element element = _elements[i]; | 93 | //Element element = _elements[i]; |
94 | //element.ID = _elements[_elements[i].ID].ID; | 94 | //element.ID = _elements[_elements[i].ID].ID; |
95 | _elements[i].ID = _elements[_elements[i].ID].ID; | 95 | _elements[i].ID = _elements[_elements[i].ID].ID; |
96 | i = _elements[i].ID; | 96 | i = _elements[i].ID; |
97 | } | 97 | } |
98 | 98 | ||
99 | return i; | 99 | return i; |
100 | } | 100 | } |
101 | 101 | ||
102 | public void Unite(int p, int q) | 102 | public void Unite(int p, int q) |
103 | { | 103 | { |
104 | int i = Find(p), j = Find(q); | 104 | int i = Find(p), j = Find(q); |
105 | if (i == j) | 105 | if (i == j) |
106 | return; | 106 | return; |
107 | 107 | ||
108 | //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) | 108 | //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) |
109 | //if (_elements[i].Size < _elements[j].Size) | 109 | //if (_elements[i].Size < _elements[j].Size) |
110 | //{ | 110 | //{ |
111 | // Element element = _elements[i]; | 111 | // Element element = _elements[i]; |
112 | // element.ID = j; | 112 | // element.ID = j; |
113 | // _elements[i] = element; | 113 | // _elements[i] = element; |
114 | 114 | ||
115 | // element = _elements[j]; | 115 | // element = _elements[j]; |
116 | // element.Size += _elements[i].Size; | 116 | // element.Size += _elements[i].Size; |
117 | // _elements[j] = element; | 117 | // _elements[j] = element; |
118 | //} | 118 | //} |
119 | //else | 119 | //else |
120 | //{ | 120 | //{ |
121 | // Element element = _elements[j]; | 121 | // Element element = _elements[j]; |
122 | // element.ID = i; | 122 | // element.ID = i; |
123 | // _elements[j] = element; | 123 | // _elements[j] = element; |
124 | 124 | ||
125 | // element = _elements[i]; | 125 | // element = _elements[i]; |
126 | // element.Size += _elements[j].Size; | 126 | // element.Size += _elements[j].Size; |
127 | // _elements[i] = element; | 127 | // _elements[i] = element; |
128 | //} | 128 | //} |
129 | _elements[i].ID = j; | 129 | _elements[i].ID = j; |
130 | _elements[j].Size += _elements[i].Size; | 130 | _elements[j].Size += _elements[i].Size; |
131 | } | 131 | } |
132 | 132 | ||
133 | #region IDisposable Members | 133 | #region IDisposable Members |
134 | 134 | ||
135 | public void Dispose() | 135 | public void Dispose() |
136 | { | 136 | { |
137 | _elements.Clear(); | 137 | _elements.Clear(); |
138 | } | 138 | } |
139 | 139 | ||
140 | #endregion | 140 | #endregion |
141 | } | 141 | } |
142 | 142 | ||
143 | public class Element | 143 | public class Element |
144 | { | 144 | { |
145 | private int _id; | 145 | private int _id; |
146 | private int _size; | 146 | private int _size; |
147 | 147 | ||
148 | public int ID { get { return _id; } set { _id = value; } } | 148 | public int ID { get { return _id; } set { _id = value; } } |
149 | public int Size { get { return _size; } set { _size = value; } } | 149 | public int Size { get { return _size; } set { _size = value; } } |
150 | } | 150 | } |
151 | } \ No newline at end of file | 151 | } \ No newline at end of file |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs index b75c2b6..34f7442 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs | |||
@@ -1,215 +1,215 @@ | |||
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 | /// BUSimplex1to4 implements feature based and implicit simplex of up to 4 vertices (tetrahedron, triangle, line, vertex). | 30 | /// BUSimplex1to4 implements feature based and implicit simplex of up to 4 vertices (tetrahedron, triangle, line, vertex). |
31 | /// </summary> | 31 | /// </summary> |
32 | public class BUSimplex1to4 : PolyhedralConvexShape | 32 | public class BUSimplex1to4 : PolyhedralConvexShape |
33 | { | 33 | { |
34 | private int _numVertices = 0; | 34 | private int _numVertices = 0; |
35 | private Vector3[] _vertices = new Vector3[4]; | 35 | private Vector3[] _vertices = new Vector3[4]; |
36 | 36 | ||
37 | public BUSimplex1to4() { } | 37 | public BUSimplex1to4() { } |
38 | 38 | ||
39 | public BUSimplex1to4(Vector3 pointA) | 39 | public BUSimplex1to4(Vector3 pointA) |
40 | { | 40 | { |
41 | AddVertex(pointA); | 41 | AddVertex(pointA); |
42 | } | 42 | } |
43 | 43 | ||
44 | public BUSimplex1to4(Vector3 pointA, Vector3 pointB) | 44 | public BUSimplex1to4(Vector3 pointA, Vector3 pointB) |
45 | { | 45 | { |
46 | AddVertex(pointA); | 46 | AddVertex(pointA); |
47 | AddVertex(pointB); | 47 | AddVertex(pointB); |
48 | } | 48 | } |
49 | 49 | ||
50 | public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC) | 50 | public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC) |
51 | { | 51 | { |
52 | AddVertex(pointA); | 52 | AddVertex(pointA); |
53 | AddVertex(pointB); | 53 | AddVertex(pointB); |
54 | AddVertex(pointC); | 54 | AddVertex(pointC); |
55 | } | 55 | } |
56 | 56 | ||
57 | public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC, Vector3 pointD) | 57 | public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC, Vector3 pointD) |
58 | { | 58 | { |
59 | AddVertex(pointA); | 59 | AddVertex(pointA); |
60 | AddVertex(pointB); | 60 | AddVertex(pointB); |
61 | AddVertex(pointC); | 61 | AddVertex(pointC); |
62 | AddVertex(pointD); | 62 | AddVertex(pointD); |
63 | } | 63 | } |
64 | 64 | ||
65 | protected Vector3[] Vertices { get { return _vertices; } set { _vertices = value; } } | 65 | protected Vector3[] Vertices { get { return _vertices; } set { _vertices = value; } } |
66 | 66 | ||
67 | public override int VertexCount | 67 | public override int VertexCount |
68 | { | 68 | { |
69 | get | 69 | get |
70 | { | 70 | { |
71 | return _numVertices; | 71 | return _numVertices; |
72 | } | 72 | } |
73 | } | 73 | } |
74 | 74 | ||
75 | public override int EdgeCount | 75 | public override int EdgeCount |
76 | { | 76 | { |
77 | get | 77 | get |
78 | { | 78 | { |
79 | //euler formula, F-E+V = 2, so E = F+V-2 | 79 | //euler formula, F-E+V = 2, so E = F+V-2 |
80 | switch (_numVertices) | 80 | switch (_numVertices) |
81 | { | 81 | { |
82 | case 0: return 0; | 82 | case 0: return 0; |
83 | case 1: return 0; | 83 | case 1: return 0; |
84 | case 2: return 1; | 84 | case 2: return 1; |
85 | case 3: return 3; | 85 | case 3: return 3; |
86 | case 4: return 6; | 86 | case 4: return 6; |
87 | } | 87 | } |
88 | return 0; | 88 | return 0; |
89 | } | 89 | } |
90 | } | 90 | } |
91 | 91 | ||
92 | public override int PlaneCount | 92 | public override int PlaneCount |
93 | { | 93 | { |
94 | get | 94 | get |
95 | { | 95 | { |
96 | switch (_numVertices) | 96 | switch (_numVertices) |
97 | { | 97 | { |
98 | case 0: | 98 | case 0: |
99 | return 0; | 99 | return 0; |
100 | case 1: | 100 | case 1: |
101 | return 0; | 101 | return 0; |
102 | case 2: | 102 | case 2: |
103 | return 0; | 103 | return 0; |
104 | case 3: | 104 | case 3: |
105 | return 2; | 105 | return 2; |
106 | case 4: | 106 | case 4: |
107 | return 4; | 107 | return 4; |
108 | } | 108 | } |
109 | return 0; | 109 | return 0; |
110 | } | 110 | } |
111 | } | 111 | } |
112 | 112 | ||
113 | public override BroadphaseNativeTypes ShapeType | 113 | public override BroadphaseNativeTypes ShapeType |
114 | { | 114 | { |
115 | get | 115 | get |
116 | { | 116 | { |
117 | return BroadphaseNativeTypes.Tetrahedral; | 117 | return BroadphaseNativeTypes.Tetrahedral; |
118 | } | 118 | } |
119 | } | 119 | } |
120 | 120 | ||
121 | public override string Name | 121 | public override string Name |
122 | { | 122 | { |
123 | get | 123 | get |
124 | { | 124 | { |
125 | return "BUSimplex1to4"; | 125 | return "BUSimplex1to4"; |
126 | } | 126 | } |
127 | } | 127 | } |
128 | 128 | ||
129 | public void AddVertex(Vector3 v) | 129 | public void AddVertex(Vector3 v) |
130 | { | 130 | { |
131 | _vertices[_numVertices++] = v; | 131 | _vertices[_numVertices++] = v; |
132 | } | 132 | } |
133 | 133 | ||
134 | public void Reset() | 134 | public void Reset() |
135 | { | 135 | { |
136 | _numVertices = 0; | 136 | _numVertices = 0; |
137 | } | 137 | } |
138 | 138 | ||
139 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) | 139 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) |
140 | { | 140 | { |
141 | switch (_numVertices) | 141 | switch (_numVertices) |
142 | { | 142 | { |
143 | case 2: | 143 | case 2: |
144 | pa = _vertices[0]; | 144 | pa = _vertices[0]; |
145 | pb = _vertices[1]; | 145 | pb = _vertices[1]; |
146 | return; | 146 | return; |
147 | case 3: | 147 | case 3: |
148 | switch (i) | 148 | switch (i) |
149 | { | 149 | { |
150 | case 0: | 150 | case 0: |
151 | pa = _vertices[0]; | 151 | pa = _vertices[0]; |
152 | pb = _vertices[1]; | 152 | pb = _vertices[1]; |
153 | return; | 153 | return; |
154 | case 1: | 154 | case 1: |
155 | pa = _vertices[1]; | 155 | pa = _vertices[1]; |
156 | pb = _vertices[2]; | 156 | pb = _vertices[2]; |
157 | return; | 157 | return; |
158 | case 2: | 158 | case 2: |
159 | pa = _vertices[2]; | 159 | pa = _vertices[2]; |
160 | pb = _vertices[0]; | 160 | pb = _vertices[0]; |
161 | return; | 161 | return; |
162 | } | 162 | } |
163 | break; | 163 | break; |
164 | case 4: | 164 | case 4: |
165 | switch (i) | 165 | switch (i) |
166 | { | 166 | { |
167 | case 0: | 167 | case 0: |
168 | pa = _vertices[0]; | 168 | pa = _vertices[0]; |
169 | pb = _vertices[1]; | 169 | pb = _vertices[1]; |
170 | return; | 170 | return; |
171 | case 1: | 171 | case 1: |
172 | pa = _vertices[1]; | 172 | pa = _vertices[1]; |
173 | pb = _vertices[2]; | 173 | pb = _vertices[2]; |
174 | return; | 174 | return; |
175 | case 2: | 175 | case 2: |
176 | pa = _vertices[2]; | 176 | pa = _vertices[2]; |
177 | pb = _vertices[0]; | 177 | pb = _vertices[0]; |
178 | return; | 178 | return; |
179 | case 3: | 179 | case 3: |
180 | pa = _vertices[0]; | 180 | pa = _vertices[0]; |
181 | pb = _vertices[3]; | 181 | pb = _vertices[3]; |
182 | return; | 182 | return; |
183 | case 4: | 183 | case 4: |
184 | pa = _vertices[1]; | 184 | pa = _vertices[1]; |
185 | pb = _vertices[3]; | 185 | pb = _vertices[3]; |
186 | return; | 186 | return; |
187 | case 5: | 187 | case 5: |
188 | pa = _vertices[2]; | 188 | pa = _vertices[2]; |
189 | pb = _vertices[3]; | 189 | pb = _vertices[3]; |
190 | return; | 190 | return; |
191 | } | 191 | } |
192 | break; | 192 | break; |
193 | } | 193 | } |
194 | 194 | ||
195 | pa = new Vector3(); | 195 | pa = new Vector3(); |
196 | pb = new Vector3(); | 196 | pb = new Vector3(); |
197 | } | 197 | } |
198 | 198 | ||
199 | public override void GetVertex(int i, out Vector3 vtx) | 199 | public override void GetVertex(int i, out Vector3 vtx) |
200 | { | 200 | { |
201 | vtx = _vertices[i]; | 201 | vtx = _vertices[i]; |
202 | } | 202 | } |
203 | 203 | ||
204 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) | 204 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) |
205 | { | 205 | { |
206 | planeNormal = new Vector3(); | 206 | planeNormal = new Vector3(); |
207 | planeSupport = new Vector3(); | 207 | planeSupport = new Vector3(); |
208 | } | 208 | } |
209 | 209 | ||
210 | public override bool IsInside(Vector3 pt, float tolerance) | 210 | public override bool IsInside(Vector3 pt, float tolerance) |
211 | { | 211 | { |
212 | return false; | 212 | return false; |
213 | } | 213 | } |
214 | } | 214 | } |
215 | } | 215 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs index ba6f3b7..7fab5c7 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs | |||
@@ -1,316 +1,316 @@ | |||
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 | public class BoxShape : PolyhedralConvexShape | 29 | public class BoxShape : PolyhedralConvexShape |
30 | { | 30 | { |
31 | public BoxShape(Vector3 boxHalfExtents) | 31 | public BoxShape(Vector3 boxHalfExtents) |
32 | { | 32 | { |
33 | ImplicitShapeDimensions = boxHalfExtents; | 33 | ImplicitShapeDimensions = boxHalfExtents; |
34 | } | 34 | } |
35 | 35 | ||
36 | public override int VertexCount | 36 | public override int VertexCount |
37 | { | 37 | { |
38 | get | 38 | get |
39 | { | 39 | { |
40 | return 8; | 40 | return 8; |
41 | } | 41 | } |
42 | } | 42 | } |
43 | 43 | ||
44 | public override int EdgeCount | 44 | public override int EdgeCount |
45 | { | 45 | { |
46 | get | 46 | get |
47 | { | 47 | { |
48 | return 12; | 48 | return 12; |
49 | } | 49 | } |
50 | } | 50 | } |
51 | 51 | ||
52 | public override BroadphaseNativeTypes ShapeType | 52 | public override BroadphaseNativeTypes ShapeType |
53 | { | 53 | { |
54 | get | 54 | get |
55 | { | 55 | { |
56 | return BroadphaseNativeTypes.Box; | 56 | return BroadphaseNativeTypes.Box; |
57 | } | 57 | } |
58 | } | 58 | } |
59 | 59 | ||
60 | public override string Name | 60 | public override string Name |
61 | { | 61 | { |
62 | get | 62 | get |
63 | { | 63 | { |
64 | return "Box"; | 64 | return "Box"; |
65 | } | 65 | } |
66 | } | 66 | } |
67 | 67 | ||
68 | public override int PreferredPenetrationDirectionsCount | 68 | public override int PreferredPenetrationDirectionsCount |
69 | { | 69 | { |
70 | get | 70 | get |
71 | { | 71 | { |
72 | return 6; | 72 | return 6; |
73 | } | 73 | } |
74 | } | 74 | } |
75 | 75 | ||
76 | public override int PlaneCount | 76 | public override int PlaneCount |
77 | { | 77 | { |
78 | get | 78 | get |
79 | { | 79 | { |
80 | return 6; | 80 | return 6; |
81 | } | 81 | } |
82 | } | 82 | } |
83 | 83 | ||
84 | public Vector3 HalfExtents { get { return ImplicitShapeDimensions * LocalScaling; } } | 84 | public Vector3 HalfExtents { get { return ImplicitShapeDimensions * LocalScaling; } } |
85 | 85 | ||
86 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) | 86 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) |
87 | { | 87 | { |
88 | int edgeVert0 = 0; | 88 | int edgeVert0 = 0; |
89 | int edgeVert1 = 0; | 89 | int edgeVert1 = 0; |
90 | 90 | ||
91 | switch (i) | 91 | switch (i) |
92 | { | 92 | { |
93 | case 0: | 93 | case 0: |
94 | edgeVert0 = 0; | 94 | edgeVert0 = 0; |
95 | edgeVert1 = 1; | 95 | edgeVert1 = 1; |
96 | break; | 96 | break; |
97 | case 1: | 97 | case 1: |
98 | edgeVert0 = 0; | 98 | edgeVert0 = 0; |
99 | edgeVert1 = 2; | 99 | edgeVert1 = 2; |
100 | break; | 100 | break; |
101 | case 2: | 101 | case 2: |
102 | edgeVert0 = 1; | 102 | edgeVert0 = 1; |
103 | edgeVert1 = 3; | 103 | edgeVert1 = 3; |
104 | 104 | ||
105 | break; | 105 | break; |
106 | case 3: | 106 | case 3: |
107 | edgeVert0 = 2; | 107 | edgeVert0 = 2; |
108 | edgeVert1 = 3; | 108 | edgeVert1 = 3; |
109 | break; | 109 | break; |
110 | case 4: | 110 | case 4: |
111 | edgeVert0 = 0; | 111 | edgeVert0 = 0; |
112 | edgeVert1 = 4; | 112 | edgeVert1 = 4; |
113 | break; | 113 | break; |
114 | case 5: | 114 | case 5: |
115 | edgeVert0 = 1; | 115 | edgeVert0 = 1; |
116 | edgeVert1 = 5; | 116 | edgeVert1 = 5; |
117 | 117 | ||
118 | break; | 118 | break; |
119 | case 6: | 119 | case 6: |
120 | edgeVert0 = 2; | 120 | edgeVert0 = 2; |
121 | edgeVert1 = 6; | 121 | edgeVert1 = 6; |
122 | break; | 122 | break; |
123 | case 7: | 123 | case 7: |
124 | edgeVert0 = 3; | 124 | edgeVert0 = 3; |
125 | edgeVert1 = 7; | 125 | edgeVert1 = 7; |
126 | break; | 126 | break; |
127 | case 8: | 127 | case 8: |
128 | edgeVert0 = 4; | 128 | edgeVert0 = 4; |
129 | edgeVert1 = 5; | 129 | edgeVert1 = 5; |
130 | break; | 130 | break; |
131 | case 9: | 131 | case 9: |
132 | edgeVert0 = 4; | 132 | edgeVert0 = 4; |
133 | edgeVert1 = 6; | 133 | edgeVert1 = 6; |
134 | break; | 134 | break; |
135 | case 10: | 135 | case 10: |
136 | edgeVert0 = 5; | 136 | edgeVert0 = 5; |
137 | edgeVert1 = 7; | 137 | edgeVert1 = 7; |
138 | break; | 138 | break; |
139 | case 11: | 139 | case 11: |
140 | edgeVert0 = 6; | 140 | edgeVert0 = 6; |
141 | edgeVert1 = 7; | 141 | edgeVert1 = 7; |
142 | break; | 142 | break; |
143 | default: | 143 | default: |
144 | throw new BulletException(); | 144 | throw new BulletException(); |
145 | 145 | ||
146 | } | 146 | } |
147 | 147 | ||
148 | GetVertex(edgeVert0, out pa); | 148 | GetVertex(edgeVert0, out pa); |
149 | GetVertex(edgeVert1, out pb); | 149 | GetVertex(edgeVert1, out pb); |
150 | } | 150 | } |
151 | 151 | ||
152 | public override void GetVertex(int i, out Vector3 vtx) | 152 | public override void GetVertex(int i, out Vector3 vtx) |
153 | { | 153 | { |
154 | Vector3 halfExtents = HalfExtents; | 154 | Vector3 halfExtents = HalfExtents; |
155 | 155 | ||
156 | vtx = new Vector3( | 156 | vtx = new Vector3( |
157 | halfExtents.X * (1 - (i & 1)) - halfExtents.X * (i & 1), | 157 | halfExtents.X * (1 - (i & 1)) - halfExtents.X * (i & 1), |
158 | halfExtents.Y * (1 - ((i & 2) >> 1)) - halfExtents.Y * ((i & 2) >> 1), | 158 | halfExtents.Y * (1 - ((i & 2) >> 1)) - halfExtents.Y * ((i & 2) >> 1), |
159 | halfExtents.Z * (1 - ((i & 4) >> 2)) - halfExtents.Z * ((i & 4) >> 2)); | 159 | halfExtents.Z * (1 - ((i & 4) >> 2)) - halfExtents.Z * ((i & 4) >> 2)); |
160 | } | 160 | } |
161 | 161 | ||
162 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) | 162 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) |
163 | { | 163 | { |
164 | //this plane might not be aligned... | 164 | //this plane might not be aligned... |
165 | Vector4 plane; | 165 | Vector4 plane; |
166 | GetPlaneEquation(out plane, i); | 166 | GetPlaneEquation(out plane, i); |
167 | planeNormal = new Vector3(plane.X, plane.Y, plane.Z); | 167 | planeNormal = new Vector3(plane.X, plane.Y, plane.Z); |
168 | planeSupport = LocalGetSupportingVertex(-planeNormal); | 168 | planeSupport = LocalGetSupportingVertex(-planeNormal); |
169 | } | 169 | } |
170 | 170 | ||
171 | public override bool IsInside(Vector3 pt, float tolerance) | 171 | public override bool IsInside(Vector3 pt, float tolerance) |
172 | { | 172 | { |
173 | Vector3 halfExtents = HalfExtents; | 173 | Vector3 halfExtents = HalfExtents; |
174 | 174 | ||
175 | //btScalar minDist = 2*tolerance; | 175 | //btScalar minDist = 2*tolerance; |
176 | 176 | ||
177 | bool result = (pt.X <= ( halfExtents.X + tolerance)) && | 177 | bool result = (pt.X <= ( halfExtents.X + tolerance)) && |
178 | (pt.X >= (-halfExtents.X - tolerance)) && | 178 | (pt.X >= (-halfExtents.X - tolerance)) && |
179 | (pt.Y <= ( halfExtents.Y + tolerance)) && | 179 | (pt.Y <= ( halfExtents.Y + tolerance)) && |
180 | (pt.Y >= (-halfExtents.Y - tolerance)) && | 180 | (pt.Y >= (-halfExtents.Y - tolerance)) && |
181 | (pt.Z <= ( halfExtents.Z + tolerance)) && | 181 | (pt.Z <= ( halfExtents.Z + tolerance)) && |
182 | (pt.Z >= (-halfExtents.Z - tolerance)); | 182 | (pt.Z >= (-halfExtents.Z - tolerance)); |
183 | 183 | ||
184 | return result; | 184 | return result; |
185 | } | 185 | } |
186 | 186 | ||
187 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | 187 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) |
188 | { | 188 | { |
189 | Vector3 halfExtents = HalfExtents; | 189 | Vector3 halfExtents = HalfExtents; |
190 | 190 | ||
191 | return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X, | 191 | return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X, |
192 | vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, | 192 | vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, |
193 | vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); | 193 | vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); |
194 | } | 194 | } |
195 | 195 | ||
196 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | 196 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) |
197 | { | 197 | { |
198 | Vector3 halfExtents = HalfExtents; | 198 | Vector3 halfExtents = HalfExtents; |
199 | Vector3 margin = new Vector3(Margin, Margin, Margin); | 199 | Vector3 margin = new Vector3(Margin, Margin, Margin); |
200 | halfExtents -= margin; | 200 | halfExtents -= margin; |
201 | 201 | ||
202 | return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X, | 202 | return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X, |
203 | vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, | 203 | vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, |
204 | vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); | 204 | vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); |
205 | } | 205 | } |
206 | 206 | ||
207 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | 207 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) |
208 | { | 208 | { |
209 | Vector3 halfExtents = HalfExtents; | 209 | Vector3 halfExtents = HalfExtents; |
210 | Vector3 margin = new Vector3(Margin, Margin, Margin); | 210 | Vector3 margin = new Vector3(Margin, Margin, Margin); |
211 | halfExtents -= margin; | 211 | halfExtents -= margin; |
212 | 212 | ||
213 | for (int i = 0; i < vectors.Length; i++) | 213 | for (int i = 0; i < vectors.Length; i++) |
214 | { | 214 | { |
215 | Vector3 vec = vectors[i]; | 215 | Vector3 vec = vectors[i]; |
216 | supportVerticesOut[i] = new Vector3(vec.X < 0.0f ? -halfExtents.X : halfExtents.X, | 216 | supportVerticesOut[i] = new Vector3(vec.X < 0.0f ? -halfExtents.X : halfExtents.X, |
217 | vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, | 217 | vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, |
218 | vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); | 218 | vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); |
219 | } | 219 | } |
220 | } | 220 | } |
221 | 221 | ||
222 | public virtual void GetPlaneEquation(out Vector4 plane, int i) | 222 | public virtual void GetPlaneEquation(out Vector4 plane, int i) |
223 | { | 223 | { |
224 | Vector3 halfExtents = HalfExtents; | 224 | Vector3 halfExtents = HalfExtents; |
225 | 225 | ||
226 | switch (i) | 226 | switch (i) |
227 | { | 227 | { |
228 | case 0: | 228 | case 0: |
229 | plane = new Vector4(1, 0, 0, 0); | 229 | plane = new Vector4(1, 0, 0, 0); |
230 | plane.W = -halfExtents.X; | 230 | plane.W = -halfExtents.X; |
231 | break; | 231 | break; |
232 | case 1: | 232 | case 1: |
233 | plane = new Vector4(-1, 0, 0, 0); | 233 | plane = new Vector4(-1, 0, 0, 0); |
234 | plane.W = -halfExtents.X; | 234 | plane.W = -halfExtents.X; |
235 | break; | 235 | break; |
236 | case 2: | 236 | case 2: |
237 | plane = new Vector4(0, 1, 0, 0); | 237 | plane = new Vector4(0, 1, 0, 0); |
238 | plane.W = -halfExtents.Y; | 238 | plane.W = -halfExtents.Y; |
239 | break; | 239 | break; |
240 | case 3: | 240 | case 3: |
241 | plane = new Vector4(0, -1, 0, 0); | 241 | plane = new Vector4(0, -1, 0, 0); |
242 | plane.W = -halfExtents.Y; | 242 | plane.W = -halfExtents.Y; |
243 | break; | 243 | break; |
244 | case 4: | 244 | case 4: |
245 | plane = new Vector4(0, 0, 1, 0); | 245 | plane = new Vector4(0, 0, 1, 0); |
246 | plane.W = -halfExtents.Z; | 246 | plane.W = -halfExtents.Z; |
247 | break; | 247 | break; |
248 | case 5: | 248 | case 5: |
249 | plane = new Vector4(0, 0, -1, 0); | 249 | plane = new Vector4(0, 0, -1, 0); |
250 | plane.W = -halfExtents.Z; | 250 | plane.W = -halfExtents.Z; |
251 | break; | 251 | break; |
252 | default: | 252 | default: |
253 | throw new BulletException(); | 253 | throw new BulletException(); |
254 | } | 254 | } |
255 | } | 255 | } |
256 | 256 | ||
257 | public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) | 257 | public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) |
258 | { | 258 | { |
259 | switch (index) | 259 | switch (index) |
260 | { | 260 | { |
261 | case 0: | 261 | case 0: |
262 | penetrationVector = new Vector3(1, 0, 0); | 262 | penetrationVector = new Vector3(1, 0, 0); |
263 | break; | 263 | break; |
264 | case 1: | 264 | case 1: |
265 | penetrationVector = new Vector3(-1, 0, 0); | 265 | penetrationVector = new Vector3(-1, 0, 0); |
266 | break; | 266 | break; |
267 | case 2: | 267 | case 2: |
268 | penetrationVector = new Vector3(0, 1, 0); | 268 | penetrationVector = new Vector3(0, 1, 0); |
269 | break; | 269 | break; |
270 | case 3: | 270 | case 3: |
271 | penetrationVector = new Vector3(0, -1, 0); | 271 | penetrationVector = new Vector3(0, -1, 0); |
272 | break; | 272 | break; |
273 | case 4: | 273 | case 4: |
274 | penetrationVector = new Vector3(0, 0, 1); | 274 | penetrationVector = new Vector3(0, 0, 1); |
275 | break; | 275 | break; |
276 | case 5: | 276 | case 5: |
277 | penetrationVector = new Vector3(0, 0, -1); | 277 | penetrationVector = new Vector3(0, 0, -1); |
278 | break; | 278 | break; |
279 | default: | 279 | default: |
280 | throw new BulletException(); | 280 | throw new BulletException(); |
281 | } | 281 | } |
282 | } | 282 | } |
283 | 283 | ||
284 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | 284 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) |
285 | { | 285 | { |
286 | Vector3 halfExtents = HalfExtents; | 286 | Vector3 halfExtents = HalfExtents; |
287 | 287 | ||
288 | Matrix abs_b = MathHelper.Absolute(t); | 288 | Matrix abs_b = MathHelper.Absolute(t); |
289 | Vector3 center = t.Translation; | 289 | Vector3 center = t.Translation; |
290 | Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13); | 290 | Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13); |
291 | Vector3 row2 = new Vector3(abs_b.M21, abs_b.M22, abs_b.M23); | 291 | Vector3 row2 = new Vector3(abs_b.M21, abs_b.M22, abs_b.M23); |
292 | Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33); | 292 | Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33); |
293 | Vector3 extent = new Vector3(Vector3.Dot(row1, halfExtents), | 293 | Vector3 extent = new Vector3(Vector3.Dot(row1, halfExtents), |
294 | Vector3.Dot(row2, halfExtents), | 294 | Vector3.Dot(row2, halfExtents), |
295 | Vector3.Dot(row3, halfExtents)); | 295 | Vector3.Dot(row3, halfExtents)); |
296 | extent += new Vector3(Margin, Margin, Margin); | 296 | extent += new Vector3(Margin, Margin, Margin); |
297 | 297 | ||
298 | aabbMin = center - extent; | 298 | aabbMin = center - extent; |
299 | aabbMax = center + extent; | 299 | aabbMax = center + extent; |
300 | } | 300 | } |
301 | 301 | ||
302 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | 302 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) |
303 | { | 303 | { |
304 | Vector3 halfExtents = HalfExtents; | 304 | Vector3 halfExtents = HalfExtents; |
305 | 305 | ||
306 | float lx = 2f * (halfExtents.X); | 306 | float lx = 2f * (halfExtents.X); |
307 | float ly = 2f * (halfExtents.Y); | 307 | float ly = 2f * (halfExtents.Y); |
308 | float lz = 2f * (halfExtents.Z); | 308 | float lz = 2f * (halfExtents.Z); |
309 | 309 | ||
310 | inertia = new Vector3(); | 310 | inertia = new Vector3(); |
311 | inertia.X = mass / (12.0f) * (ly * ly + lz * lz); | 311 | inertia.X = mass / (12.0f) * (ly * ly + lz * lz); |
312 | inertia.Y = mass / (12.0f) * (lx * lx + lz * lz); | 312 | inertia.Y = mass / (12.0f) * (lx * lx + lz * lz); |
313 | inertia.Z = mass / (12.0f) * (lx * lx + ly * ly); | 313 | inertia.Z = mass / (12.0f) * (lx * lx + ly * ly); |
314 | } | 314 | } |
315 | } | 315 | } |
316 | } | 316 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs index 00247a0..03e1e44 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs | |||
@@ -1,83 +1,83 @@ | |||
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 | class MyNodeOverlapCallback : INodeOverlapCallback | 29 | class MyNodeOverlapCallback : INodeOverlapCallback |
30 | { | 30 | { |
31 | StridingMeshInterface _meshInterface; | 31 | StridingMeshInterface _meshInterface; |
32 | ITriangleCallback _callback; | 32 | ITriangleCallback _callback; |
33 | Vector3[] _triangle = new Vector3[3]; | 33 | Vector3[] _triangle = new Vector3[3]; |
34 | 34 | ||
35 | public MyNodeOverlapCallback(ITriangleCallback callback, StridingMeshInterface meshInterface) | 35 | public MyNodeOverlapCallback(ITriangleCallback callback, StridingMeshInterface meshInterface) |
36 | { | 36 | { |
37 | _meshInterface = meshInterface; | 37 | _meshInterface = meshInterface; |
38 | _callback = callback; | 38 | _callback = callback; |
39 | } | 39 | } |
40 | 40 | ||
41 | public void ProcessNode(OptimizedBvhNode node) | 41 | public void ProcessNode(OptimizedBvhNode node) |
42 | { | 42 | { |
43 | List<Vector3> verts; | 43 | List<Vector3> verts; |
44 | List<int> indicies; | 44 | List<int> indicies; |
45 | int numtriangles; | 45 | int numtriangles; |
46 | 46 | ||
47 | _meshInterface.GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, node.SubPart); | 47 | _meshInterface.GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, node.SubPart); |
48 | Vector3 meshScaling = _meshInterface.Scaling; | 48 | Vector3 meshScaling = _meshInterface.Scaling; |
49 | 49 | ||
50 | for (int j = 0; j < 3; j++) | 50 | for (int j = 0; j < 3; j++) |
51 | { | 51 | { |
52 | _triangle[j] = verts[indicies[j + node.TriangleIndex * 3]] * meshScaling; | 52 | _triangle[j] = verts[indicies[j + node.TriangleIndex * 3]] * meshScaling; |
53 | } | 53 | } |
54 | 54 | ||
55 | _callback.ProcessTriangle(_triangle, node.SubPart, node.TriangleIndex); | 55 | _callback.ProcessTriangle(_triangle, node.SubPart, node.TriangleIndex); |
56 | _meshInterface.UnLockReadOnlyVertexBase(node.SubPart); | 56 | _meshInterface.UnLockReadOnlyVertexBase(node.SubPart); |
57 | } | 57 | } |
58 | } | 58 | } |
59 | 59 | ||
60 | public class BvhTriangleMeshShape : TriangleMeshShape | 60 | public class BvhTriangleMeshShape : TriangleMeshShape |
61 | { | 61 | { |
62 | OptimizedBvh _bvh = new OptimizedBvh(); | 62 | OptimizedBvh _bvh = new OptimizedBvh(); |
63 | 63 | ||
64 | public BvhTriangleMeshShape(StridingMeshInterface meshInterface) : base(meshInterface) | 64 | public BvhTriangleMeshShape(StridingMeshInterface meshInterface) : base(meshInterface) |
65 | { | 65 | { |
66 | _bvh.Build(meshInterface); | 66 | _bvh.Build(meshInterface); |
67 | } | 67 | } |
68 | 68 | ||
69 | public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) | 69 | public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) |
70 | { | 70 | { |
71 | MyNodeOverlapCallback myNodeCallback = new MyNodeOverlapCallback(callback, MeshInterface); | 71 | MyNodeOverlapCallback myNodeCallback = new MyNodeOverlapCallback(callback, MeshInterface); |
72 | _bvh.ReportAabbOverlappingNodex(myNodeCallback, aabbMin, aabbMax); | 72 | _bvh.ReportAabbOverlappingNodex(myNodeCallback, aabbMin, aabbMax); |
73 | } | 73 | } |
74 | 74 | ||
75 | public override string Name | 75 | public override string Name |
76 | { | 76 | { |
77 | get | 77 | get |
78 | { | 78 | { |
79 | return "BvhTriangleMesh"; | 79 | return "BvhTriangleMesh"; |
80 | } | 80 | } |
81 | } | 81 | } |
82 | } | 82 | } |
83 | } | 83 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs index a9ce3be..1a4fac2 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs | |||
@@ -1,148 +1,148 @@ | |||
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 | /// CollisionShape provides generic interface for collidable objects | 30 | /// CollisionShape provides generic interface for collidable objects |
31 | /// </summary> | 31 | /// </summary> |
32 | public abstract class CollisionShape | 32 | public abstract class CollisionShape |
33 | { | 33 | { |
34 | //debugging support | 34 | //debugging support |
35 | private string _tempDebug; | 35 | private string _tempDebug; |
36 | 36 | ||
37 | public abstract string Name { get; } | 37 | public abstract string Name { get; } |
38 | public string ExtraDebugInfo { get { return _tempDebug; } set { _tempDebug = value; } } | 38 | public string ExtraDebugInfo { get { return _tempDebug; } set { _tempDebug = value; } } |
39 | 39 | ||
40 | public bool IsPolyhedral | 40 | public bool IsPolyhedral |
41 | { | 41 | { |
42 | get | 42 | get |
43 | { | 43 | { |
44 | return BroadphaseProxy.IsPolyhedral(ShapeType); | 44 | return BroadphaseProxy.IsPolyhedral(ShapeType); |
45 | } | 45 | } |
46 | } | 46 | } |
47 | 47 | ||
48 | public bool IsConvex | 48 | public bool IsConvex |
49 | { | 49 | { |
50 | get | 50 | get |
51 | { | 51 | { |
52 | return BroadphaseProxy.IsConvex(ShapeType); | 52 | return BroadphaseProxy.IsConvex(ShapeType); |
53 | } | 53 | } |
54 | } | 54 | } |
55 | public bool IsConcave | 55 | public bool IsConcave |
56 | { | 56 | { |
57 | get | 57 | get |
58 | { | 58 | { |
59 | return BroadphaseProxy.IsConcave(ShapeType); | 59 | return BroadphaseProxy.IsConcave(ShapeType); |
60 | } | 60 | } |
61 | } | 61 | } |
62 | public bool IsCompound | 62 | public bool IsCompound |
63 | { | 63 | { |
64 | get | 64 | get |
65 | { | 65 | { |
66 | return BroadphaseProxy.IsCompound(ShapeType); | 66 | return BroadphaseProxy.IsCompound(ShapeType); |
67 | } | 67 | } |
68 | } | 68 | } |
69 | 69 | ||
70 | //isInfinite is used to catch simulation error (aabb check) | 70 | //isInfinite is used to catch simulation error (aabb check) |
71 | public bool IsInfinite | 71 | public bool IsInfinite |
72 | { | 72 | { |
73 | get | 73 | get |
74 | { | 74 | { |
75 | return BroadphaseProxy.IsInfinite(ShapeType); | 75 | return BroadphaseProxy.IsInfinite(ShapeType); |
76 | } | 76 | } |
77 | } | 77 | } |
78 | 78 | ||
79 | public abstract float Margin { get; set; } | 79 | public abstract float Margin { get; set; } |
80 | public abstract Vector3 LocalScaling { get; set; } | 80 | public abstract Vector3 LocalScaling { get; set; } |
81 | public abstract BroadphaseNativeTypes ShapeType { get; } | 81 | public abstract BroadphaseNativeTypes ShapeType { get; } |
82 | 82 | ||
83 | 83 | ||
84 | public virtual void GetBoundingSphere(out Vector3 center, out float radius) | 84 | public virtual void GetBoundingSphere(out Vector3 center, out float radius) |
85 | { | 85 | { |
86 | Matrix tr = Matrix.Identity; | 86 | Matrix tr = Matrix.Identity; |
87 | Vector3 aabbMin, aabbMax; | 87 | Vector3 aabbMin, aabbMax; |
88 | 88 | ||
89 | GetAabb(tr, out aabbMin, out aabbMax); | 89 | GetAabb(tr, out aabbMin, out aabbMax); |
90 | 90 | ||
91 | radius = (aabbMax - aabbMin).Length() * 0.5f; | 91 | radius = (aabbMax - aabbMin).Length() * 0.5f; |
92 | center = (aabbMin + aabbMax) * 0.5f; | 92 | center = (aabbMin + aabbMax) * 0.5f; |
93 | } | 93 | } |
94 | 94 | ||
95 | public virtual float GetAngularMotionDisc() | 95 | public virtual float GetAngularMotionDisc() |
96 | { | 96 | { |
97 | Vector3 center; | 97 | Vector3 center; |
98 | float disc; | 98 | float disc; |
99 | GetBoundingSphere(out center, out disc); | 99 | GetBoundingSphere(out center, out disc); |
100 | disc += center.Length(); | 100 | disc += center.Length(); |
101 | return disc; | 101 | return disc; |
102 | } | 102 | } |
103 | 103 | ||
104 | //calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) | 104 | //calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) |
105 | //result is conservative | 105 | //result is conservative |
106 | public void CalculateTemporalAabb(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, out Vector3 temporalAabbMin, out Vector3 temporalAabbMax) | 106 | public void CalculateTemporalAabb(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, out Vector3 temporalAabbMin, out Vector3 temporalAabbMax) |
107 | { | 107 | { |
108 | //start with static aabb | 108 | //start with static aabb |
109 | GetAabb(currentTransform, out temporalAabbMin, out temporalAabbMax); | 109 | GetAabb(currentTransform, out temporalAabbMin, out temporalAabbMax); |
110 | 110 | ||
111 | float temporalAabbMaxx = temporalAabbMax.X; | 111 | float temporalAabbMaxx = temporalAabbMax.X; |
112 | float temporalAabbMaxy = temporalAabbMax.Y; | 112 | float temporalAabbMaxy = temporalAabbMax.Y; |
113 | float temporalAabbMaxz = temporalAabbMax.Z; | 113 | float temporalAabbMaxz = temporalAabbMax.Z; |
114 | float temporalAabbMinx = temporalAabbMin.X; | 114 | float temporalAabbMinx = temporalAabbMin.X; |
115 | float temporalAabbMiny = temporalAabbMin.Y; | 115 | float temporalAabbMiny = temporalAabbMin.Y; |
116 | float temporalAabbMinz = temporalAabbMin.Z; | 116 | float temporalAabbMinz = temporalAabbMin.Z; |
117 | 117 | ||
118 | // add linear motion | 118 | // add linear motion |
119 | Vector3 linMotion = linearVelocity * timeStep; | 119 | Vector3 linMotion = linearVelocity * timeStep; |
120 | //todo: simd would have a vector max/min operation, instead of per-element access | 120 | //todo: simd would have a vector max/min operation, instead of per-element access |
121 | if (linMotion.X > 0) | 121 | if (linMotion.X > 0) |
122 | temporalAabbMaxx += linMotion.X; | 122 | temporalAabbMaxx += linMotion.X; |
123 | else | 123 | else |
124 | temporalAabbMinx += linMotion.X; | 124 | temporalAabbMinx += linMotion.X; |
125 | if (linMotion.Y > 0) | 125 | if (linMotion.Y > 0) |
126 | temporalAabbMaxy += linMotion.Y; | 126 | temporalAabbMaxy += linMotion.Y; |
127 | else | 127 | else |
128 | temporalAabbMiny += linMotion.Y; | 128 | temporalAabbMiny += linMotion.Y; |
129 | if (linMotion.Z > 0) | 129 | if (linMotion.Z > 0) |
130 | temporalAabbMaxz += linMotion.Z; | 130 | temporalAabbMaxz += linMotion.Z; |
131 | else | 131 | else |
132 | temporalAabbMinz += linMotion.Z; | 132 | temporalAabbMinz += linMotion.Z; |
133 | 133 | ||
134 | //add conservative angular motion | 134 | //add conservative angular motion |
135 | float angularMotion = angularVelocity.Length() * GetAngularMotionDisc() * timeStep; | 135 | float angularMotion = angularVelocity.Length() * GetAngularMotionDisc() * timeStep; |
136 | Vector3 angularMotion3d = new Vector3(angularMotion, angularMotion, angularMotion); | 136 | Vector3 angularMotion3d = new Vector3(angularMotion, angularMotion, angularMotion); |
137 | temporalAabbMin = new Vector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz); | 137 | temporalAabbMin = new Vector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz); |
138 | temporalAabbMax = new Vector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz); | 138 | temporalAabbMax = new Vector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz); |
139 | 139 | ||
140 | temporalAabbMin -= angularMotion3d; | 140 | temporalAabbMin -= angularMotion3d; |
141 | temporalAabbMax += angularMotion3d; | 141 | temporalAabbMax += angularMotion3d; |
142 | } | 142 | } |
143 | 143 | ||
144 | public abstract void GetAabb(Matrix transform, out Vector3 aabbMin, out Vector3 aabbMax); | 144 | public abstract void GetAabb(Matrix transform, out Vector3 aabbMin, out Vector3 aabbMax); |
145 | 145 | ||
146 | public abstract void CalculateLocalInertia(float mass, out Vector3 inertia); | 146 | public abstract void CalculateLocalInertia(float mass, out Vector3 inertia); |
147 | } | 147 | } |
148 | } | 148 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs index 08aeec8..bd13ffd 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs | |||
@@ -1,183 +1,183 @@ | |||
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 | /// CompoundShape allows to store multiple other CollisionShapes | 30 | /// CompoundShape allows to store multiple other CollisionShapes |
31 | /// This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape. | 31 | /// This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape. |
32 | /// </summary> | 32 | /// </summary> |
33 | public class CompoundShape : CollisionShape | 33 | public class CompoundShape : CollisionShape |
34 | { | 34 | { |
35 | private List<Matrix> _childTransforms = new List<Matrix>(); | 35 | private List<Matrix> _childTransforms = new List<Matrix>(); |
36 | private List<CollisionShape> _childShapes = new List<CollisionShape>(); | 36 | private List<CollisionShape> _childShapes = new List<CollisionShape>(); |
37 | private Vector3 _localAabbMin; | 37 | private Vector3 _localAabbMin; |
38 | private Vector3 _localAabbMax; | 38 | private Vector3 _localAabbMax; |
39 | 39 | ||
40 | private OptimizedBvh _aabbTree; | 40 | private OptimizedBvh _aabbTree; |
41 | private float _collisionMargin; | 41 | private float _collisionMargin; |
42 | private Vector3 _localScaling; | 42 | private Vector3 _localScaling; |
43 | 43 | ||
44 | public CompoundShape() | 44 | public CompoundShape() |
45 | { | 45 | { |
46 | _localAabbMin = new Vector3(1e30f, 1e30f, 1e30f); | 46 | _localAabbMin = new Vector3(1e30f, 1e30f, 1e30f); |
47 | _localAabbMax = new Vector3(-1e30f, -1e30f, -1e30f); | 47 | _localAabbMax = new Vector3(-1e30f, -1e30f, -1e30f); |
48 | _aabbTree = null; | 48 | _aabbTree = null; |
49 | _collisionMargin = 0f; | 49 | _collisionMargin = 0f; |
50 | _localScaling = new Vector3(1f, 1f, 1f); | 50 | _localScaling = new Vector3(1f, 1f, 1f); |
51 | } | 51 | } |
52 | 52 | ||
53 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | 53 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) |
54 | { | 54 | { |
55 | Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin); | 55 | Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin); |
56 | Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin); | 56 | Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin); |
57 | 57 | ||
58 | Matrix abs_b = MathHelper.Absolute(t); | 58 | Matrix abs_b = MathHelper.Absolute(t); |
59 | 59 | ||
60 | Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13); | 60 | Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13); |
61 | Vector3 row2 = new Vector3(abs_b.M21, abs_b.M22, abs_b.M23); | 61 | Vector3 row2 = new Vector3(abs_b.M21, abs_b.M22, abs_b.M23); |
62 | Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33); | 62 | Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33); |
63 | 63 | ||
64 | Vector3 center = new Vector3(Vector3.Dot(row1, localCenter) + t.Translation.X, | 64 | Vector3 center = new Vector3(Vector3.Dot(row1, localCenter) + t.Translation.X, |
65 | Vector3.Dot(row2, localCenter) + t.Translation.Y, | 65 | Vector3.Dot(row2, localCenter) + t.Translation.Y, |
66 | Vector3.Dot(row3, localCenter) + t.Translation.Z); | 66 | Vector3.Dot(row3, localCenter) + t.Translation.Z); |
67 | 67 | ||
68 | Vector3 extent = new Vector3(Vector3.Dot(row1, localHalfExtents), | 68 | Vector3 extent = new Vector3(Vector3.Dot(row1, localHalfExtents), |
69 | Vector3.Dot(row2, localHalfExtents), | 69 | Vector3.Dot(row2, localHalfExtents), |
70 | Vector3.Dot(row3, localHalfExtents)); | 70 | Vector3.Dot(row3, localHalfExtents)); |
71 | 71 | ||
72 | aabbMin = center - extent; | 72 | aabbMin = center - extent; |
73 | aabbMax = center + extent; | 73 | aabbMax = center + extent; |
74 | } | 74 | } |
75 | 75 | ||
76 | public override BroadphaseNativeTypes ShapeType | 76 | public override BroadphaseNativeTypes ShapeType |
77 | { | 77 | { |
78 | get | 78 | get |
79 | { | 79 | { |
80 | return BroadphaseNativeTypes.Compound; | 80 | return BroadphaseNativeTypes.Compound; |
81 | } | 81 | } |
82 | } | 82 | } |
83 | 83 | ||
84 | public override Vector3 LocalScaling | 84 | public override Vector3 LocalScaling |
85 | { | 85 | { |
86 | get | 86 | get |
87 | { | 87 | { |
88 | return _localScaling; | 88 | return _localScaling; |
89 | } | 89 | } |
90 | set | 90 | set |
91 | { | 91 | { |
92 | _localScaling = value; | 92 | _localScaling = value; |
93 | } | 93 | } |
94 | } | 94 | } |
95 | 95 | ||
96 | public override string Name | 96 | public override string Name |
97 | { | 97 | { |
98 | get | 98 | get |
99 | { | 99 | { |
100 | return "Compound"; | 100 | return "Compound"; |
101 | } | 101 | } |
102 | } | 102 | } |
103 | 103 | ||
104 | public override float Margin | 104 | public override float Margin |
105 | { | 105 | { |
106 | get | 106 | get |
107 | { | 107 | { |
108 | return _collisionMargin; | 108 | return _collisionMargin; |
109 | } | 109 | } |
110 | set | 110 | set |
111 | { | 111 | { |
112 | _collisionMargin = value; | 112 | _collisionMargin = value; |
113 | } | 113 | } |
114 | } | 114 | } |
115 | 115 | ||
116 | public int ChildShapeCount { get { return _childShapes.Count; } } | 116 | public int ChildShapeCount { get { return _childShapes.Count; } } |
117 | //this is optional, but should make collision queries faster, by culling non-overlapping nodes | 117 | //this is optional, but should make collision queries faster, by culling non-overlapping nodes |
118 | public OptimizedBvh AabbTree { get { return _aabbTree; } } | 118 | public OptimizedBvh AabbTree { get { return _aabbTree; } } |
119 | 119 | ||
120 | public CollisionShape GetChildShape(int index) | 120 | public CollisionShape GetChildShape(int index) |
121 | { | 121 | { |
122 | return _childShapes[index]; | 122 | return _childShapes[index]; |
123 | } | 123 | } |
124 | 124 | ||
125 | public Matrix GetChildTransform(int index) | 125 | public Matrix GetChildTransform(int index) |
126 | { | 126 | { |
127 | return _childTransforms[index]; | 127 | return _childTransforms[index]; |
128 | } | 128 | } |
129 | 129 | ||
130 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | 130 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) |
131 | { | 131 | { |
132 | //approximation: take the inertia from the aabb for now | 132 | //approximation: take the inertia from the aabb for now |
133 | Matrix ident = Matrix.Identity; | 133 | Matrix ident = Matrix.Identity; |
134 | Vector3 aabbMin, aabbMax; | 134 | Vector3 aabbMin, aabbMax; |
135 | GetAabb(ident, out aabbMin, out aabbMax); | 135 | GetAabb(ident, out aabbMin, out aabbMax); |
136 | 136 | ||
137 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; | 137 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; |
138 | 138 | ||
139 | float lx = 2f * (halfExtents.X); | 139 | float lx = 2f * (halfExtents.X); |
140 | float ly = 2f * (halfExtents.Y); | 140 | float ly = 2f * (halfExtents.Y); |
141 | float lz = 2f * (halfExtents.Z); | 141 | float lz = 2f * (halfExtents.Z); |
142 | 142 | ||
143 | inertia = new Vector3(); | 143 | inertia = new Vector3(); |
144 | inertia.X = mass / (12.0f) * (ly * ly + lz * lz); | 144 | inertia.X = mass / (12.0f) * (ly * ly + lz * lz); |
145 | inertia.Y = mass / (12.0f) * (lx * lx + lz * lz); | 145 | inertia.Y = mass / (12.0f) * (lx * lx + lz * lz); |
146 | inertia.Z = mass / (12.0f) * (lx * lx + ly * ly); | 146 | inertia.Z = mass / (12.0f) * (lx * lx + ly * ly); |
147 | } | 147 | } |
148 | 148 | ||
149 | public void AddChildShape(Matrix localTransform, CollisionShape shape) | 149 | public void AddChildShape(Matrix localTransform, CollisionShape shape) |
150 | { | 150 | { |
151 | _childTransforms.Add(localTransform); | 151 | _childTransforms.Add(localTransform); |
152 | _childShapes.Add(shape); | 152 | _childShapes.Add(shape); |
153 | 153 | ||
154 | //extend the local aabbMin/aabbMax | 154 | //extend the local aabbMin/aabbMax |
155 | Vector3 localAabbMin, localAabbMax; | 155 | Vector3 localAabbMin, localAabbMax; |
156 | shape.GetAabb(localTransform, out localAabbMin, out localAabbMax); | 156 | shape.GetAabb(localTransform, out localAabbMin, out localAabbMax); |
157 | if (_localAabbMin.X > localAabbMin.X) | 157 | if (_localAabbMin.X > localAabbMin.X) |
158 | { | 158 | { |
159 | _localAabbMin.X = localAabbMin.X; | 159 | _localAabbMin.X = localAabbMin.X; |
160 | } | 160 | } |
161 | if (_localAabbMax.X < localAabbMax.X) | 161 | if (_localAabbMax.X < localAabbMax.X) |
162 | { | 162 | { |
163 | _localAabbMax.X = localAabbMax.X; | 163 | _localAabbMax.X = localAabbMax.X; |
164 | } | 164 | } |
165 | if (_localAabbMin.Y > localAabbMin.Y) | 165 | if (_localAabbMin.Y > localAabbMin.Y) |
166 | { | 166 | { |
167 | _localAabbMin.Y = localAabbMin.Y; | 167 | _localAabbMin.Y = localAabbMin.Y; |
168 | } | 168 | } |
169 | if (_localAabbMax.Y < localAabbMax.Y) | 169 | if (_localAabbMax.Y < localAabbMax.Y) |
170 | { | 170 | { |
171 | _localAabbMax.Y = localAabbMax.Y; | 171 | _localAabbMax.Y = localAabbMax.Y; |
172 | } | 172 | } |
173 | if (_localAabbMin.Z > localAabbMin.Z) | 173 | if (_localAabbMin.Z > localAabbMin.Z) |
174 | { | 174 | { |
175 | _localAabbMin.Z = localAabbMin.Z; | 175 | _localAabbMin.Z = localAabbMin.Z; |
176 | } | 176 | } |
177 | if (_localAabbMax.Z < localAabbMax.Z) | 177 | if (_localAabbMax.Z < localAabbMax.Z) |
178 | { | 178 | { |
179 | _localAabbMax.Z = localAabbMax.Z; | 179 | _localAabbMax.Z = localAabbMax.Z; |
180 | } | 180 | } |
181 | } | 181 | } |
182 | } | 182 | } |
183 | } | 183 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs index 1e773f0..c0b177d 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs | |||
@@ -1,55 +1,55 @@ | |||
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 | public abstract class ConcaveShape : CollisionShape | 29 | public abstract class ConcaveShape : CollisionShape |
30 | { | 30 | { |
31 | private float _collisionMargin; | 31 | private float _collisionMargin; |
32 | 32 | ||
33 | public ConcaveShape() { } | 33 | public ConcaveShape() { } |
34 | 34 | ||
35 | public float CollisionMargin | 35 | public float CollisionMargin |
36 | { | 36 | { |
37 | get { return _collisionMargin; } | 37 | get { return _collisionMargin; } |
38 | set { _collisionMargin = value; } | 38 | set { _collisionMargin = value; } |
39 | } | 39 | } |
40 | 40 | ||
41 | public override float Margin | 41 | public override float Margin |
42 | { | 42 | { |
43 | get | 43 | get |
44 | { | 44 | { |
45 | return _collisionMargin; | 45 | return _collisionMargin; |
46 | } | 46 | } |
47 | set | 47 | set |
48 | { | 48 | { |
49 | _collisionMargin = value; | 49 | _collisionMargin = value; |
50 | } | 50 | } |
51 | } | 51 | } |
52 | 52 | ||
53 | public abstract void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax); | 53 | public abstract void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax); |
54 | } | 54 | } |
55 | } \ No newline at end of file | 55 | } \ No newline at end of file |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs index 361c277..52084f4 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs | |||
@@ -1,208 +1,208 @@ | |||
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 | /// ConeShape implements a Cone shape, around the X axis | 30 | /// ConeShape implements a Cone shape, around the X axis |
31 | /// </summary> | 31 | /// </summary> |
32 | public class ConeShapeX : ConeShape | 32 | public class ConeShapeX : ConeShape |
33 | { | 33 | { |
34 | public ConeShapeX(float radius, float height) | 34 | public ConeShapeX(float radius, float height) |
35 | : base(radius, height) | 35 | : base(radius, height) |
36 | { | 36 | { |
37 | ConeUpIndex = 0; | 37 | ConeUpIndex = 0; |
38 | } | 38 | } |
39 | } | 39 | } |
40 | 40 | ||
41 | /// <summary> | 41 | /// <summary> |
42 | /// ConeShape implements a Cone shape, around the Z axis | 42 | /// ConeShape implements a Cone shape, around the Z axis |
43 | /// </summary> | 43 | /// </summary> |
44 | public class ConeShapeZ : ConeShape | 44 | public class ConeShapeZ : ConeShape |
45 | { | 45 | { |
46 | public ConeShapeZ(float radius, float height) | 46 | public ConeShapeZ(float radius, float height) |
47 | : base(radius, height) | 47 | : base(radius, height) |
48 | { | 48 | { |
49 | ConeUpIndex = 2; | 49 | ConeUpIndex = 2; |
50 | } | 50 | } |
51 | } | 51 | } |
52 | 52 | ||
53 | /// <summary> | 53 | /// <summary> |
54 | /// ConeShape implements a Cone shape, around the Y axis | 54 | /// ConeShape implements a Cone shape, around the Y axis |
55 | /// </summary> | 55 | /// </summary> |
56 | public class ConeShape : ConvexShape | 56 | public class ConeShape : ConvexShape |
57 | { | 57 | { |
58 | private float _sinAngle; | 58 | private float _sinAngle; |
59 | private float _radius; | 59 | private float _radius; |
60 | private float _height; | 60 | private float _height; |
61 | private int[] _coneIndices = new int[3]; | 61 | private int[] _coneIndices = new int[3]; |
62 | 62 | ||
63 | public ConeShape(float radius, float height) | 63 | public ConeShape(float radius, float height) |
64 | { | 64 | { |
65 | _radius = radius; | 65 | _radius = radius; |
66 | _height = height; | 66 | _height = height; |
67 | ConeUpIndex = 1; | 67 | ConeUpIndex = 1; |
68 | _sinAngle = (_radius / (float)Math.Sqrt(_radius * _radius + _height * _height)); | 68 | _sinAngle = (_radius / (float)Math.Sqrt(_radius * _radius + _height * _height)); |
69 | } | 69 | } |
70 | 70 | ||
71 | public float Radius { get { return _radius; } } | 71 | public float Radius { get { return _radius; } } |
72 | public float Height { get { return _height; } } | 72 | public float Height { get { return _height; } } |
73 | 73 | ||
74 | public override BroadphaseNativeTypes ShapeType | 74 | public override BroadphaseNativeTypes ShapeType |
75 | { | 75 | { |
76 | get | 76 | get |
77 | { | 77 | { |
78 | return BroadphaseNativeTypes.Cone; | 78 | return BroadphaseNativeTypes.Cone; |
79 | } | 79 | } |
80 | } | 80 | } |
81 | 81 | ||
82 | public override string Name | 82 | public override string Name |
83 | { | 83 | { |
84 | get | 84 | get |
85 | { | 85 | { |
86 | return "Cone"; | 86 | return "Cone"; |
87 | } | 87 | } |
88 | } | 88 | } |
89 | 89 | ||
90 | //choose upAxis index | 90 | //choose upAxis index |
91 | public int ConeUpIndex | 91 | public int ConeUpIndex |
92 | { | 92 | { |
93 | get { return _coneIndices[1]; } | 93 | get { return _coneIndices[1]; } |
94 | set | 94 | set |
95 | { | 95 | { |
96 | switch (value) | 96 | switch (value) |
97 | { | 97 | { |
98 | case 0: | 98 | case 0: |
99 | _coneIndices[0] = 1; | 99 | _coneIndices[0] = 1; |
100 | _coneIndices[1] = 0; | 100 | _coneIndices[1] = 0; |
101 | _coneIndices[2] = 2; | 101 | _coneIndices[2] = 2; |
102 | break; | 102 | break; |
103 | case 1: | 103 | case 1: |
104 | _coneIndices[0] = 0; | 104 | _coneIndices[0] = 0; |
105 | _coneIndices[1] = 1; | 105 | _coneIndices[1] = 1; |
106 | _coneIndices[2] = 2; | 106 | _coneIndices[2] = 2; |
107 | break; | 107 | break; |
108 | case 2: | 108 | case 2: |
109 | _coneIndices[0] = 0; | 109 | _coneIndices[0] = 0; |
110 | _coneIndices[1] = 2; | 110 | _coneIndices[1] = 2; |
111 | _coneIndices[2] = 1; | 111 | _coneIndices[2] = 1; |
112 | break; | 112 | break; |
113 | default: | 113 | default: |
114 | BulletDebug.Assert(false); | 114 | BulletDebug.Assert(false); |
115 | break; | 115 | break; |
116 | } | 116 | } |
117 | } | 117 | } |
118 | } | 118 | } |
119 | 119 | ||
120 | private Vector3 ConeLocalSupport(Vector3 v) | 120 | private Vector3 ConeLocalSupport(Vector3 v) |
121 | { | 121 | { |
122 | float halfHeight = _height * 0.5f; | 122 | float halfHeight = _height * 0.5f; |
123 | bool condition; | 123 | bool condition; |
124 | 124 | ||
125 | if (_coneIndices[1] == 0) | 125 | if (_coneIndices[1] == 0) |
126 | condition = v.X > v.Length() * _sinAngle; | 126 | condition = v.X > v.Length() * _sinAngle; |
127 | else if (_coneIndices[1] == 1) | 127 | else if (_coneIndices[1] == 1) |
128 | condition = v.Y > v.Length() * _sinAngle; | 128 | condition = v.Y > v.Length() * _sinAngle; |
129 | else | 129 | else |
130 | condition = v.Z > v.Length() * _sinAngle; | 130 | condition = v.Z > v.Length() * _sinAngle; |
131 | 131 | ||
132 | if (condition) | 132 | if (condition) |
133 | { | 133 | { |
134 | Vector3 tmp = new Vector3(); | 134 | Vector3 tmp = new Vector3(); |
135 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], halfHeight); | 135 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], halfHeight); |
136 | return tmp; | 136 | return tmp; |
137 | } | 137 | } |
138 | else | 138 | else |
139 | { | 139 | { |
140 | float s = (float)Math.Sqrt(MathHelper.GetValueByIndex(v, _coneIndices[0]) * MathHelper.GetValueByIndex(v, _coneIndices[0]) | 140 | float s = (float)Math.Sqrt(MathHelper.GetValueByIndex(v, _coneIndices[0]) * MathHelper.GetValueByIndex(v, _coneIndices[0]) |
141 | + MathHelper.GetValueByIndex(v, _coneIndices[2]) * MathHelper.GetValueByIndex(v, _coneIndices[2])); | 141 | + MathHelper.GetValueByIndex(v, _coneIndices[2]) * MathHelper.GetValueByIndex(v, _coneIndices[2])); |
142 | if (s > MathHelper.Epsilon) | 142 | if (s > MathHelper.Epsilon) |
143 | { | 143 | { |
144 | float d = _radius / s; | 144 | float d = _radius / s; |
145 | Vector3 tmp = new Vector3(); | 145 | Vector3 tmp = new Vector3(); |
146 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[0], MathHelper.GetValueByIndex(v, _coneIndices[0]) * d); | 146 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[0], MathHelper.GetValueByIndex(v, _coneIndices[0]) * d); |
147 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); | 147 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); |
148 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[2], MathHelper.GetValueByIndex(v, _coneIndices[2]) * d); | 148 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[2], MathHelper.GetValueByIndex(v, _coneIndices[2]) * d); |
149 | return tmp; | 149 | return tmp; |
150 | } | 150 | } |
151 | else | 151 | else |
152 | { | 152 | { |
153 | Vector3 tmp = new Vector3(); | 153 | Vector3 tmp = new Vector3(); |
154 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); | 154 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); |
155 | return tmp; | 155 | return tmp; |
156 | } | 156 | } |
157 | } | 157 | } |
158 | } | 158 | } |
159 | 159 | ||
160 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | 160 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) |
161 | { | 161 | { |
162 | return ConeLocalSupport(vec); | 162 | return ConeLocalSupport(vec); |
163 | } | 163 | } |
164 | 164 | ||
165 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | 165 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) |
166 | { | 166 | { |
167 | for (int i = 0; i < vectors.Length; i++) | 167 | for (int i = 0; i < vectors.Length; i++) |
168 | supportVerticesOut[i] = ConeLocalSupport(vectors[i]); | 168 | supportVerticesOut[i] = ConeLocalSupport(vectors[i]); |
169 | } | 169 | } |
170 | 170 | ||
171 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | 171 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) |
172 | { | 172 | { |
173 | Matrix identity = Matrix.Identity; | 173 | Matrix identity = Matrix.Identity; |
174 | Vector3 aabbMin, aabbMax; | 174 | Vector3 aabbMin, aabbMax; |
175 | GetAabb(identity, out aabbMin, out aabbMax); | 175 | GetAabb(identity, out aabbMin, out aabbMax); |
176 | 176 | ||
177 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; | 177 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; |
178 | 178 | ||
179 | float margin = Margin; | 179 | float margin = Margin; |
180 | 180 | ||
181 | float lx = 2f * (halfExtents.X + margin); | 181 | float lx = 2f * (halfExtents.X + margin); |
182 | float ly = 2f * (halfExtents.Y + margin); | 182 | float ly = 2f * (halfExtents.Y + margin); |
183 | float lz = 2f * (halfExtents.Z + margin); | 183 | float lz = 2f * (halfExtents.Z + margin); |
184 | float x2 = lx * lx; | 184 | float x2 = lx * lx; |
185 | float y2 = ly * ly; | 185 | float y2 = ly * ly; |
186 | float z2 = lz * lz; | 186 | float z2 = lz * lz; |
187 | float scaledmass = mass * 0.08333333f; | 187 | float scaledmass = mass * 0.08333333f; |
188 | 188 | ||
189 | inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2)); | 189 | inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2)); |
190 | } | 190 | } |
191 | 191 | ||
192 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | 192 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) |
193 | { | 193 | { |
194 | Vector3 supVertex = ConeLocalSupport(vec); | 194 | Vector3 supVertex = ConeLocalSupport(vec); |
195 | if (Margin != 0) | 195 | if (Margin != 0) |
196 | { | 196 | { |
197 | Vector3 vecnorm = vec; | 197 | Vector3 vecnorm = vec; |
198 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | 198 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) |
199 | { | 199 | { |
200 | vecnorm = new Vector3(-1f, -1f, -1f); | 200 | vecnorm = new Vector3(-1f, -1f, -1f); |
201 | } | 201 | } |
202 | vecnorm = Vector3.Normalize(vecnorm); | 202 | vecnorm = Vector3.Normalize(vecnorm); |
203 | supVertex += Margin * vecnorm; | 203 | supVertex += Margin * vecnorm; |
204 | } | 204 | } |
205 | return supVertex; | 205 | return supVertex; |
206 | } | 206 | } |
207 | } | 207 | } |
208 | } | 208 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs index eb78533..8d06c90 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs | |||
@@ -1,184 +1,184 @@ | |||
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 | /// ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (vertices) | 30 | /// ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (vertices) |
31 | /// No connectivity is needed. localGetSupportingVertex iterates linearly though all vertices. | 31 | /// No connectivity is needed. localGetSupportingVertex iterates linearly though all vertices. |
32 | /// on modern hardware, due to cache coherency this isn't that bad. Complex algorithms tend to trash the cash. | 32 | /// on modern hardware, due to cache coherency this isn't that bad. Complex algorithms tend to trash the cash. |
33 | /// (memory is much slower then the cpu) | 33 | /// (memory is much slower then the cpu) |
34 | /// </summary> | 34 | /// </summary> |
35 | public class ConvexHullShape : PolyhedralConvexShape | 35 | public class ConvexHullShape : PolyhedralConvexShape |
36 | { | 36 | { |
37 | private List<Vector3> _points = new List<Vector3>(); | 37 | private List<Vector3> _points = new List<Vector3>(); |
38 | 38 | ||
39 | public ConvexHullShape() { } | 39 | public ConvexHullShape() { } |
40 | 40 | ||
41 | public override int VertexCount | 41 | public override int VertexCount |
42 | { | 42 | { |
43 | get | 43 | get |
44 | { | 44 | { |
45 | return _points.Count; | 45 | return _points.Count; |
46 | } | 46 | } |
47 | } | 47 | } |
48 | 48 | ||
49 | public override int EdgeCount | 49 | public override int EdgeCount |
50 | { | 50 | { |
51 | get | 51 | get |
52 | { | 52 | { |
53 | return _points.Count; | 53 | return _points.Count; |
54 | } | 54 | } |
55 | } | 55 | } |
56 | 56 | ||
57 | public override int PlaneCount | 57 | public override int PlaneCount |
58 | { | 58 | { |
59 | get | 59 | get |
60 | { | 60 | { |
61 | return 0; | 61 | return 0; |
62 | } | 62 | } |
63 | } | 63 | } |
64 | 64 | ||
65 | public override BroadphaseNativeTypes ShapeType | 65 | public override BroadphaseNativeTypes ShapeType |
66 | { | 66 | { |
67 | get | 67 | get |
68 | { | 68 | { |
69 | return BroadphaseNativeTypes.ConvexHull; | 69 | return BroadphaseNativeTypes.ConvexHull; |
70 | } | 70 | } |
71 | } | 71 | } |
72 | 72 | ||
73 | public override string Name | 73 | public override string Name |
74 | { | 74 | { |
75 | get | 75 | get |
76 | { | 76 | { |
77 | return "Convex"; | 77 | return "Convex"; |
78 | } | 78 | } |
79 | } | 79 | } |
80 | 80 | ||
81 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | 81 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) |
82 | { | 82 | { |
83 | Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); | 83 | Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); |
84 | 84 | ||
85 | if (Margin != 0) | 85 | if (Margin != 0) |
86 | { | 86 | { |
87 | Vector3 vecnorm = vec; | 87 | Vector3 vecnorm = vec; |
88 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | 88 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) |
89 | { | 89 | { |
90 | vecnorm=new Vector3(-1, -1, -1); | 90 | vecnorm=new Vector3(-1, -1, -1); |
91 | } | 91 | } |
92 | vecnorm = Vector3.Normalize(vecnorm); | 92 | vecnorm = Vector3.Normalize(vecnorm); |
93 | supVertex += Margin * vecnorm; | 93 | supVertex += Margin * vecnorm; |
94 | } | 94 | } |
95 | return supVertex; | 95 | return supVertex; |
96 | } | 96 | } |
97 | 97 | ||
98 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0) | 98 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0) |
99 | { | 99 | { |
100 | Vector3 supVec = new Vector3(); | 100 | Vector3 supVec = new Vector3(); |
101 | float newDot, maxDot = -1e30f; | 101 | float newDot, maxDot = -1e30f; |
102 | 102 | ||
103 | Vector3 vec = vec0; | 103 | Vector3 vec = vec0; |
104 | float lenSqr = vec.LengthSquared(); | 104 | float lenSqr = vec.LengthSquared(); |
105 | if (lenSqr < 0.0001f) | 105 | if (lenSqr < 0.0001f) |
106 | { | 106 | { |
107 | vec = new Vector3(1, 0, 0); | 107 | vec = new Vector3(1, 0, 0); |
108 | } | 108 | } |
109 | else | 109 | else |
110 | { | 110 | { |
111 | float rlen = 1f / (float)Math.Sqrt(lenSqr); | 111 | float rlen = 1f / (float)Math.Sqrt(lenSqr); |
112 | vec *= rlen; | 112 | vec *= rlen; |
113 | } | 113 | } |
114 | 114 | ||
115 | for (int i = 0; i < _points.Count; i++) | 115 | for (int i = 0; i < _points.Count; i++) |
116 | { | 116 | { |
117 | Vector3 vtx = _points[i] * LocalScaling; | 117 | Vector3 vtx = _points[i] * LocalScaling; |
118 | 118 | ||
119 | newDot = Vector3.Dot(vec, vtx); | 119 | newDot = Vector3.Dot(vec, vtx); |
120 | if (newDot > maxDot) | 120 | if (newDot > maxDot) |
121 | { | 121 | { |
122 | maxDot = newDot; | 122 | maxDot = newDot; |
123 | supVec = vtx; | 123 | supVec = vtx; |
124 | } | 124 | } |
125 | } | 125 | } |
126 | return supVec; | 126 | return supVec; |
127 | } | 127 | } |
128 | 128 | ||
129 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | 129 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) |
130 | { | 130 | { |
131 | float newDot; | 131 | float newDot; |
132 | //use 'w' component of supportVerticesOut? | 132 | //use 'w' component of supportVerticesOut? |
133 | /*{ | 133 | /*{ |
134 | for (int i = 0; i < numVectors; i++) | 134 | for (int i = 0; i < numVectors; i++) |
135 | { | 135 | { |
136 | supportVerticesOut[i][3] = -1e30f; | 136 | supportVerticesOut[i][3] = -1e30f; |
137 | } | 137 | } |
138 | }*/ | 138 | }*/ |
139 | #warning Warning! | 139 | #warning Warning! |
140 | for (int i = 0; i < _points.Count; i++) | 140 | for (int i = 0; i < _points.Count; i++) |
141 | { | 141 | { |
142 | Vector3 vtx = _points[i] * LocalScaling; | 142 | Vector3 vtx = _points[i] * LocalScaling; |
143 | 143 | ||
144 | for (int j = 0; j < vectors.Length; j++) | 144 | for (int j = 0; j < vectors.Length; j++) |
145 | { | 145 | { |
146 | newDot = Vector3.Dot(vectors[j], vtx); | 146 | newDot = Vector3.Dot(vectors[j], vtx); |
147 | if (newDot > -1e30f) | 147 | if (newDot > -1e30f) |
148 | { | 148 | { |
149 | //WARNING: don't swap next lines, the w component would get overwritten! | 149 | //WARNING: don't swap next lines, the w component would get overwritten! |
150 | supportVerticesOut[j] = vtx; | 150 | supportVerticesOut[j] = vtx; |
151 | //supportVerticesOut[j][3] = newDot; | 151 | //supportVerticesOut[j][3] = newDot; |
152 | #warning Warning! | 152 | #warning Warning! |
153 | } | 153 | } |
154 | } | 154 | } |
155 | } | 155 | } |
156 | } | 156 | } |
157 | 157 | ||
158 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) | 158 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) |
159 | { | 159 | { |
160 | int index0 = i % _points.Count; | 160 | int index0 = i % _points.Count; |
161 | int index1 = (i + 1) % _points.Count; | 161 | int index1 = (i + 1) % _points.Count; |
162 | pa = _points[index0] * LocalScaling; | 162 | pa = _points[index0] * LocalScaling; |
163 | pb = _points[index1] * LocalScaling; | 163 | pb = _points[index1] * LocalScaling; |
164 | } | 164 | } |
165 | 165 | ||
166 | public override void GetVertex(int i, out Vector3 vtx) | 166 | public override void GetVertex(int i, out Vector3 vtx) |
167 | { | 167 | { |
168 | vtx = _points[i] * LocalScaling; | 168 | vtx = _points[i] * LocalScaling; |
169 | } | 169 | } |
170 | 170 | ||
171 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) | 171 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) |
172 | { | 172 | { |
173 | planeNormal = new Vector3(); | 173 | planeNormal = new Vector3(); |
174 | planeSupport = new Vector3(); | 174 | planeSupport = new Vector3(); |
175 | BulletDebug.Assert(false); | 175 | BulletDebug.Assert(false); |
176 | } | 176 | } |
177 | 177 | ||
178 | public override bool IsInside(Vector3 pt, float tolerance) | 178 | public override bool IsInside(Vector3 pt, float tolerance) |
179 | { | 179 | { |
180 | BulletDebug.Assert(false); | 180 | BulletDebug.Assert(false); |
181 | return false; | 181 | return false; |
182 | } | 182 | } |
183 | } | 183 | } |
184 | } | 184 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs index 02d678e..d5c2fbd 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs | |||
@@ -1,141 +1,141 @@ | |||
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 | /// ConvexShape is an abstract shape interface. | 30 | /// ConvexShape is an abstract shape interface. |
31 | /// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface. | 31 | /// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface. |
32 | /// used in combination with GJK or btConvexCast | 32 | /// used in combination with GJK or btConvexCast |
33 | /// </summary> | 33 | /// </summary> |
34 | public abstract class ConvexShape : CollisionShape | 34 | public abstract class ConvexShape : CollisionShape |
35 | { | 35 | { |
36 | private const int _maxPreferredPenetrationDirections = 10; | 36 | private const int _maxPreferredPenetrationDirections = 10; |
37 | private const float _convexDistanceMargin = 0.04f; | 37 | private const float _convexDistanceMargin = 0.04f; |
38 | 38 | ||
39 | private Vector3 _localScaling; | 39 | private Vector3 _localScaling; |
40 | private Vector3 _implicitShapeDimensions; | 40 | private Vector3 _implicitShapeDimensions; |
41 | private float _collisionMargin; | 41 | private float _collisionMargin; |
42 | 42 | ||
43 | public ConvexShape() | 43 | public ConvexShape() |
44 | : base() | 44 | : base() |
45 | { | 45 | { |
46 | _localScaling = Vector3.One; | 46 | _localScaling = Vector3.One; |
47 | _collisionMargin = ConvexDistanceMargin; | 47 | _collisionMargin = ConvexDistanceMargin; |
48 | } | 48 | } |
49 | 49 | ||
50 | public static int MaxPreferredPenetrationDirections { get { return _maxPreferredPenetrationDirections; } } | 50 | public static int MaxPreferredPenetrationDirections { get { return _maxPreferredPenetrationDirections; } } |
51 | public static float ConvexDistanceMargin { get { return _convexDistanceMargin; } } | 51 | public static float ConvexDistanceMargin { get { return _convexDistanceMargin; } } |
52 | 52 | ||
53 | public Vector3 ImplicitShapeDimensions { get { return _implicitShapeDimensions; } protected set { _implicitShapeDimensions = value; } } | 53 | public Vector3 ImplicitShapeDimensions { get { return _implicitShapeDimensions; } protected set { _implicitShapeDimensions = value; } } |
54 | public virtual int PreferredPenetrationDirectionsCount { get { return 0; } } | 54 | public virtual int PreferredPenetrationDirectionsCount { get { return 0; } } |
55 | 55 | ||
56 | protected float CollisionMargin { get { return _collisionMargin; } set { _collisionMargin = value; } } | 56 | protected float CollisionMargin { get { return _collisionMargin; } set { _collisionMargin = value; } } |
57 | 57 | ||
58 | public virtual void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) | 58 | public virtual void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) |
59 | { | 59 | { |
60 | penetrationVector = new Vector3(); | 60 | penetrationVector = new Vector3(); |
61 | BulletDebug.Assert(false); | 61 | BulletDebug.Assert(false); |
62 | } | 62 | } |
63 | 63 | ||
64 | public abstract Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec); | 64 | public abstract Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec); |
65 | //notice that the vectors should be unit length | 65 | //notice that the vectors should be unit length |
66 | public abstract void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut); | 66 | public abstract void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut); |
67 | 67 | ||
68 | /// <summary> | 68 | /// <summary> |
69 | /// getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version | 69 | /// getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version |
70 | /// </summary> | 70 | /// </summary> |
71 | /// <param name="t"></param> | 71 | /// <param name="t"></param> |
72 | /// <param name="aabbMin"></param> | 72 | /// <param name="aabbMin"></param> |
73 | /// <param name="aabbMax"></param> | 73 | /// <param name="aabbMax"></param> |
74 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | 74 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) |
75 | { | 75 | { |
76 | GetAabbSlow(t, out aabbMin, out aabbMax); | 76 | GetAabbSlow(t, out aabbMin, out aabbMax); |
77 | } | 77 | } |
78 | 78 | ||
79 | public override Vector3 LocalScaling | 79 | public override Vector3 LocalScaling |
80 | { | 80 | { |
81 | get | 81 | get |
82 | { | 82 | { |
83 | return _localScaling; | 83 | return _localScaling; |
84 | } | 84 | } |
85 | set | 85 | set |
86 | { | 86 | { |
87 | _localScaling = value; | 87 | _localScaling = value; |
88 | } | 88 | } |
89 | } | 89 | } |
90 | 90 | ||
91 | public override float Margin | 91 | public override float Margin |
92 | { | 92 | { |
93 | get | 93 | get |
94 | { | 94 | { |
95 | return _collisionMargin; | 95 | return _collisionMargin; |
96 | } | 96 | } |
97 | set | 97 | set |
98 | { | 98 | { |
99 | _collisionMargin = value; | 99 | _collisionMargin = value; |
100 | } | 100 | } |
101 | } | 101 | } |
102 | 102 | ||
103 | public virtual Vector3 LocalGetSupportingVertex(Vector3 vec) | 103 | public virtual Vector3 LocalGetSupportingVertex(Vector3 vec) |
104 | { | 104 | { |
105 | Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); | 105 | Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); |
106 | 106 | ||
107 | if (Margin != 0f) | 107 | if (Margin != 0f) |
108 | { | 108 | { |
109 | Vector3 vecnorm = vec; | 109 | Vector3 vecnorm = vec; |
110 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | 110 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) |
111 | { | 111 | { |
112 | vecnorm = new Vector3(-1f, -1f, -1f); | 112 | vecnorm = new Vector3(-1f, -1f, -1f); |
113 | } | 113 | } |
114 | vecnorm.Normalize(); | 114 | vecnorm.Normalize(); |
115 | supVertex += Margin * vecnorm; | 115 | supVertex += Margin * vecnorm; |
116 | } | 116 | } |
117 | return supVertex; | 117 | return supVertex; |
118 | } | 118 | } |
119 | 119 | ||
120 | public virtual void GetAabbSlow(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | 120 | public virtual void GetAabbSlow(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) |
121 | { | 121 | { |
122 | float margin = Margin; | 122 | float margin = Margin; |
123 | aabbMax = new Vector3(); | 123 | aabbMax = new Vector3(); |
124 | aabbMin = new Vector3(); | 124 | aabbMin = new Vector3(); |
125 | 125 | ||
126 | for (int i = 0; i < 3; i++) | 126 | for (int i = 0; i < 3; i++) |
127 | { | 127 | { |
128 | Vector3 vec = new Vector3(0f, 0f, 0f); | 128 | Vector3 vec = new Vector3(0f, 0f, 0f); |
129 | MathHelper.SetElement(ref vec, i, 1); | 129 | MathHelper.SetElement(ref vec, i, 1); |
130 | 130 | ||
131 | Vector3 sv = LocalGetSupportingVertex(Vector3.TransformNormal(vec, t)); | 131 | Vector3 sv = LocalGetSupportingVertex(Vector3.TransformNormal(vec, t)); |
132 | 132 | ||
133 | Vector3 tmp = MathHelper.MatrixToVector(t, sv); | 133 | Vector3 tmp = MathHelper.MatrixToVector(t, sv); |
134 | MathHelper.SetElement(ref aabbMax, i, MathHelper.GetElement(tmp, i) + margin); | 134 | MathHelper.SetElement(ref aabbMax, i, MathHelper.GetElement(tmp, i) + margin); |
135 | MathHelper.SetElement(ref vec, i, -1f); | 135 | MathHelper.SetElement(ref vec, i, -1f); |
136 | tmp = MathHelper.MatrixToVector(t, LocalGetSupportingVertex(Vector3.TransformNormal(vec, t))); | 136 | tmp = MathHelper.MatrixToVector(t, LocalGetSupportingVertex(Vector3.TransformNormal(vec, t))); |
137 | MathHelper.SetElement(ref aabbMin, i, MathHelper.GetElement(tmp, i) - margin); | 137 | MathHelper.SetElement(ref aabbMin, i, MathHelper.GetElement(tmp, i) - margin); |
138 | } | 138 | } |
139 | } | 139 | } |
140 | } | 140 | } |
141 | } | 141 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs index 0fd3fa7..3e46c66 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs | |||
@@ -1,185 +1,185 @@ | |||
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 | /// ConvexTriangleMeshShape is a convex hull of a triangle mesh. If you just have a point cloud, you can use ConvexHullShape instead. | 30 | /// ConvexTriangleMeshShape is a convex hull of a triangle mesh. If you just have a point cloud, you can use ConvexHullShape instead. |
31 | /// It uses the StridingMeshInterface instead of a point cloud. This can avoid the duplication of the triangle mesh data. | 31 | /// It uses the StridingMeshInterface instead of a point cloud. This can avoid the duplication of the triangle mesh data. |
32 | /// </summary> | 32 | /// </summary> |
33 | public class ConvexTriangleMeshShape : PolyhedralConvexShape | 33 | public class ConvexTriangleMeshShape : PolyhedralConvexShape |
34 | { | 34 | { |
35 | private StridingMeshInterface _stridingMesh; | 35 | private StridingMeshInterface _stridingMesh; |
36 | 36 | ||
37 | public ConvexTriangleMeshShape(StridingMeshInterface meshInterface) | 37 | public ConvexTriangleMeshShape(StridingMeshInterface meshInterface) |
38 | { | 38 | { |
39 | _stridingMesh = meshInterface; | 39 | _stridingMesh = meshInterface; |
40 | } | 40 | } |
41 | 41 | ||
42 | public StridingMeshInterface getStridingMesh() | 42 | public StridingMeshInterface getStridingMesh() |
43 | { | 43 | { |
44 | return _stridingMesh; | 44 | return _stridingMesh; |
45 | } | 45 | } |
46 | 46 | ||
47 | public override int VertexCount | 47 | public override int VertexCount |
48 | { | 48 | { |
49 | get | 49 | get |
50 | { | 50 | { |
51 | return 0; | 51 | return 0; |
52 | } | 52 | } |
53 | } | 53 | } |
54 | 54 | ||
55 | public override int EdgeCount | 55 | public override int EdgeCount |
56 | { | 56 | { |
57 | get | 57 | get |
58 | { | 58 | { |
59 | return 0; | 59 | return 0; |
60 | } | 60 | } |
61 | } | 61 | } |
62 | 62 | ||
63 | public override int PlaneCount | 63 | public override int PlaneCount |
64 | { | 64 | { |
65 | get | 65 | get |
66 | { | 66 | { |
67 | return 0; | 67 | return 0; |
68 | } | 68 | } |
69 | } | 69 | } |
70 | 70 | ||
71 | public override Vector3 LocalScaling | 71 | public override Vector3 LocalScaling |
72 | { | 72 | { |
73 | get | 73 | get |
74 | { | 74 | { |
75 | return base.LocalScaling; | 75 | return base.LocalScaling; |
76 | } | 76 | } |
77 | set | 77 | set |
78 | { | 78 | { |
79 | _stridingMesh.Scaling = value; | 79 | _stridingMesh.Scaling = value; |
80 | } | 80 | } |
81 | } | 81 | } |
82 | 82 | ||
83 | public override BroadphaseNativeTypes ShapeType | 83 | public override BroadphaseNativeTypes ShapeType |
84 | { | 84 | { |
85 | get | 85 | get |
86 | { | 86 | { |
87 | return BroadphaseNativeTypes.ConvexTriangleMesh; | 87 | return BroadphaseNativeTypes.ConvexTriangleMesh; |
88 | } | 88 | } |
89 | } | 89 | } |
90 | 90 | ||
91 | public override string Name | 91 | public override string Name |
92 | { | 92 | { |
93 | get | 93 | get |
94 | { | 94 | { |
95 | return "ConvexTrimesh"; | 95 | return "ConvexTrimesh"; |
96 | } | 96 | } |
97 | } | 97 | } |
98 | 98 | ||
99 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) | 99 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) |
100 | { | 100 | { |
101 | pa = new Vector3(); | 101 | pa = new Vector3(); |
102 | pb = new Vector3(); | 102 | pb = new Vector3(); |
103 | BulletDebug.Assert(false); | 103 | BulletDebug.Assert(false); |
104 | } | 104 | } |
105 | 105 | ||
106 | public override void GetVertex(int i, out Vector3 vtx) | 106 | public override void GetVertex(int i, out Vector3 vtx) |
107 | { | 107 | { |
108 | vtx = new Vector3(); | 108 | vtx = new Vector3(); |
109 | BulletDebug.Assert(false); | 109 | BulletDebug.Assert(false); |
110 | } | 110 | } |
111 | 111 | ||
112 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) | 112 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) |
113 | { | 113 | { |
114 | planeNormal = new Vector3(); | 114 | planeNormal = new Vector3(); |
115 | planeSupport = new Vector3(); | 115 | planeSupport = new Vector3(); |
116 | BulletDebug.Assert(false); | 116 | BulletDebug.Assert(false); |
117 | } | 117 | } |
118 | 118 | ||
119 | public override bool IsInside(Vector3 pt, float tolerance) | 119 | public override bool IsInside(Vector3 pt, float tolerance) |
120 | { | 120 | { |
121 | BulletDebug.Assert(false); | 121 | BulletDebug.Assert(false); |
122 | return false; | 122 | return false; |
123 | } | 123 | } |
124 | 124 | ||
125 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | 125 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) |
126 | { | 126 | { |
127 | Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); | 127 | Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); |
128 | 128 | ||
129 | if (Margin != 0) | 129 | if (Margin != 0) |
130 | { | 130 | { |
131 | Vector3 vecnorm = vec; | 131 | Vector3 vecnorm = vec; |
132 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | 132 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) |
133 | { | 133 | { |
134 | vecnorm = new Vector3(-1, -1, -1); | 134 | vecnorm = new Vector3(-1, -1, -1); |
135 | } | 135 | } |
136 | vecnorm = Vector3.Normalize(vecnorm); | 136 | vecnorm = Vector3.Normalize(vecnorm); |
137 | supVertex += Margin * vecnorm; | 137 | supVertex += Margin * vecnorm; |
138 | } | 138 | } |
139 | return supVertex; | 139 | return supVertex; |
140 | } | 140 | } |
141 | 141 | ||
142 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0) | 142 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0) |
143 | { | 143 | { |
144 | Vector3 supVec = new Vector3(); | 144 | Vector3 supVec = new Vector3(); |
145 | 145 | ||
146 | Vector3 vec = vec0; | 146 | Vector3 vec = vec0; |
147 | float lenSqr = vec.LengthSquared(); | 147 | float lenSqr = vec.LengthSquared(); |
148 | if (lenSqr < 0.0001f) | 148 | if (lenSqr < 0.0001f) |
149 | { | 149 | { |
150 | vec = new Vector3(1, 0, 0); | 150 | vec = new Vector3(1, 0, 0); |
151 | } | 151 | } |
152 | else | 152 | else |
153 | { | 153 | { |
154 | float rlen = 1f / (float)Math.Sqrt(lenSqr); | 154 | float rlen = 1f / (float)Math.Sqrt(lenSqr); |
155 | vec *= rlen; | 155 | vec *= rlen; |
156 | } | 156 | } |
157 | 157 | ||
158 | LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec); | 158 | LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec); |
159 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | 159 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); |
160 | _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); | 160 | _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); |
161 | supVec = supportCallback.SupportVertexLocal; | 161 | supVec = supportCallback.SupportVertexLocal; |
162 | 162 | ||
163 | return supVec; | 163 | return supVec; |
164 | } | 164 | } |
165 | 165 | ||
166 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | 166 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) |
167 | { | 167 | { |
168 | //use 'w' component of supportVerticesOut? | 168 | //use 'w' component of supportVerticesOut? |
169 | /*{ | 169 | /*{ |
170 | for (int i = 0; i < numVectors; i++) | 170 | for (int i = 0; i < numVectors; i++) |
171 | { | 171 | { |
172 | supportVerticesOut[i][3] = -1e30f; | 172 | supportVerticesOut[i][3] = -1e30f; |
173 | } | 173 | } |
174 | }*/ | 174 | }*/ |
175 | for (int j = 0; j < vectors.Length; j++) | 175 | for (int j = 0; j < vectors.Length; j++) |
176 | { | 176 | { |
177 | Vector3 vec = vectors[j]; | 177 | Vector3 vec = vectors[j]; |
178 | LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec); | 178 | LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec); |
179 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | 179 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); |
180 | _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); | 180 | _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); |
181 | supportVerticesOut[j] = supportCallback.SupportVertexLocal; | 181 | supportVerticesOut[j] = supportCallback.SupportVertexLocal; |
182 | } | 182 | } |
183 | } | 183 | } |
184 | } | 184 | } |
185 | } | 185 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs index 9a86eaf..85bf769 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs | |||
@@ -1,136 +1,136 @@ | |||
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 | /// implements cylinder shape interface | 30 | /// implements cylinder shape interface |
31 | /// </summary> | 31 | /// </summary> |
32 | public class CylinderShape : BoxShape | 32 | public class CylinderShape : BoxShape |
33 | { | 33 | { |
34 | public CylinderShape(Vector3 halfExtents) | 34 | public CylinderShape(Vector3 halfExtents) |
35 | : base(halfExtents) | 35 | : base(halfExtents) |
36 | { | 36 | { |
37 | } | 37 | } |
38 | 38 | ||
39 | public override BroadphaseNativeTypes ShapeType | 39 | public override BroadphaseNativeTypes ShapeType |
40 | { | 40 | { |
41 | get | 41 | get |
42 | { | 42 | { |
43 | return BroadphaseNativeTypes.Cylinder; | 43 | return BroadphaseNativeTypes.Cylinder; |
44 | } | 44 | } |
45 | } | 45 | } |
46 | 46 | ||
47 | public virtual int UpAxis | 47 | public virtual int UpAxis |
48 | { | 48 | { |
49 | get | 49 | get |
50 | { | 50 | { |
51 | return 1; | 51 | return 1; |
52 | } | 52 | } |
53 | } | 53 | } |
54 | 54 | ||
55 | public virtual float Radius | 55 | public virtual float Radius |
56 | { | 56 | { |
57 | get | 57 | get |
58 | { | 58 | { |
59 | return HalfExtents.Z; | 59 | return HalfExtents.Z; |
60 | } | 60 | } |
61 | } | 61 | } |
62 | 62 | ||
63 | //debugging | 63 | //debugging |
64 | public override string Name | 64 | public override string Name |
65 | { | 65 | { |
66 | get | 66 | get |
67 | { | 67 | { |
68 | return "CylinderY"; | 68 | return "CylinderY"; |
69 | } | 69 | } |
70 | } | 70 | } |
71 | 71 | ||
72 | //getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version | 72 | //getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version |
73 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | 73 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) |
74 | { | 74 | { |
75 | GetAabbSlow(t, out aabbMin, out aabbMax); | 75 | GetAabbSlow(t, out aabbMin, out aabbMax); |
76 | } | 76 | } |
77 | 77 | ||
78 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | 78 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) |
79 | { | 79 | { |
80 | return CylinderLocalSupportY(HalfExtents, vec); | 80 | return CylinderLocalSupportY(HalfExtents, vec); |
81 | } | 81 | } |
82 | 82 | ||
83 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | 83 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) |
84 | { | 84 | { |
85 | for (int i = 0; i < vectors.Length; i++) | 85 | for (int i = 0; i < vectors.Length; i++) |
86 | { | 86 | { |
87 | supportVerticesOut[i] = CylinderLocalSupportY(HalfExtents, vectors[i]); | 87 | supportVerticesOut[i] = CylinderLocalSupportY(HalfExtents, vectors[i]); |
88 | } | 88 | } |
89 | } | 89 | } |
90 | 90 | ||
91 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | 91 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) |
92 | { | 92 | { |
93 | 93 | ||
94 | Vector3 supVertex; | 94 | Vector3 supVertex; |
95 | supVertex = LocalGetSupportingVertexWithoutMargin(vec); | 95 | supVertex = LocalGetSupportingVertexWithoutMargin(vec); |
96 | 96 | ||
97 | if (Margin != 0) | 97 | if (Margin != 0) |
98 | { | 98 | { |
99 | Vector3 vecnorm = vec; | 99 | Vector3 vecnorm = vec; |
100 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | 100 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) |
101 | { | 101 | { |
102 | vecnorm=new Vector3(-1, -1, -1); | 102 | vecnorm=new Vector3(-1, -1, -1); |
103 | } | 103 | } |
104 | vecnorm = Vector3.Normalize(vecnorm); | 104 | vecnorm = Vector3.Normalize(vecnorm); |
105 | supVertex += Margin * vecnorm; | 105 | supVertex += Margin * vecnorm; |
106 | } | 106 | } |
107 | return supVertex; | 107 | return supVertex; |
108 | } | 108 | } |
109 | 109 | ||
110 | private Vector3 CylinderLocalSupportY(Vector3 halfExtents, Vector3 v) | 110 | private Vector3 CylinderLocalSupportY(Vector3 halfExtents, Vector3 v) |
111 | { | 111 | { |
112 | float radius = halfExtents.X; | 112 | float radius = halfExtents.X; |
113 | float halfHeight = halfExtents.Y; | 113 | float halfHeight = halfExtents.Y; |
114 | 114 | ||
115 | Vector3 tmp = new Vector3(); | 115 | Vector3 tmp = new Vector3(); |
116 | float d; | 116 | float d; |
117 | 117 | ||
118 | float s = (float)Math.Sqrt(v.X * v.X + v.Z * v.Z); | 118 | float s = (float)Math.Sqrt(v.X * v.X + v.Z * v.Z); |
119 | if (s != 0) | 119 | if (s != 0) |
120 | { | 120 | { |
121 | d = radius / s; | 121 | d = radius / s; |
122 | tmp.X = v.X * d; | 122 | tmp.X = v.X * d; |
123 | tmp.Y = v.Y < 0 ? -halfHeight : halfHeight; | 123 | tmp.Y = v.Y < 0 ? -halfHeight : halfHeight; |
124 | tmp.Z = v.Z * d; | 124 | tmp.Z = v.Z * d; |
125 | return tmp; | 125 | return tmp; |
126 | } | 126 | } |
127 | else | 127 | else |
128 | { | 128 | { |
129 | tmp.X = radius; | 129 | tmp.X = radius; |
130 | tmp.Y = v.Y < 0 ? -halfHeight : halfHeight; | 130 | tmp.Y = v.Y < 0 ? -halfHeight : halfHeight; |
131 | tmp.Z = 0; | 131 | tmp.Z = 0; |
132 | return tmp; | 132 | return tmp; |
133 | } | 133 | } |
134 | } | 134 | } |
135 | } | 135 | } |
136 | } | 136 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs index 9dc8e2d..da84462 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs | |||
@@ -1,100 +1,100 @@ | |||
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 | public class CylinderShapeX : CylinderShape | 29 | public class CylinderShapeX : CylinderShape |
30 | { | 30 | { |
31 | public CylinderShapeX(Vector3 halfExtents) | 31 | public CylinderShapeX(Vector3 halfExtents) |
32 | : base(halfExtents) { } | 32 | : base(halfExtents) { } |
33 | 33 | ||
34 | public override int UpAxis | 34 | public override int UpAxis |
35 | { | 35 | { |
36 | get | 36 | get |
37 | { | 37 | { |
38 | return 0; | 38 | return 0; |
39 | } | 39 | } |
40 | } | 40 | } |
41 | 41 | ||
42 | public override float Radius | 42 | public override float Radius |
43 | { | 43 | { |
44 | get | 44 | get |
45 | { | 45 | { |
46 | return HalfExtents.Y; | 46 | return HalfExtents.Y; |
47 | } | 47 | } |
48 | } | 48 | } |
49 | 49 | ||
50 | //debugging | 50 | //debugging |
51 | public override string Name | 51 | public override string Name |
52 | { | 52 | { |
53 | get | 53 | get |
54 | { | 54 | { |
55 | return "CylinderX"; | 55 | return "CylinderX"; |
56 | } | 56 | } |
57 | } | 57 | } |
58 | 58 | ||
59 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | 59 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) |
60 | { | 60 | { |
61 | return CylinderLocalSupportX(HalfExtents, vec); | 61 | return CylinderLocalSupportX(HalfExtents, vec); |
62 | } | 62 | } |
63 | 63 | ||
64 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | 64 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) |
65 | { | 65 | { |
66 | for (int i = 0; i < vectors.Length; i++) | 66 | for (int i = 0; i < vectors.Length; i++) |
67 | { | 67 | { |
68 | supportVerticesOut[i] = CylinderLocalSupportX(HalfExtents, vectors[i]); | 68 | supportVerticesOut[i] = CylinderLocalSupportX(HalfExtents, vectors[i]); |
69 | } | 69 | } |
70 | } | 70 | } |
71 | 71 | ||
72 | private Vector3 CylinderLocalSupportX(Vector3 halfExtents, Vector3 v) | 72 | private Vector3 CylinderLocalSupportX(Vector3 halfExtents, Vector3 v) |
73 | { | 73 | { |
74 | //mapping depends on how cylinder local orientation is | 74 | //mapping depends on how cylinder local orientation is |
75 | // extents of the cylinder is: X,Y is for radius, and Z for height | 75 | // extents of the cylinder is: X,Y is for radius, and Z for height |
76 | float radius = halfExtents.Y; | 76 | float radius = halfExtents.Y; |
77 | float halfHeight = halfExtents.X; | 77 | float halfHeight = halfExtents.X; |
78 | 78 | ||
79 | Vector3 tmp = new Vector3(); | 79 | Vector3 tmp = new Vector3(); |
80 | float d; | 80 | float d; |
81 | 81 | ||
82 | float s = (float)Math.Sqrt(v.Y * v.Y + v.Z * v.Z); | 82 | float s = (float)Math.Sqrt(v.Y * v.Y + v.Z * v.Z); |
83 | if (s != 0) | 83 | if (s != 0) |
84 | { | 84 | { |
85 | d = radius / s; | 85 | d = radius / s; |
86 | tmp.Y = v.Y * d; | 86 | tmp.Y = v.Y * d; |
87 | tmp.X = v.X < 0 ? -halfHeight : halfHeight; | 87 | tmp.X = v.X < 0 ? -halfHeight : halfHeight; |
88 | tmp.Z = v.Z * d; | 88 | tmp.Z = v.Z * d; |
89 | return tmp; | 89 | return tmp; |
90 | } | 90 | } |
91 | else | 91 | else |
92 | { | 92 | { |
93 | tmp.Y = radius; | 93 | tmp.Y = radius; |
94 | tmp.X = v.X < 0 ? -halfHeight : halfHeight; | 94 | tmp.X = v.X < 0 ? -halfHeight : halfHeight; |
95 | tmp.Z = 0; | 95 | tmp.Z = 0; |
96 | return tmp; | 96 | return tmp; |
97 | } | 97 | } |
98 | } | 98 | } |
99 | } | 99 | } |
100 | } | 100 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs index 1913e9c..10a1995 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs | |||
@@ -1,100 +1,100 @@ | |||
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 | public class CylinderShapeZ : CylinderShape | 29 | public class CylinderShapeZ : CylinderShape |
30 | { | 30 | { |
31 | public CylinderShapeZ(Vector3 halfExtents) | 31 | public CylinderShapeZ(Vector3 halfExtents) |
32 | : base(halfExtents) { } | 32 | : base(halfExtents) { } |
33 | 33 | ||
34 | public override int UpAxis | 34 | public override int UpAxis |
35 | { | 35 | { |
36 | get | 36 | get |
37 | { | 37 | { |
38 | return 2; | 38 | return 2; |
39 | } | 39 | } |
40 | } | 40 | } |
41 | 41 | ||
42 | public override float Radius | 42 | public override float Radius |
43 | { | 43 | { |
44 | get | 44 | get |
45 | { | 45 | { |
46 | return HalfExtents.X; | 46 | return HalfExtents.X; |
47 | } | 47 | } |
48 | } | 48 | } |
49 | 49 | ||
50 | //debugging | 50 | //debugging |
51 | public override string Name | 51 | public override string Name |
52 | { | 52 | { |
53 | get | 53 | get |
54 | { | 54 | { |
55 | return "CylinderZ"; | 55 | return "CylinderZ"; |
56 | } | 56 | } |
57 | } | 57 | } |
58 | 58 | ||
59 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | 59 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) |
60 | { | 60 | { |
61 | return CylinderLocalSupportZ(HalfExtents, vec); | 61 | return CylinderLocalSupportZ(HalfExtents, vec); |
62 | } | 62 | } |
63 | 63 | ||
64 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | 64 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) |
65 | { | 65 | { |
66 | for (int i = 0; i < vectors.Length; i++) | 66 | for (int i = 0; i < vectors.Length; i++) |
67 | { | 67 | { |
68 | supportVerticesOut[i] = CylinderLocalSupportZ(HalfExtents, vectors[i]); | 68 | supportVerticesOut[i] = CylinderLocalSupportZ(HalfExtents, vectors[i]); |
69 | } | 69 | } |
70 | } | 70 | } |
71 | 71 | ||
72 | Vector3 CylinderLocalSupportZ(Vector3 halfExtents, Vector3 v) | 72 | Vector3 CylinderLocalSupportZ(Vector3 halfExtents, Vector3 v) |
73 | { | 73 | { |
74 | //mapping depends on how cylinder local orientation is | 74 | //mapping depends on how cylinder local orientation is |
75 | // extents of the cylinder is: X,Y is for radius, and Z for height | 75 | // extents of the cylinder is: X,Y is for radius, and Z for height |
76 | float radius = halfExtents.X; | 76 | float radius = halfExtents.X; |
77 | float halfHeight = halfExtents.Z; | 77 | float halfHeight = halfExtents.Z; |
78 | 78 | ||
79 | Vector3 tmp = new Vector3(); | 79 | Vector3 tmp = new Vector3(); |
80 | float d; | 80 | float d; |
81 | 81 | ||
82 | float s = (float)Math.Sqrt(v.X * v.X + v.Y * v.Y); | 82 | float s = (float)Math.Sqrt(v.X * v.X + v.Y * v.Y); |
83 | if (s != 0) | 83 | if (s != 0) |
84 | { | 84 | { |
85 | d = radius / s; | 85 | d = radius / s; |
86 | tmp.X = v.X * d; | 86 | tmp.X = v.X * d; |
87 | tmp.Z = v.Z < 0 ? -halfHeight : halfHeight; | 87 | tmp.Z = v.Z < 0 ? -halfHeight : halfHeight; |
88 | tmp.Y = v.Y * d; | 88 | tmp.Y = v.Y * d; |
89 | return tmp; | 89 | return tmp; |
90 | } | 90 | } |
91 | else | 91 | else |
92 | { | 92 | { |
93 | tmp.X = radius; | 93 | tmp.X = radius; |
94 | tmp.Z = v.Z < 0 ? -halfHeight : halfHeight; | 94 | tmp.Z = v.Z < 0 ? -halfHeight : halfHeight; |
95 | tmp.Y = 0; | 95 | tmp.Y = 0; |
96 | return tmp; | 96 | return tmp; |
97 | } | 97 | } |
98 | } | 98 | } |
99 | } | 99 | } |
100 | } | 100 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs index b094c39..ef6f134 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs | |||
@@ -1,80 +1,80 @@ | |||
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 | using System.Diagnostics; | 26 | using System.Diagnostics; |
27 | 27 | ||
28 | namespace XnaDevRu.BulletX | 28 | namespace XnaDevRu.BulletX |
29 | { | 29 | { |
30 | public class EmptyShape : ConcaveShape | 30 | public class EmptyShape : ConcaveShape |
31 | { | 31 | { |
32 | private Vector3 _localScaling; | 32 | private Vector3 _localScaling; |
33 | 33 | ||
34 | public override string Name | 34 | public override string Name |
35 | { | 35 | { |
36 | get | 36 | get |
37 | { | 37 | { |
38 | return "Empty"; | 38 | return "Empty"; |
39 | } | 39 | } |
40 | } | 40 | } |
41 | 41 | ||
42 | public override BroadphaseNativeTypes ShapeType | 42 | public override BroadphaseNativeTypes ShapeType |
43 | { | 43 | { |
44 | get | 44 | get |
45 | { | 45 | { |
46 | return BroadphaseNativeTypes.Empty; | 46 | return BroadphaseNativeTypes.Empty; |
47 | } | 47 | } |
48 | } | 48 | } |
49 | 49 | ||
50 | public override Vector3 LocalScaling | 50 | public override Vector3 LocalScaling |
51 | { | 51 | { |
52 | get | 52 | get |
53 | { | 53 | { |
54 | return _localScaling; | 54 | return _localScaling; |
55 | } | 55 | } |
56 | set | 56 | set |
57 | { | 57 | { |
58 | _localScaling = value; | 58 | _localScaling = value; |
59 | } | 59 | } |
60 | } | 60 | } |
61 | 61 | ||
62 | public override void ProcessAllTriangles(ITriangleCallback callback, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax) | 62 | public override void ProcessAllTriangles(ITriangleCallback callback, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax) |
63 | { | 63 | { |
64 | throw new Exception("The method or operation is not implemented."); | 64 | throw new Exception("The method or operation is not implemented."); |
65 | } | 65 | } |
66 | 66 | ||
67 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | 67 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) |
68 | { | 68 | { |
69 | Vector3 margin = new Vector3(Margin, Margin, Margin); | 69 | Vector3 margin = new Vector3(Margin, Margin, Margin); |
70 | aabbMin = t.Translation - margin; | 70 | aabbMin = t.Translation - margin; |
71 | aabbMax = t.Translation + margin; | 71 | aabbMax = t.Translation + margin; |
72 | } | 72 | } |
73 | 73 | ||
74 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | 74 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) |
75 | { | 75 | { |
76 | inertia = new Vector3(); | 76 | inertia = new Vector3(); |
77 | BulletDebug.Assert(false); | 77 | BulletDebug.Assert(false); |
78 | } | 78 | } |
79 | } | 79 | } |
80 | } | 80 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs index 5073096..dffd865 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs | |||
@@ -1,55 +1,55 @@ | |||
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 | public class FilteredCallback : ITriangleIndexCallback | 29 | public class FilteredCallback : ITriangleIndexCallback |
30 | { | 30 | { |
31 | private ITriangleCallback _callback; | 31 | private ITriangleCallback _callback; |
32 | private Vector3 _aabbMin; | 32 | private Vector3 _aabbMin; |
33 | private Vector3 _aabbMax; | 33 | private Vector3 _aabbMax; |
34 | 34 | ||
35 | public FilteredCallback(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) | 35 | public FilteredCallback(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) |
36 | { | 36 | { |
37 | _callback = callback; | 37 | _callback = callback; |
38 | _aabbMin = aabbMin; | 38 | _aabbMin = aabbMin; |
39 | _aabbMax = aabbMax; | 39 | _aabbMax = aabbMax; |
40 | } | 40 | } |
41 | 41 | ||
42 | public ITriangleCallback TriangleCallback { get { return _callback; } set { _callback = value; } } | 42 | public ITriangleCallback TriangleCallback { get { return _callback; } set { _callback = value; } } |
43 | public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } } | 43 | public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } } |
44 | public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } } | 44 | public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } } |
45 | 45 | ||
46 | public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) | 46 | public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) |
47 | { | 47 | { |
48 | if (MathHelper.TestTriangleAgainstAabb2(triangle, _aabbMin, _aabbMax)) | 48 | if (MathHelper.TestTriangleAgainstAabb2(triangle, _aabbMin, _aabbMax)) |
49 | { | 49 | { |
50 | //check aabb in triangle-space, before doing this | 50 | //check aabb in triangle-space, before doing this |
51 | _callback.ProcessTriangle(triangle, partId, triangleIndex); | 51 | _callback.ProcessTriangle(triangle, partId, triangleIndex); |
52 | } | 52 | } |
53 | } | 53 | } |
54 | } | 54 | } |
55 | } | 55 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs index b88d804..51c65b7 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs | |||
@@ -1,33 +1,33 @@ | |||
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 | public interface ITriangleIndexCallback | 29 | public interface ITriangleIndexCallback |
30 | { | 30 | { |
31 | void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex); | 31 | void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex); |
32 | } | 32 | } |
33 | } | 33 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs index 951707d..1e5e141 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs | |||
@@ -1,58 +1,58 @@ | |||
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 | public class LocalSupportVertexCallback : ITriangleIndexCallback | 29 | public class LocalSupportVertexCallback : ITriangleIndexCallback |
30 | { | 30 | { |
31 | private Vector3 _supportVertexLocal; | 31 | private Vector3 _supportVertexLocal; |
32 | private float _maxDot; | 32 | private float _maxDot; |
33 | private Vector3 _supportVecLocal; | 33 | private Vector3 _supportVecLocal; |
34 | 34 | ||
35 | public LocalSupportVertexCallback(Vector3 supportVecLocal) | 35 | public LocalSupportVertexCallback(Vector3 supportVecLocal) |
36 | { | 36 | { |
37 | _supportVertexLocal = new Vector3(); | 37 | _supportVertexLocal = new Vector3(); |
38 | _maxDot = -1e30f; | 38 | _maxDot = -1e30f; |
39 | _supportVecLocal = supportVecLocal; | 39 | _supportVecLocal = supportVecLocal; |
40 | } | 40 | } |
41 | 41 | ||
42 | public float MaxDot { get { return _maxDot; } set { _maxDot = value; } } | 42 | public float MaxDot { get { return _maxDot; } set { _maxDot = value; } } |
43 | public Vector3 SupportVertexLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } } | 43 | public Vector3 SupportVertexLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } } |
44 | 44 | ||
45 | public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) | 45 | public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) |
46 | { | 46 | { |
47 | for (int i = 0; i < 3; i++) | 47 | for (int i = 0; i < 3; i++) |
48 | { | 48 | { |
49 | float dot = Vector3.Dot(_supportVecLocal, triangle[i]); | 49 | float dot = Vector3.Dot(_supportVecLocal, triangle[i]); |
50 | if (dot > _maxDot) | 50 | if (dot > _maxDot) |
51 | { | 51 | { |
52 | _maxDot = dot; | 52 | _maxDot = dot; |
53 | _supportVertexLocal = triangle[i]; | 53 | _supportVertexLocal = triangle[i]; |
54 | } | 54 | } |
55 | } | 55 | } |
56 | } | 56 | } |
57 | } | 57 | } |
58 | } | 58 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs index 6f70cae..8cdcfbe 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs | |||
@@ -1,99 +1,99 @@ | |||
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 | /// MinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes. | 30 | /// MinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes. |
31 | /// </summary> | 31 | /// </summary> |
32 | public class MinkowskiSumShape : ConvexShape | 32 | public class MinkowskiSumShape : ConvexShape |
33 | { | 33 | { |
34 | private Matrix _transformA; | 34 | private Matrix _transformA; |
35 | private Matrix _transformB; | 35 | private Matrix _transformB; |
36 | private ConvexShape _shapeA; | 36 | private ConvexShape _shapeA; |
37 | private ConvexShape _shapeB; | 37 | private ConvexShape _shapeB; |
38 | 38 | ||
39 | public MinkowskiSumShape(ConvexShape shapeA, ConvexShape shapeB) | 39 | public MinkowskiSumShape(ConvexShape shapeA, ConvexShape shapeB) |
40 | { | 40 | { |
41 | _shapeA = shapeA; | 41 | _shapeA = shapeA; |
42 | _shapeB = shapeB; | 42 | _shapeB = shapeB; |
43 | _transformA = Matrix.Identity; | 43 | _transformA = Matrix.Identity; |
44 | _transformB = Matrix.Identity; | 44 | _transformB = Matrix.Identity; |
45 | } | 45 | } |
46 | 46 | ||
47 | public Matrix TransformA { get { return _transformA; } set { _transformA = value; } } | 47 | public Matrix TransformA { get { return _transformA; } set { _transformA = value; } } |
48 | public Matrix TransformB { get { return _transformB; } set { _transformB = value; } } | 48 | public Matrix TransformB { get { return _transformB; } set { _transformB = value; } } |
49 | public ConvexShape ShapeA { get { return _shapeA; } } | 49 | public ConvexShape ShapeA { get { return _shapeA; } } |
50 | public ConvexShape ShapeB { get { return _shapeB; } } | 50 | public ConvexShape ShapeB { get { return _shapeB; } } |
51 | 51 | ||
52 | public override float Margin | 52 | public override float Margin |
53 | { | 53 | { |
54 | get | 54 | get |
55 | { | 55 | { |
56 | return _shapeA.Margin + _shapeB.Margin; | 56 | return _shapeA.Margin + _shapeB.Margin; |
57 | } | 57 | } |
58 | set | 58 | set |
59 | { | 59 | { |
60 | base.Margin = value; | 60 | base.Margin = value; |
61 | } | 61 | } |
62 | } | 62 | } |
63 | 63 | ||
64 | public override BroadphaseNativeTypes ShapeType | 64 | public override BroadphaseNativeTypes ShapeType |
65 | { | 65 | { |
66 | get | 66 | get |
67 | { | 67 | { |
68 | return BroadphaseNativeTypes.MinkowskiDifference; | 68 | return BroadphaseNativeTypes.MinkowskiDifference; |
69 | } | 69 | } |
70 | } | 70 | } |
71 | 71 | ||
72 | public override string Name | 72 | public override string Name |
73 | { | 73 | { |
74 | get | 74 | get |
75 | { | 75 | { |
76 | return "MinkowskiSum"; | 76 | return "MinkowskiSum"; |
77 | } | 77 | } |
78 | } | 78 | } |
79 | 79 | ||
80 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | 80 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) |
81 | { | 81 | { |
82 | Vector3 supVertexA = MathHelper.MatrixToVector(_transformA, _shapeA.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformA))); | 82 | Vector3 supVertexA = MathHelper.MatrixToVector(_transformA, _shapeA.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformA))); |
83 | Vector3 supVertexB = MathHelper.MatrixToVector(_transformB, _shapeB.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformB))); | 83 | Vector3 supVertexB = MathHelper.MatrixToVector(_transformB, _shapeB.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformB))); |
84 | return supVertexA + supVertexB; | 84 | return supVertexA + supVertexB; |
85 | } | 85 | } |
86 | 86 | ||
87 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | 87 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) |
88 | { | 88 | { |
89 | for (int i = 0; i < vectors.Length; i++) | 89 | for (int i = 0; i < vectors.Length; i++) |
90 | supportVerticesOut[i] = LocalGetSupportingVertexWithoutMargin(vectors[i]); | 90 | supportVerticesOut[i] = LocalGetSupportingVertexWithoutMargin(vectors[i]); |
91 | } | 91 | } |
92 | 92 | ||
93 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | 93 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) |
94 | { | 94 | { |
95 | inertia = new Vector3(); | 95 | inertia = new Vector3(); |
96 | BulletDebug.Assert(false); | 96 | BulletDebug.Assert(false); |
97 | } | 97 | } |
98 | } | 98 | } |
99 | } | 99 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs index 9959c01..73fd094 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs | |||
@@ -1,154 +1,154 @@ | |||
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 | /// MultiSphereShape represents implicit convex hull of a collection of spheres (using getSupportingVertex) | 30 | /// MultiSphereShape represents implicit convex hull of a collection of spheres (using getSupportingVertex) |
31 | /// </summary> | 31 | /// </summary> |
32 | public class MultiSphereShape : ConvexShape | 32 | public class MultiSphereShape : ConvexShape |
33 | { | 33 | { |
34 | private const int _maxNumSpheres = 5; | 34 | private const int _maxNumSpheres = 5; |
35 | private Vector3[] _localPositions = new Vector3[MaxNumSpheres]; | 35 | private Vector3[] _localPositions = new Vector3[MaxNumSpheres]; |
36 | private float[] _radi = new float[MaxNumSpheres]; | 36 | private float[] _radi = new float[MaxNumSpheres]; |
37 | private Vector3 _inertiaHalfExtents; | 37 | private Vector3 _inertiaHalfExtents; |
38 | 38 | ||
39 | private int m_numSpheres; | 39 | private int m_numSpheres; |
40 | 40 | ||
41 | public MultiSphereShape(Vector3 inertiaHalfExtents, Vector3[] positions, float[] radi, int numSpheres) | 41 | public MultiSphereShape(Vector3 inertiaHalfExtents, Vector3[] positions, float[] radi, int numSpheres) |
42 | { | 42 | { |
43 | _inertiaHalfExtents = inertiaHalfExtents; | 43 | _inertiaHalfExtents = inertiaHalfExtents; |
44 | float startMargin = 1e30f; | 44 | float startMargin = 1e30f; |
45 | 45 | ||
46 | m_numSpheres = numSpheres; | 46 | m_numSpheres = numSpheres; |
47 | for (int i = 0; i < m_numSpheres; i++) | 47 | for (int i = 0; i < m_numSpheres; i++) |
48 | { | 48 | { |
49 | _localPositions[i] = positions[i]; | 49 | _localPositions[i] = positions[i]; |
50 | _radi[i] = radi[i]; | 50 | _radi[i] = radi[i]; |
51 | if (radi[i] < startMargin) | 51 | if (radi[i] < startMargin) |
52 | startMargin = radi[i]; | 52 | startMargin = radi[i]; |
53 | } | 53 | } |
54 | Margin = startMargin; | 54 | Margin = startMargin; |
55 | } | 55 | } |
56 | 56 | ||
57 | public static int MaxNumSpheres { get { return _maxNumSpheres; } } | 57 | public static int MaxNumSpheres { get { return _maxNumSpheres; } } |
58 | 58 | ||
59 | public override BroadphaseNativeTypes ShapeType | 59 | public override BroadphaseNativeTypes ShapeType |
60 | { | 60 | { |
61 | get | 61 | get |
62 | { | 62 | { |
63 | return BroadphaseNativeTypes.MultiSphere; | 63 | return BroadphaseNativeTypes.MultiSphere; |
64 | } | 64 | } |
65 | } | 65 | } |
66 | 66 | ||
67 | public override string Name | 67 | public override string Name |
68 | { | 68 | { |
69 | get | 69 | get |
70 | { | 70 | { |
71 | return "MultiSphere"; | 71 | return "MultiSphere"; |
72 | } | 72 | } |
73 | } | 73 | } |
74 | 74 | ||
75 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vecA) | 75 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vecA) |
76 | { | 76 | { |
77 | Vector3 supVec = new Vector3(); | 77 | Vector3 supVec = new Vector3(); |
78 | 78 | ||
79 | float maxDot = -1e30f; | 79 | float maxDot = -1e30f; |
80 | 80 | ||
81 | 81 | ||
82 | Vector3 vec = vecA; | 82 | Vector3 vec = vecA; |
83 | float lenSqr = vec.LengthSquared(); | 83 | float lenSqr = vec.LengthSquared(); |
84 | if (lenSqr < 0.0001f) | 84 | if (lenSqr < 0.0001f) |
85 | { | 85 | { |
86 | vec = new Vector3(1, 0, 0); | 86 | vec = new Vector3(1, 0, 0); |
87 | } | 87 | } |
88 | else | 88 | else |
89 | { | 89 | { |
90 | float rlen = 1f / (float)Math.Sqrt(lenSqr); | 90 | float rlen = 1f / (float)Math.Sqrt(lenSqr); |
91 | vec *= rlen; | 91 | vec *= rlen; |
92 | } | 92 | } |
93 | 93 | ||
94 | Vector3 vtx; | 94 | Vector3 vtx; |
95 | float newDot; | 95 | float newDot; |
96 | 96 | ||
97 | for (int i = 0; i < m_numSpheres; i++) | 97 | for (int i = 0; i < m_numSpheres; i++) |
98 | { | 98 | { |
99 | vtx = _localPositions[i] + vec * LocalScaling * _radi[i] - vec * Margin; | 99 | vtx = _localPositions[i] + vec * LocalScaling * _radi[i] - vec * Margin; |
100 | newDot = Vector3.Dot(vec, vtx); | 100 | newDot = Vector3.Dot(vec, vtx); |
101 | if (newDot > maxDot) | 101 | if (newDot > maxDot) |
102 | { | 102 | { |
103 | maxDot = newDot; | 103 | maxDot = newDot; |
104 | supVec = vtx; | 104 | supVec = vtx; |
105 | } | 105 | } |
106 | } | 106 | } |
107 | 107 | ||
108 | return supVec; | 108 | return supVec; |
109 | } | 109 | } |
110 | 110 | ||
111 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | 111 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) |
112 | { | 112 | { |
113 | for (int j = 0; j < vectors.Length; j++) | 113 | for (int j = 0; j < vectors.Length; j++) |
114 | { | 114 | { |
115 | float maxDot = -1e30f; | 115 | float maxDot = -1e30f; |
116 | Vector3 vtx; | 116 | Vector3 vtx; |
117 | float newDot; | 117 | float newDot; |
118 | 118 | ||
119 | for (int i = 0; i < m_numSpheres; i++) | 119 | for (int i = 0; i < m_numSpheres; i++) |
120 | { | 120 | { |
121 | vtx = _localPositions[i] + vectors[j] * LocalScaling * _radi[i] - vectors[j] * Margin; | 121 | vtx = _localPositions[i] + vectors[j] * LocalScaling * _radi[i] - vectors[j] * Margin; |
122 | newDot = Vector3.Dot(vectors[j], vtx); | 122 | newDot = Vector3.Dot(vectors[j], vtx); |
123 | if (newDot > maxDot) | 123 | if (newDot > maxDot) |
124 | { | 124 | { |
125 | maxDot = newDot; | 125 | maxDot = newDot; |
126 | supportVerticesOut[j] = vtx; | 126 | supportVerticesOut[j] = vtx; |
127 | } | 127 | } |
128 | } | 128 | } |
129 | } | 129 | } |
130 | } | 130 | } |
131 | 131 | ||
132 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | 132 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) |
133 | { | 133 | { |
134 | //as an approximation, take the inertia of the box that bounds the spheres | 134 | //as an approximation, take the inertia of the box that bounds the spheres |
135 | Matrix ident = Matrix.Identity; | 135 | Matrix ident = Matrix.Identity; |
136 | Vector3 halfExtents = _inertiaHalfExtents; | 136 | Vector3 halfExtents = _inertiaHalfExtents; |
137 | 137 | ||
138 | float margin = ConvexDistanceMargin; | 138 | float margin = ConvexDistanceMargin; |
139 | 139 | ||
140 | float lx = 2f * (halfExtents.X + margin); | 140 | float lx = 2f * (halfExtents.X + margin); |
141 | float ly = 2f * (halfExtents.Y + margin); | 141 | float ly = 2f * (halfExtents.Y + margin); |
142 | float lz = 2f * (halfExtents.Z + margin); | 142 | float lz = 2f * (halfExtents.Z + margin); |
143 | float x2 = lx * lx; | 143 | float x2 = lx * lx; |
144 | float y2 = ly * ly; | 144 | float y2 = ly * ly; |
145 | float z2 = lz * lz; | 145 | float z2 = lz * lz; |
146 | float scaledmass = mass * 0.08333333f; | 146 | float scaledmass = mass * 0.08333333f; |
147 | 147 | ||
148 | inertia = new Vector3(); | 148 | inertia = new Vector3(); |
149 | inertia.X = scaledmass * (y2 + z2); | 149 | inertia.X = scaledmass * (y2 + z2); |
150 | inertia.Y = scaledmass * (x2 + z2); | 150 | inertia.Y = scaledmass * (x2 + z2); |
151 | inertia.Z = scaledmass * (x2 + y2); | 151 | inertia.Z = scaledmass * (x2 + y2); |
152 | } | 152 | } |
153 | } | 153 | } |
154 | } | 154 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs index fac344e..324cdbc 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs | |||
@@ -1,32 +1,32 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX | 26 | namespace XnaDevRu.BulletX |
27 | { | 27 | { |
28 | public interface INodeOverlapCallback | 28 | public interface INodeOverlapCallback |
29 | { | 29 | { |
30 | void ProcessNode(OptimizedBvhNode node); | 30 | void ProcessNode(OptimizedBvhNode node); |
31 | } | 31 | } |
32 | } | 32 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs index c22c990..5b7a5e4 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs | |||
@@ -1,293 +1,293 @@ | |||
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 | /// OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future) | 30 | /// OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future) |
31 | /// </summary> | 31 | /// </summary> |
32 | public class OptimizedBvh | 32 | public class OptimizedBvh |
33 | { | 33 | { |
34 | private static int _maxIterations = 0; | 34 | private static int _maxIterations = 0; |
35 | private OptimizedBvhNode _rootNode; | 35 | private OptimizedBvhNode _rootNode; |
36 | 36 | ||
37 | private OptimizedBvhNode[] _contiguousNodes; | 37 | private OptimizedBvhNode[] _contiguousNodes; |
38 | private int _curNodeIndex; | 38 | private int _curNodeIndex; |
39 | 39 | ||
40 | private List<OptimizedBvhNode> _leafNodes = new List<OptimizedBvhNode>(); | 40 | private List<OptimizedBvhNode> _leafNodes = new List<OptimizedBvhNode>(); |
41 | 41 | ||
42 | public OptimizedBvh() { } | 42 | public OptimizedBvh() { } |
43 | 43 | ||
44 | public void Build(StridingMeshInterface triangles) | 44 | public void Build(StridingMeshInterface triangles) |
45 | { | 45 | { |
46 | NodeTriangleCallback callback = new NodeTriangleCallback(_leafNodes); | 46 | NodeTriangleCallback callback = new NodeTriangleCallback(_leafNodes); |
47 | 47 | ||
48 | Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); | 48 | Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); |
49 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | 49 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); |
50 | 50 | ||
51 | triangles.InternalProcessAllTriangles(callback, aabbMin, aabbMax); | 51 | triangles.InternalProcessAllTriangles(callback, aabbMin, aabbMax); |
52 | 52 | ||
53 | //now we have an array of leafnodes in m_leafNodes | 53 | //now we have an array of leafnodes in m_leafNodes |
54 | _contiguousNodes = new OptimizedBvhNode[2 * _leafNodes.Count]; | 54 | _contiguousNodes = new OptimizedBvhNode[2 * _leafNodes.Count]; |
55 | for (int i = 0; i < _contiguousNodes.Length; i++) | 55 | for (int i = 0; i < _contiguousNodes.Length; i++) |
56 | _contiguousNodes[i] = new OptimizedBvhNode(); | 56 | _contiguousNodes[i] = new OptimizedBvhNode(); |
57 | _curNodeIndex = 0; | 57 | _curNodeIndex = 0; |
58 | 58 | ||
59 | _rootNode = BuildTree(_leafNodes, 0, _leafNodes.Count); | 59 | _rootNode = BuildTree(_leafNodes, 0, _leafNodes.Count); |
60 | } | 60 | } |
61 | 61 | ||
62 | public OptimizedBvhNode BuildTree(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex) | 62 | public OptimizedBvhNode BuildTree(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex) |
63 | { | 63 | { |
64 | OptimizedBvhNode internalNode; | 64 | OptimizedBvhNode internalNode; |
65 | 65 | ||
66 | int splitAxis, splitIndex, i; | 66 | int splitAxis, splitIndex, i; |
67 | int numIndices = endIndex - startIndex; | 67 | int numIndices = endIndex - startIndex; |
68 | int curIndex = _curNodeIndex; | 68 | int curIndex = _curNodeIndex; |
69 | 69 | ||
70 | if (numIndices <= 0) | 70 | if (numIndices <= 0) |
71 | throw new BulletException(); | 71 | throw new BulletException(); |
72 | 72 | ||
73 | if (numIndices == 1) | 73 | if (numIndices == 1) |
74 | { | 74 | { |
75 | _contiguousNodes[_curNodeIndex++] = leafNodes[startIndex]; | 75 | _contiguousNodes[_curNodeIndex++] = leafNodes[startIndex]; |
76 | //return new (&m_contiguousNodes[m_curNodeIndex++]) btOptimizedBvhNode(leafNodes[startIndex]); | 76 | //return new (&m_contiguousNodes[m_curNodeIndex++]) btOptimizedBvhNode(leafNodes[startIndex]); |
77 | return leafNodes[startIndex]; | 77 | return leafNodes[startIndex]; |
78 | } | 78 | } |
79 | 79 | ||
80 | //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. | 80 | //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. |
81 | splitAxis = CalculateSplittingAxis(leafNodes, startIndex, endIndex); | 81 | splitAxis = CalculateSplittingAxis(leafNodes, startIndex, endIndex); |
82 | 82 | ||
83 | splitIndex = SortAndCalculateSplittingIndex(leafNodes, startIndex, endIndex, splitAxis); | 83 | splitIndex = SortAndCalculateSplittingIndex(leafNodes, startIndex, endIndex, splitAxis); |
84 | 84 | ||
85 | internalNode = _contiguousNodes[_curNodeIndex++]; | 85 | internalNode = _contiguousNodes[_curNodeIndex++]; |
86 | 86 | ||
87 | internalNode.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f); | 87 | internalNode.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f); |
88 | internalNode.AabbMin = new Vector3(1e30f, 1e30f, 1e30f); | 88 | internalNode.AabbMin = new Vector3(1e30f, 1e30f, 1e30f); |
89 | 89 | ||
90 | for (i = startIndex; i < endIndex; i++) | 90 | for (i = startIndex; i < endIndex; i++) |
91 | { | 91 | { |
92 | internalNode.AabbMax = MathHelper.SetMax(internalNode.AabbMax, leafNodes[i].AabbMax); | 92 | internalNode.AabbMax = MathHelper.SetMax(internalNode.AabbMax, leafNodes[i].AabbMax); |
93 | internalNode.AabbMin = MathHelper.SetMin(internalNode.AabbMin, leafNodes[i].AabbMin); | 93 | internalNode.AabbMin = MathHelper.SetMin(internalNode.AabbMin, leafNodes[i].AabbMin); |
94 | } | 94 | } |
95 | 95 | ||
96 | //internalNode->m_escapeIndex; | 96 | //internalNode->m_escapeIndex; |
97 | internalNode.LeftChild = BuildTree(leafNodes, startIndex, splitIndex); | 97 | internalNode.LeftChild = BuildTree(leafNodes, startIndex, splitIndex); |
98 | internalNode.RightChild = BuildTree(leafNodes, splitIndex, endIndex); | 98 | internalNode.RightChild = BuildTree(leafNodes, splitIndex, endIndex); |
99 | 99 | ||
100 | internalNode.EscapeIndex = _curNodeIndex - curIndex; | 100 | internalNode.EscapeIndex = _curNodeIndex - curIndex; |
101 | return internalNode; | 101 | return internalNode; |
102 | } | 102 | } |
103 | 103 | ||
104 | public int CalculateSplittingAxis(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex) | 104 | public int CalculateSplittingAxis(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex) |
105 | { | 105 | { |
106 | Vector3 means = new Vector3(); | 106 | Vector3 means = new Vector3(); |
107 | Vector3 variance = new Vector3(); | 107 | Vector3 variance = new Vector3(); |
108 | int numIndices = endIndex - startIndex; | 108 | int numIndices = endIndex - startIndex; |
109 | 109 | ||
110 | for (int i = startIndex; i < endIndex; i++) | 110 | for (int i = startIndex; i < endIndex; i++) |
111 | { | 111 | { |
112 | Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); | 112 | Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); |
113 | means += center; | 113 | means += center; |
114 | } | 114 | } |
115 | means *= (1f / (float)numIndices); | 115 | means *= (1f / (float)numIndices); |
116 | 116 | ||
117 | for (int i = startIndex; i < endIndex; i++) | 117 | for (int i = startIndex; i < endIndex; i++) |
118 | { | 118 | { |
119 | Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); | 119 | Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); |
120 | Vector3 diff2 = center - means; | 120 | Vector3 diff2 = center - means; |
121 | diff2 = diff2 * diff2; | 121 | diff2 = diff2 * diff2; |
122 | variance += diff2; | 122 | variance += diff2; |
123 | } | 123 | } |
124 | variance *= (1f / ((float)numIndices - 1)); | 124 | variance *= (1f / ((float)numIndices - 1)); |
125 | 125 | ||
126 | return MathHelper.MaxAxis(variance); | 126 | return MathHelper.MaxAxis(variance); |
127 | } | 127 | } |
128 | 128 | ||
129 | public int SortAndCalculateSplittingIndex(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex, int splitAxis) | 129 | public int SortAndCalculateSplittingIndex(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex, int splitAxis) |
130 | { | 130 | { |
131 | int splitIndex = startIndex; | 131 | int splitIndex = startIndex; |
132 | int numIndices = endIndex - startIndex; | 132 | int numIndices = endIndex - startIndex; |
133 | float splitValue; | 133 | float splitValue; |
134 | 134 | ||
135 | Vector3 means = new Vector3(); | 135 | Vector3 means = new Vector3(); |
136 | for (int i = startIndex; i < endIndex; i++) | 136 | for (int i = startIndex; i < endIndex; i++) |
137 | { | 137 | { |
138 | Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); | 138 | Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); |
139 | means += center; | 139 | means += center; |
140 | } | 140 | } |
141 | means *= (1f / (float)numIndices); | 141 | means *= (1f / (float)numIndices); |
142 | 142 | ||
143 | if (splitAxis == 0) | 143 | if (splitAxis == 0) |
144 | splitValue = means.X; | 144 | splitValue = means.X; |
145 | else if (splitAxis == 1) | 145 | else if (splitAxis == 1) |
146 | splitValue = means.Y; | 146 | splitValue = means.Y; |
147 | else if (splitAxis == 2) | 147 | else if (splitAxis == 2) |
148 | splitValue = means.Z; | 148 | splitValue = means.Z; |
149 | else | 149 | else |
150 | throw new ArgumentException(); | 150 | throw new ArgumentException(); |
151 | 151 | ||
152 | //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'. | 152 | //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'. |
153 | for (int i = startIndex; i < endIndex; i++) | 153 | for (int i = startIndex; i < endIndex; i++) |
154 | { | 154 | { |
155 | Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); | 155 | Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); |
156 | float centerSplit; | 156 | float centerSplit; |
157 | 157 | ||
158 | if (splitAxis == 0) | 158 | if (splitAxis == 0) |
159 | centerSplit = means.X; | 159 | centerSplit = means.X; |
160 | else if (splitAxis == 1) | 160 | else if (splitAxis == 1) |
161 | centerSplit = means.Y; | 161 | centerSplit = means.Y; |
162 | else if (splitAxis == 2) | 162 | else if (splitAxis == 2) |
163 | centerSplit = means.Z; | 163 | centerSplit = means.Z; |
164 | else | 164 | else |
165 | throw new ArgumentException(); | 165 | throw new ArgumentException(); |
166 | 166 | ||
167 | if (centerSplit > splitValue) | 167 | if (centerSplit > splitValue) |
168 | { | 168 | { |
169 | //swap | 169 | //swap |
170 | OptimizedBvhNode tmp = leafNodes[i]; | 170 | OptimizedBvhNode tmp = leafNodes[i]; |
171 | leafNodes[i] = leafNodes[splitIndex]; | 171 | leafNodes[i] = leafNodes[splitIndex]; |
172 | leafNodes[splitIndex] = tmp; | 172 | leafNodes[splitIndex] = tmp; |
173 | splitIndex++; | 173 | splitIndex++; |
174 | } | 174 | } |
175 | } | 175 | } |
176 | if ((splitIndex == startIndex) || (splitIndex == (endIndex - 1))) | 176 | if ((splitIndex == startIndex) || (splitIndex == (endIndex - 1))) |
177 | { | 177 | { |
178 | splitIndex = startIndex + (numIndices >> 1); | 178 | splitIndex = startIndex + (numIndices >> 1); |
179 | } | 179 | } |
180 | return splitIndex; | 180 | return splitIndex; |
181 | } | 181 | } |
182 | 182 | ||
183 | public void WalkTree(OptimizedBvhNode rootNode, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) | 183 | public void WalkTree(OptimizedBvhNode rootNode, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) |
184 | { | 184 | { |
185 | bool isLeafNode, aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax); | 185 | bool isLeafNode, aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax); |
186 | if (aabbOverlap) | 186 | if (aabbOverlap) |
187 | { | 187 | { |
188 | isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null); | 188 | isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null); |
189 | if (isLeafNode) | 189 | if (isLeafNode) |
190 | { | 190 | { |
191 | nodeCallback.ProcessNode(rootNode); | 191 | nodeCallback.ProcessNode(rootNode); |
192 | } | 192 | } |
193 | else | 193 | else |
194 | { | 194 | { |
195 | WalkTree(rootNode.LeftChild, nodeCallback, aabbMin, aabbMax); | 195 | WalkTree(rootNode.LeftChild, nodeCallback, aabbMin, aabbMax); |
196 | WalkTree(rootNode.RightChild, nodeCallback, aabbMin, aabbMax); | 196 | WalkTree(rootNode.RightChild, nodeCallback, aabbMin, aabbMax); |
197 | } | 197 | } |
198 | } | 198 | } |
199 | } | 199 | } |
200 | 200 | ||
201 | public void WalkStacklessTree(OptimizedBvhNode[] rootNodeArray, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) | 201 | public void WalkStacklessTree(OptimizedBvhNode[] rootNodeArray, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) |
202 | { | 202 | { |
203 | int escapeIndex, curIndex = 0; | 203 | int escapeIndex, curIndex = 0; |
204 | int walkIterations = 0; | 204 | int walkIterations = 0; |
205 | bool aabbOverlap, isLeafNode; | 205 | bool aabbOverlap, isLeafNode; |
206 | int rootNodeIndex = 0; | 206 | int rootNodeIndex = 0; |
207 | OptimizedBvhNode rootNode = rootNodeArray[rootNodeIndex]; | 207 | OptimizedBvhNode rootNode = rootNodeArray[rootNodeIndex]; |
208 | 208 | ||
209 | while (curIndex < _curNodeIndex) | 209 | while (curIndex < _curNodeIndex) |
210 | { | 210 | { |
211 | //catch bugs in tree data | 211 | //catch bugs in tree data |
212 | if (walkIterations >= _curNodeIndex) | 212 | if (walkIterations >= _curNodeIndex) |
213 | throw new BulletException(); | 213 | throw new BulletException(); |
214 | 214 | ||
215 | walkIterations++; | 215 | walkIterations++; |
216 | aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax); | 216 | aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax); |
217 | isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null); | 217 | isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null); |
218 | 218 | ||
219 | if (isLeafNode && aabbOverlap) | 219 | if (isLeafNode && aabbOverlap) |
220 | { | 220 | { |
221 | nodeCallback.ProcessNode(rootNode); | 221 | nodeCallback.ProcessNode(rootNode); |
222 | } | 222 | } |
223 | 223 | ||
224 | if (aabbOverlap || isLeafNode) | 224 | if (aabbOverlap || isLeafNode) |
225 | { | 225 | { |
226 | rootNodeIndex++; // this | 226 | rootNodeIndex++; // this |
227 | curIndex++; | 227 | curIndex++; |
228 | if (rootNodeIndex < rootNodeArray.Length) | 228 | if (rootNodeIndex < rootNodeArray.Length) |
229 | rootNode = rootNodeArray[rootNodeIndex]; | 229 | rootNode = rootNodeArray[rootNodeIndex]; |
230 | } | 230 | } |
231 | else | 231 | else |
232 | { | 232 | { |
233 | escapeIndex = rootNode.EscapeIndex; | 233 | escapeIndex = rootNode.EscapeIndex; |
234 | rootNodeIndex += escapeIndex; // and this | 234 | rootNodeIndex += escapeIndex; // and this |
235 | curIndex += escapeIndex; | 235 | curIndex += escapeIndex; |
236 | if (rootNodeIndex < rootNodeArray.Length) | 236 | if (rootNodeIndex < rootNodeArray.Length) |
237 | rootNode = rootNodeArray[rootNodeIndex]; | 237 | rootNode = rootNodeArray[rootNodeIndex]; |
238 | } | 238 | } |
239 | 239 | ||
240 | } | 240 | } |
241 | 241 | ||
242 | if (_maxIterations < walkIterations) | 242 | if (_maxIterations < walkIterations) |
243 | _maxIterations = walkIterations; | 243 | _maxIterations = walkIterations; |
244 | } | 244 | } |
245 | 245 | ||
246 | public void ReportAabbOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) | 246 | public void ReportAabbOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) |
247 | { | 247 | { |
248 | //either choose recursive traversal (walkTree) or stackless (walkStacklessTree) | 248 | //either choose recursive traversal (walkTree) or stackless (walkStacklessTree) |
249 | //walkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax); | 249 | //walkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax); |
250 | //WalkStacklessTree(_rootNode, nodeCallback, aabbMin, aabbMax); | 250 | //WalkStacklessTree(_rootNode, nodeCallback, aabbMin, aabbMax); |
251 | WalkStacklessTree(_contiguousNodes, nodeCallback, aabbMin, aabbMax); | 251 | WalkStacklessTree(_contiguousNodes, nodeCallback, aabbMin, aabbMax); |
252 | } | 252 | } |
253 | 253 | ||
254 | public void ReportSphereOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) { } | 254 | public void ReportSphereOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) { } |
255 | } | 255 | } |
256 | 256 | ||
257 | public class NodeTriangleCallback : ITriangleIndexCallback | 257 | public class NodeTriangleCallback : ITriangleIndexCallback |
258 | { | 258 | { |
259 | private List<OptimizedBvhNode> _triangleNodes; | 259 | private List<OptimizedBvhNode> _triangleNodes; |
260 | 260 | ||
261 | public NodeTriangleCallback(List<OptimizedBvhNode> triangleNodes) | 261 | public NodeTriangleCallback(List<OptimizedBvhNode> triangleNodes) |
262 | { | 262 | { |
263 | _triangleNodes = triangleNodes; | 263 | _triangleNodes = triangleNodes; |
264 | } | 264 | } |
265 | 265 | ||
266 | public List<OptimizedBvhNode> TriangleNodes { get { return _triangleNodes; } set { _triangleNodes = value; } } | 266 | public List<OptimizedBvhNode> TriangleNodes { get { return _triangleNodes; } set { _triangleNodes = value; } } |
267 | 267 | ||
268 | public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) | 268 | public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) |
269 | { | 269 | { |
270 | 270 | ||
271 | OptimizedBvhNode node = new OptimizedBvhNode(); | 271 | OptimizedBvhNode node = new OptimizedBvhNode(); |
272 | node.AabbMin = new Vector3(1e30f, 1e30f, 1e30f); | 272 | node.AabbMin = new Vector3(1e30f, 1e30f, 1e30f); |
273 | node.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f); | 273 | node.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f); |
274 | 274 | ||
275 | node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[0]); | 275 | node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[0]); |
276 | node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[0]); | 276 | node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[0]); |
277 | node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[1]); | 277 | node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[1]); |
278 | node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[1]); | 278 | node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[1]); |
279 | node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[2]); | 279 | node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[2]); |
280 | node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[2]); | 280 | node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[2]); |
281 | 281 | ||
282 | node.EscapeIndex = -1; | 282 | node.EscapeIndex = -1; |
283 | node.LeftChild = null; | 283 | node.LeftChild = null; |
284 | node.RightChild = null; | 284 | node.RightChild = null; |
285 | 285 | ||
286 | //for child nodes | 286 | //for child nodes |
287 | node.SubPart = partId; | 287 | node.SubPart = partId; |
288 | node.TriangleIndex = triangleIndex; | 288 | node.TriangleIndex = triangleIndex; |
289 | 289 | ||
290 | _triangleNodes.Add(node); | 290 | _triangleNodes.Add(node); |
291 | } | 291 | } |
292 | } | 292 | } |
293 | } | 293 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs index d43f2fb..83cc6e5 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs | |||
@@ -1,63 +1,63 @@ | |||
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 | /// OptimizedBvhNode contains both internal and leaf node information. | 30 | /// OptimizedBvhNode contains both internal and leaf node information. |
31 | /// It hasn't been optimized yet for storage. Some obvious optimizations are: | 31 | /// It hasn't been optimized yet for storage. Some obvious optimizations are: |
32 | /// Removal of the pointers (can already be done, they are not used for traversal) | 32 | /// Removal of the pointers (can already be done, they are not used for traversal) |
33 | /// and storing aabbmin/max as quantized integers. | 33 | /// and storing aabbmin/max as quantized integers. |
34 | /// 'subpart' doesn't need an integer either. It allows to re-use graphics triangle | 34 | /// 'subpart' doesn't need an integer either. It allows to re-use graphics triangle |
35 | /// meshes stored in a non-uniform way (like batches/subparts of triangle-fans | 35 | /// meshes stored in a non-uniform way (like batches/subparts of triangle-fans |
36 | /// </summary> | 36 | /// </summary> |
37 | public class OptimizedBvhNode | 37 | public class OptimizedBvhNode |
38 | { | 38 | { |
39 | private Vector3 _aabbMin; | 39 | private Vector3 _aabbMin; |
40 | private Vector3 _aabbMax; | 40 | private Vector3 _aabbMax; |
41 | 41 | ||
42 | //these 2 pointers are obsolete, the stackless traversal just uses the escape index | 42 | //these 2 pointers are obsolete, the stackless traversal just uses the escape index |
43 | private OptimizedBvhNode _leftChild; | 43 | private OptimizedBvhNode _leftChild; |
44 | private OptimizedBvhNode _rightChild; | 44 | private OptimizedBvhNode _rightChild; |
45 | 45 | ||
46 | private int _escapeIndex; | 46 | private int _escapeIndex; |
47 | 47 | ||
48 | //for child nodes | 48 | //for child nodes |
49 | private int _subPart; | 49 | private int _subPart; |
50 | private int _triangleIndex; | 50 | private int _triangleIndex; |
51 | 51 | ||
52 | public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } } | 52 | public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } } |
53 | public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } } | 53 | public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } } |
54 | 54 | ||
55 | public OptimizedBvhNode LeftChild { get { return _leftChild; } set { _leftChild = value; } } | 55 | public OptimizedBvhNode LeftChild { get { return _leftChild; } set { _leftChild = value; } } |
56 | public OptimizedBvhNode RightChild { get { return _rightChild; } set { _rightChild = value; } } | 56 | public OptimizedBvhNode RightChild { get { return _rightChild; } set { _rightChild = value; } } |
57 | 57 | ||
58 | public int EscapeIndex { get { return _escapeIndex; } set { _escapeIndex = value; } } | 58 | public int EscapeIndex { get { return _escapeIndex; } set { _escapeIndex = value; } } |
59 | 59 | ||
60 | public int SubPart { get { return _subPart; } set { _subPart = value; } } | 60 | public int SubPart { get { return _subPart; } set { _subPart = value; } } |
61 | public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } | 61 | public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } |
62 | } | 62 | } |
63 | } | 63 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs index f61371a..1a911b3 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs | |||
@@ -1,133 +1,133 @@ | |||
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 | public abstract class PolyhedralConvexShape : ConvexShape | 29 | public abstract class PolyhedralConvexShape : ConvexShape |
30 | { | 30 | { |
31 | public PolyhedralConvexShape() | 31 | public PolyhedralConvexShape() |
32 | { | 32 | { |
33 | //m_optionalHull = null; | 33 | //m_optionalHull = null; |
34 | } | 34 | } |
35 | 35 | ||
36 | public abstract int VertexCount { get; } | 36 | public abstract int VertexCount { get; } |
37 | public abstract int EdgeCount { get; } | 37 | public abstract int EdgeCount { get; } |
38 | public abstract int PlaneCount { get; } | 38 | public abstract int PlaneCount { get; } |
39 | 39 | ||
40 | public abstract void GetEdge(int i, out Vector3 pointA, out Vector3 pointB); | 40 | public abstract void GetEdge(int i, out Vector3 pointA, out Vector3 pointB); |
41 | public abstract void GetVertex(int i, out Vector3 vertex); | 41 | public abstract void GetVertex(int i, out Vector3 vertex); |
42 | public abstract void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i); | 42 | public abstract void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i); |
43 | // abstract int getIndex(int i); | 43 | // abstract int getIndex(int i); |
44 | 44 | ||
45 | public abstract bool IsInside(Vector3 point, float tolerance); | 45 | public abstract bool IsInside(Vector3 point, float tolerance); |
46 | 46 | ||
47 | // optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp | 47 | // optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp |
48 | //public class Hull m_optionalHull; | 48 | //public class Hull m_optionalHull; |
49 | 49 | ||
50 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | 50 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) |
51 | { | 51 | { |
52 | Vector3 supVec = new Vector3(); | 52 | Vector3 supVec = new Vector3(); |
53 | 53 | ||
54 | float maxDot = -1e30f; | 54 | float maxDot = -1e30f; |
55 | 55 | ||
56 | float lenSqr = vec.LengthSquared(); | 56 | float lenSqr = vec.LengthSquared(); |
57 | if (lenSqr < 0.0001f) | 57 | if (lenSqr < 0.0001f) |
58 | { | 58 | { |
59 | vec = new Vector3(1, 0, 0); | 59 | vec = new Vector3(1, 0, 0); |
60 | } | 60 | } |
61 | else | 61 | else |
62 | { | 62 | { |
63 | float rlen = 1f / (float)Math.Sqrt(lenSqr); | 63 | float rlen = 1f / (float)Math.Sqrt(lenSqr); |
64 | vec *= rlen; | 64 | vec *= rlen; |
65 | } | 65 | } |
66 | 66 | ||
67 | Vector3 vtx; | 67 | Vector3 vtx; |
68 | float newDot; | 68 | float newDot; |
69 | 69 | ||
70 | for (int i = 0; i < VertexCount; i++) | 70 | for (int i = 0; i < VertexCount; i++) |
71 | { | 71 | { |
72 | GetVertex(i, out vtx); | 72 | GetVertex(i, out vtx); |
73 | newDot = Vector3.Dot(vec, vtx); | 73 | newDot = Vector3.Dot(vec, vtx); |
74 | if (newDot > maxDot) | 74 | if (newDot > maxDot) |
75 | { | 75 | { |
76 | maxDot = newDot; | 76 | maxDot = newDot; |
77 | supVec = vtx; | 77 | supVec = vtx; |
78 | } | 78 | } |
79 | } | 79 | } |
80 | return supVec; | 80 | return supVec; |
81 | } | 81 | } |
82 | 82 | ||
83 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | 83 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) |
84 | { | 84 | { |
85 | #warning Think about this | 85 | #warning Think about this |
86 | /*Vector3 vtx; | 86 | /*Vector3 vtx; |
87 | float newDot; | 87 | float newDot; |
88 | 88 | ||
89 | for (int i = 0; i < vectors.Length; i++) | 89 | for (int i = 0; i < vectors.Length; i++) |
90 | { | 90 | { |
91 | supportVerticesOut[i][3] = -1e30f; | 91 | supportVerticesOut[i][3] = -1e30f; |
92 | } | 92 | } |
93 | 93 | ||
94 | for (int j = 0; j < vectors.Length; j++) | 94 | for (int j = 0; j < vectors.Length; j++) |
95 | { | 95 | { |
96 | Vector3 vec = vectors[j]; | 96 | Vector3 vec = vectors[j]; |
97 | 97 | ||
98 | for (int i = 0; i < getNumVertices(); i++) | 98 | for (int i = 0; i < getNumVertices(); i++) |
99 | { | 99 | { |
100 | getVertex(i, out vtx); | 100 | getVertex(i, out vtx); |
101 | newDot = Vector3.Dot(vec,vtx); | 101 | newDot = Vector3.Dot(vec,vtx); |
102 | if (newDot > supportVerticesOut[j][3]) | 102 | if (newDot > supportVerticesOut[j][3]) |
103 | { | 103 | { |
104 | //WARNING: don't swap next lines, the w component would get overwritten! | 104 | //WARNING: don't swap next lines, the w component would get overwritten! |
105 | supportVerticesOut[j] = vtx; | 105 | supportVerticesOut[j] = vtx; |
106 | supportVerticesOut[j][3] = newDot; | 106 | supportVerticesOut[j][3] = newDot; |
107 | } | 107 | } |
108 | } | 108 | } |
109 | }*/ | 109 | }*/ |
110 | } | 110 | } |
111 | 111 | ||
112 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | 112 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) |
113 | { | 113 | { |
114 | //not yet, return box inertia | 114 | //not yet, return box inertia |
115 | float margin = Margin; | 115 | float margin = Margin; |
116 | 116 | ||
117 | Matrix ident = Matrix.Identity; | 117 | Matrix ident = Matrix.Identity; |
118 | Vector3 aabbMin, aabbMax; | 118 | Vector3 aabbMin, aabbMax; |
119 | GetAabb(ident, out aabbMin, out aabbMax); | 119 | GetAabb(ident, out aabbMin, out aabbMax); |
120 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; | 120 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; |
121 | 121 | ||
122 | float lx = 2f * (halfExtents.X + margin); | 122 | float lx = 2f * (halfExtents.X + margin); |
123 | float ly = 2f * (halfExtents.Y + margin); | 123 | float ly = 2f * (halfExtents.Y + margin); |
124 | float lz = 2f * (halfExtents.Z + margin); | 124 | float lz = 2f * (halfExtents.Z + margin); |
125 | float x2 = lx * lx; | 125 | float x2 = lx * lx; |
126 | float y2 = ly * ly; | 126 | float y2 = ly * ly; |
127 | float z2 = lz * lz; | 127 | float z2 = lz * lz; |
128 | float scaledmass = mass * 0.08333333f; | 128 | float scaledmass = mass * 0.08333333f; |
129 | 129 | ||
130 | inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2)); | 130 | inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2)); |
131 | } | 131 | } |
132 | } | 132 | } |
133 | } | 133 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs index c660a6f..cc3cea9 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs | |||
@@ -1,116 +1,116 @@ | |||
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 | /// btSphereShape implements an implicit (getSupportingVertex) Sphere | 30 | /// btSphereShape implements an implicit (getSupportingVertex) Sphere |
31 | /// </summary> | 31 | /// </summary> |
32 | public class SphereShape : ConvexShape | 32 | public class SphereShape : ConvexShape |
33 | { | 33 | { |
34 | public SphereShape(float radius) | 34 | public SphereShape(float radius) |
35 | : base() | 35 | : base() |
36 | { | 36 | { |
37 | Vector3 temp = ImplicitShapeDimensions; | 37 | Vector3 temp = ImplicitShapeDimensions; |
38 | temp.X = radius; | 38 | temp.X = radius; |
39 | ImplicitShapeDimensions = temp; | 39 | ImplicitShapeDimensions = temp; |
40 | } | 40 | } |
41 | 41 | ||
42 | public float Radius { get { return ImplicitShapeDimensions.X; } } | 42 | public float Radius { get { return ImplicitShapeDimensions.X; } } |
43 | 43 | ||
44 | public override BroadphaseNativeTypes ShapeType | 44 | public override BroadphaseNativeTypes ShapeType |
45 | { | 45 | { |
46 | get | 46 | get |
47 | { | 47 | { |
48 | return BroadphaseNativeTypes.Sphere; | 48 | return BroadphaseNativeTypes.Sphere; |
49 | } | 49 | } |
50 | } | 50 | } |
51 | 51 | ||
52 | public override string Name | 52 | public override string Name |
53 | { | 53 | { |
54 | get | 54 | get |
55 | { | 55 | { |
56 | return "Sphere"; | 56 | return "Sphere"; |
57 | } | 57 | } |
58 | } | 58 | } |
59 | 59 | ||
60 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | 60 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) |
61 | { | 61 | { |
62 | return new Vector3(); | 62 | return new Vector3(); |
63 | } | 63 | } |
64 | 64 | ||
65 | /// <summary> | 65 | /// <summary> |
66 | /// to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case | 66 | /// to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case |
67 | /// this means, non-uniform scaling is not supported anymore | 67 | /// this means, non-uniform scaling is not supported anymore |
68 | /// </summary> | 68 | /// </summary> |
69 | public override float Margin | 69 | public override float Margin |
70 | { | 70 | { |
71 | get | 71 | get |
72 | { | 72 | { |
73 | return LocalScaling.X * Radius + base.Margin; | 73 | return LocalScaling.X * Radius + base.Margin; |
74 | } | 74 | } |
75 | set | 75 | set |
76 | { | 76 | { |
77 | base.Margin = value; | 77 | base.Margin = value; |
78 | } | 78 | } |
79 | } | 79 | } |
80 | 80 | ||
81 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | 81 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) |
82 | { | 82 | { |
83 | if (supportVerticesOut != null) | 83 | if (supportVerticesOut != null) |
84 | for (int i = 0; i < supportVerticesOut.Length; i++) | 84 | for (int i = 0; i < supportVerticesOut.Length; i++) |
85 | supportVerticesOut[i] = new Vector3(); | 85 | supportVerticesOut[i] = new Vector3(); |
86 | } | 86 | } |
87 | 87 | ||
88 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | 88 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) |
89 | { | 89 | { |
90 | float elem = 0.4f * mass * Margin * Margin; | 90 | float elem = 0.4f * mass * Margin * Margin; |
91 | inertia = new Vector3(elem, elem, elem); | 91 | inertia = new Vector3(elem, elem, elem); |
92 | } | 92 | } |
93 | 93 | ||
94 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | 94 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) |
95 | { | 95 | { |
96 | Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); | 96 | Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); |
97 | 97 | ||
98 | Vector3 vecnorm = vec; | 98 | Vector3 vecnorm = vec; |
99 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | 99 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) |
100 | { | 100 | { |
101 | vecnorm = new Vector3(-1f, -1f, -1f); | 101 | vecnorm = new Vector3(-1f, -1f, -1f); |
102 | } | 102 | } |
103 | vecnorm.Normalize(); | 103 | vecnorm.Normalize(); |
104 | supVertex += Margin * vecnorm; | 104 | supVertex += Margin * vecnorm; |
105 | return supVertex; | 105 | return supVertex; |
106 | } | 106 | } |
107 | 107 | ||
108 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | 108 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) |
109 | { | 109 | { |
110 | Vector3 center = t.Translation; | 110 | Vector3 center = t.Translation; |
111 | Vector3 extent = new Vector3(Margin, Margin, Margin); | 111 | Vector3 extent = new Vector3(Margin, Margin, Margin); |
112 | aabbMin = center - extent; | 112 | aabbMin = center - extent; |
113 | aabbMax = center + extent; | 113 | aabbMax = center + extent; |
114 | } | 114 | } |
115 | } | 115 | } |
116 | } | 116 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs index 49604a0..a79a145 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs | |||
@@ -1,124 +1,124 @@ | |||
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 | public class StaticPlaneShape : ConcaveShape | 29 | public class StaticPlaneShape : ConcaveShape |
30 | { | 30 | { |
31 | private Vector3 _localAabbMin; | 31 | private Vector3 _localAabbMin; |
32 | private Vector3 _localAabbMax; | 32 | private Vector3 _localAabbMax; |
33 | 33 | ||
34 | private Vector3 _planeNormal; | 34 | private Vector3 _planeNormal; |
35 | private float _planeConstant; | 35 | private float _planeConstant; |
36 | private Vector3 _localScaling; | 36 | private Vector3 _localScaling; |
37 | 37 | ||
38 | public StaticPlaneShape(Vector3 planeNormal, float planeConstant) | 38 | public StaticPlaneShape(Vector3 planeNormal, float planeConstant) |
39 | { | 39 | { |
40 | _planeNormal = planeNormal; | 40 | _planeNormal = planeNormal; |
41 | _planeConstant = planeConstant; | 41 | _planeConstant = planeConstant; |
42 | _localScaling = new Vector3(); | 42 | _localScaling = new Vector3(); |
43 | } | 43 | } |
44 | 44 | ||
45 | protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } } | 45 | protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } } |
46 | protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } } | 46 | protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } } |
47 | 47 | ||
48 | protected Vector3 PlaneNormal { get { return _planeNormal; } set { _planeNormal = value; } } | 48 | protected Vector3 PlaneNormal { get { return _planeNormal; } set { _planeNormal = value; } } |
49 | protected float PlaneConstant { get { return _planeConstant; } set { _planeConstant = value; } } | 49 | protected float PlaneConstant { get { return _planeConstant; } set { _planeConstant = value; } } |
50 | 50 | ||
51 | public override BroadphaseNativeTypes ShapeType | 51 | public override BroadphaseNativeTypes ShapeType |
52 | { | 52 | { |
53 | get | 53 | get |
54 | { | 54 | { |
55 | return BroadphaseNativeTypes.StaticPlane; | 55 | return BroadphaseNativeTypes.StaticPlane; |
56 | } | 56 | } |
57 | } | 57 | } |
58 | 58 | ||
59 | public override Vector3 LocalScaling | 59 | public override Vector3 LocalScaling |
60 | { | 60 | { |
61 | get | 61 | get |
62 | { | 62 | { |
63 | return _localScaling; | 63 | return _localScaling; |
64 | } | 64 | } |
65 | set | 65 | set |
66 | { | 66 | { |
67 | _localScaling = value; | 67 | _localScaling = value; |
68 | } | 68 | } |
69 | } | 69 | } |
70 | 70 | ||
71 | public override string Name | 71 | public override string Name |
72 | { | 72 | { |
73 | get | 73 | get |
74 | { | 74 | { |
75 | return "StaticPlane"; | 75 | return "StaticPlane"; |
76 | } | 76 | } |
77 | } | 77 | } |
78 | 78 | ||
79 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | 79 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) |
80 | { | 80 | { |
81 | Vector3 infvec = new Vector3(1e30f, 1e30f, 1e30f); | 81 | Vector3 infvec = new Vector3(1e30f, 1e30f, 1e30f); |
82 | 82 | ||
83 | Vector3 center = _planeNormal * _planeConstant; | 83 | Vector3 center = _planeNormal * _planeConstant; |
84 | aabbMin = center + infvec * _planeNormal; | 84 | aabbMin = center + infvec * _planeNormal; |
85 | aabbMax = aabbMin; | 85 | aabbMax = aabbMin; |
86 | MathHelper.SetMin(ref aabbMin, center - infvec * _planeNormal); | 86 | MathHelper.SetMin(ref aabbMin, center - infvec * _planeNormal); |
87 | MathHelper.SetMax(ref aabbMax, center - infvec * _planeNormal); | 87 | MathHelper.SetMax(ref aabbMax, center - infvec * _planeNormal); |
88 | 88 | ||
89 | aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); | 89 | aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); |
90 | aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | 90 | aabbMax = new Vector3(1e30f, 1e30f, 1e30f); |
91 | } | 91 | } |
92 | 92 | ||
93 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | 93 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) |
94 | { | 94 | { |
95 | //moving concave objects not supported | 95 | //moving concave objects not supported |
96 | inertia = new Vector3(); | 96 | inertia = new Vector3(); |
97 | } | 97 | } |
98 | 98 | ||
99 | public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) { | 99 | public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) { |
100 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; | 100 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; |
101 | float radius = halfExtents.Length(); | 101 | float radius = halfExtents.Length(); |
102 | Vector3 center = (aabbMax + aabbMin) * 0.5f; | 102 | Vector3 center = (aabbMax + aabbMin) * 0.5f; |
103 | 103 | ||
104 | //this is where the triangles are generated, given AABB and plane equation (normal/constant) | 104 | //this is where the triangles are generated, given AABB and plane equation (normal/constant) |
105 | Vector3 tangentDir0 = new Vector3(), tangentDir1 = new Vector3(); | 105 | Vector3 tangentDir0 = new Vector3(), tangentDir1 = new Vector3(); |
106 | 106 | ||
107 | //tangentDir0/tangentDir1 can be precalculated | 107 | //tangentDir0/tangentDir1 can be precalculated |
108 | MathHelper.PlaneSpace1(_planeNormal, ref tangentDir0, ref tangentDir1); | 108 | MathHelper.PlaneSpace1(_planeNormal, ref tangentDir0, ref tangentDir1); |
109 | 109 | ||
110 | Vector3 projectedCenter = center - (Vector3.Dot(_planeNormal, center) - _planeConstant) * _planeNormal; | 110 | Vector3 projectedCenter = center - (Vector3.Dot(_planeNormal, center) - _planeConstant) * _planeNormal; |
111 | 111 | ||
112 | Vector3[] triangle = new Vector3[3]; | 112 | Vector3[] triangle = new Vector3[3]; |
113 | triangle[0] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; | 113 | triangle[0] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; |
114 | triangle[1] = projectedCenter + tangentDir0 * radius - tangentDir1 * radius; | 114 | triangle[1] = projectedCenter + tangentDir0 * radius - tangentDir1 * radius; |
115 | triangle[2] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; | 115 | triangle[2] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; |
116 | callback.ProcessTriangle(triangle, 0, 0); | 116 | callback.ProcessTriangle(triangle, 0, 0); |
117 | 117 | ||
118 | triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; | 118 | triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; |
119 | triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius; | 119 | triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius; |
120 | triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; | 120 | triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; |
121 | callback.ProcessTriangle(triangle, 0, 1); | 121 | callback.ProcessTriangle(triangle, 0, 1); |
122 | } | 122 | } |
123 | } | 123 | } |
124 | } | 124 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs index 2a2ce27..20baccb 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs | |||
@@ -1,115 +1,115 @@ | |||
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 | /// PHY_ScalarType enumerates possible scalar types. | 30 | /// PHY_ScalarType enumerates possible scalar types. |
31 | /// See the StridingMeshInterface for its use | 31 | /// See the StridingMeshInterface for its use |
32 | /// </summary> | 32 | /// </summary> |
33 | public enum PHY_ScalarType | 33 | public enum PHY_ScalarType |
34 | { | 34 | { |
35 | PHY_FLOAT, | 35 | PHY_FLOAT, |
36 | PHY_DOUBLE, | 36 | PHY_DOUBLE, |
37 | PHY_INTEGER, | 37 | PHY_INTEGER, |
38 | PHY_SHORT, | 38 | PHY_SHORT, |
39 | PHY_FIXEDPOINT88 | 39 | PHY_FIXEDPOINT88 |
40 | } | 40 | } |
41 | 41 | ||
42 | /// <summary> | 42 | /// <summary> |
43 | /// StridingMeshInterface is the interface class for high performance access to triangle meshes | 43 | /// StridingMeshInterface is the interface class for high performance access to triangle meshes |
44 | /// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory. | 44 | /// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory. |
45 | /// </summary> | 45 | /// </summary> |
46 | public abstract class StridingMeshInterface | 46 | public abstract class StridingMeshInterface |
47 | { | 47 | { |
48 | protected Vector3 _scaling; | 48 | protected Vector3 _scaling; |
49 | 49 | ||
50 | public StridingMeshInterface() | 50 | public StridingMeshInterface() |
51 | { | 51 | { |
52 | _scaling = new Vector3(1f,1f,1f); | 52 | _scaling = new Vector3(1f,1f,1f); |
53 | } | 53 | } |
54 | 54 | ||
55 | public void InternalProcessAllTriangles(ITriangleIndexCallback callback, Vector3 aabbMin, Vector3 aabbMax) | 55 | public void InternalProcessAllTriangles(ITriangleIndexCallback callback, Vector3 aabbMin, Vector3 aabbMax) |
56 | { | 56 | { |
57 | int numtotalphysicsverts = 0; | 57 | int numtotalphysicsverts = 0; |
58 | int numtriangles, gfxindex; | 58 | int numtriangles, gfxindex; |
59 | int part, graphicssubparts = SubPartsCount(); | 59 | int part, graphicssubparts = SubPartsCount(); |
60 | Vector3[] triangle = new Vector3[3]; | 60 | Vector3[] triangle = new Vector3[3]; |
61 | List<Vector3> verts; | 61 | List<Vector3> verts; |
62 | List<int> indicies; | 62 | List<int> indicies; |
63 | 63 | ||
64 | Vector3 meshScaling = Scaling; | 64 | Vector3 meshScaling = Scaling; |
65 | 65 | ||
66 | //if the number of parts is big, the performance might drop due to the innerloop switch on indextype | 66 | //if the number of parts is big, the performance might drop due to the innerloop switch on indextype |
67 | for (part = 0; part < graphicssubparts; part++) | 67 | for (part = 0; part < graphicssubparts; part++) |
68 | { | 68 | { |
69 | GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, part); | 69 | GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, part); |
70 | numtotalphysicsverts += numtriangles * 3; //upper bound | 70 | numtotalphysicsverts += numtriangles * 3; //upper bound |
71 | 71 | ||
72 | for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) | 72 | for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) |
73 | { | 73 | { |
74 | triangle[0] = verts[indicies[gfxindex * 3 + 0]]; | 74 | triangle[0] = verts[indicies[gfxindex * 3 + 0]]; |
75 | triangle[1] = verts[indicies[gfxindex * 3 + 1]]; | 75 | triangle[1] = verts[indicies[gfxindex * 3 + 1]]; |
76 | triangle[2] = verts[indicies[gfxindex * 3 + 2]]; | 76 | triangle[2] = verts[indicies[gfxindex * 3 + 2]]; |
77 | 77 | ||
78 | callback.ProcessTriangleIndex(triangle, part, gfxindex); | 78 | callback.ProcessTriangleIndex(triangle, part, gfxindex); |
79 | } | 79 | } |
80 | 80 | ||
81 | UnLockReadOnlyVertexBase(part); | 81 | UnLockReadOnlyVertexBase(part); |
82 | } | 82 | } |
83 | } | 83 | } |
84 | 84 | ||
85 | 85 | ||
86 | // get read and write access to a subpart of a triangle mesh | 86 | // get read and write access to a subpart of a triangle mesh |
87 | // this subpart has a continuous array of vertices and indices | 87 | // this subpart has a continuous array of vertices and indices |
88 | // in this way the mesh can be handled as chunks of memory with striding | 88 | // in this way the mesh can be handled as chunks of memory with striding |
89 | // very similar to OpenGL vertexarray support | 89 | // very similar to OpenGL vertexarray support |
90 | // make a call to unLockVertexBase when the read and write access is finished | 90 | // make a call to unLockVertexBase when the read and write access is finished |
91 | public abstract void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart); | 91 | public abstract void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart); |
92 | 92 | ||
93 | public abstract void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart); | 93 | public abstract void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart); |
94 | 94 | ||
95 | // unLockVertexBase finishes the access to a subpart of the triangle mesh | 95 | // unLockVertexBase finishes the access to a subpart of the triangle mesh |
96 | // make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished | 96 | // make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished |
97 | public abstract void UnLockVertexBase(int subpart); | 97 | public abstract void UnLockVertexBase(int subpart); |
98 | 98 | ||
99 | public abstract void UnLockReadOnlyVertexBase(int subpart); | 99 | public abstract void UnLockReadOnlyVertexBase(int subpart); |
100 | 100 | ||
101 | 101 | ||
102 | // getNumSubParts returns the number of seperate subparts | 102 | // getNumSubParts returns the number of seperate subparts |
103 | // each subpart has a continuous array of vertices and indices | 103 | // each subpart has a continuous array of vertices and indices |
104 | public abstract int SubPartsCount(); | 104 | public abstract int SubPartsCount(); |
105 | 105 | ||
106 | public abstract void PreallocateVertices(int numverts); | 106 | public abstract void PreallocateVertices(int numverts); |
107 | public abstract void PreallocateIndices(int numindices); | 107 | public abstract void PreallocateIndices(int numindices); |
108 | 108 | ||
109 | public Vector3 Scaling | 109 | public Vector3 Scaling |
110 | { | 110 | { |
111 | get { return _scaling; } | 111 | get { return _scaling; } |
112 | set { _scaling = value; } | 112 | set { _scaling = value; } |
113 | } | 113 | } |
114 | } | 114 | } |
115 | } | 115 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs index ffb5813..eab822e 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs | |||
@@ -1,67 +1,67 @@ | |||
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 | internal class SupportVertexCallback : ITriangleCallback | 29 | internal class SupportVertexCallback : ITriangleCallback |
30 | { | 30 | { |
31 | private Vector3 _supportVertexLocal; | 31 | private Vector3 _supportVertexLocal; |
32 | 32 | ||
33 | private Matrix _worldTransform; | 33 | private Matrix _worldTransform; |
34 | private float _maxDot; | 34 | private float _maxDot; |
35 | private Vector3 _supportVecLocal; | 35 | private Vector3 _supportVecLocal; |
36 | 36 | ||
37 | public SupportVertexCallback(Vector3 supportVecWorld, Matrix trans) | 37 | public SupportVertexCallback(Vector3 supportVecWorld, Matrix trans) |
38 | { | 38 | { |
39 | _supportVertexLocal = new Vector3(); | 39 | _supportVertexLocal = new Vector3(); |
40 | _worldTransform = trans; | 40 | _worldTransform = trans; |
41 | _maxDot = -1e30f; | 41 | _maxDot = -1e30f; |
42 | _supportVecLocal = Vector3.TransformNormal(supportVecWorld, _worldTransform); | 42 | _supportVecLocal = Vector3.TransformNormal(supportVecWorld, _worldTransform); |
43 | } | 43 | } |
44 | 44 | ||
45 | public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } } | 45 | public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } } |
46 | public float MaxDot { get { return _maxDot; } set { _maxDot = value; } } | 46 | public float MaxDot { get { return _maxDot; } set { _maxDot = value; } } |
47 | public Vector3 SupportVectorLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } } | 47 | public Vector3 SupportVectorLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } } |
48 | 48 | ||
49 | public Vector3 SupportVertexLocal { get { return _supportVertexLocal; } } | 49 | public Vector3 SupportVertexLocal { get { return _supportVertexLocal; } } |
50 | public Vector3 SupportVertexWorldSpace { get { return MathHelper.MatrixToVector(_worldTransform, _supportVertexLocal); } } | 50 | public Vector3 SupportVertexWorldSpace { get { return MathHelper.MatrixToVector(_worldTransform, _supportVertexLocal); } } |
51 | 51 | ||
52 | #region ITriangleCallback Members | 52 | #region ITriangleCallback Members |
53 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) | 53 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) |
54 | { | 54 | { |
55 | for (int i = 0; i < 3; i++) | 55 | for (int i = 0; i < 3; i++) |
56 | { | 56 | { |
57 | float dot = Vector3.Dot(_supportVecLocal, triangle[i]); | 57 | float dot = Vector3.Dot(_supportVecLocal, triangle[i]); |
58 | if (dot > _maxDot) | 58 | if (dot > _maxDot) |
59 | { | 59 | { |
60 | _maxDot = dot; | 60 | _maxDot = dot; |
61 | _supportVertexLocal = triangle[i]; | 61 | _supportVertexLocal = triangle[i]; |
62 | } | 62 | } |
63 | } | 63 | } |
64 | } | 64 | } |
65 | #endregion | 65 | #endregion |
66 | } | 66 | } |
67 | } | 67 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs index 158e42a..a472583 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs | |||
@@ -1,80 +1,80 @@ | |||
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 | public class Triangle | 29 | public class Triangle |
30 | { | 30 | { |
31 | private Vector3 _vertexA; | 31 | private Vector3 _vertexA; |
32 | private Vector3 _vertexB; | 32 | private Vector3 _vertexB; |
33 | private Vector3 _vertexC; | 33 | private Vector3 _vertexC; |
34 | private int _partId; | 34 | private int _partId; |
35 | private int _triangleIndex; | 35 | private int _triangleIndex; |
36 | 36 | ||
37 | public Vector3 VertexA { get { return _vertexA; } set { _vertexA = value; } } | 37 | public Vector3 VertexA { get { return _vertexA; } set { _vertexA = value; } } |
38 | public Vector3 VertexB { get { return _vertexB; } set { _vertexB = value; } } | 38 | public Vector3 VertexB { get { return _vertexB; } set { _vertexB = value; } } |
39 | public Vector3 VertexC { get { return _vertexC; } set { _vertexC = value; } } | 39 | public Vector3 VertexC { get { return _vertexC; } set { _vertexC = value; } } |
40 | public int PartId { get { return _partId; } set { _partId = value; } } | 40 | public int PartId { get { return _partId; } set { _partId = value; } } |
41 | public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } | 41 | public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } |
42 | } | 42 | } |
43 | 43 | ||
44 | /// <summary> | 44 | /// <summary> |
45 | /// example usage of this class: | 45 | /// example usage of this class: |
46 | /// TriangleBuffer triBuf; | 46 | /// TriangleBuffer triBuf; |
47 | /// concaveShape.processAllTriangles(triBuf, out aabbMin, out aabbMax); | 47 | /// concaveShape.processAllTriangles(triBuf, out aabbMin, out aabbMax); |
48 | /// for (int i = 0; i < triBuf.getNumTriangles(); i++) | 48 | /// for (int i = 0; i < triBuf.getNumTriangles(); i++) |
49 | /// { | 49 | /// { |
50 | /// Triangle tri = triBuf.getTriangle(i); | 50 | /// Triangle tri = triBuf.getTriangle(i); |
51 | /// //do something useful here with the triangle | 51 | /// //do something useful here with the triangle |
52 | /// } | 52 | /// } |
53 | /// </summary> | 53 | /// </summary> |
54 | public class TriangleBuffer : ITriangleCallback | 54 | public class TriangleBuffer : ITriangleCallback |
55 | { | 55 | { |
56 | private List<Triangle> _triangleBuffer = new List<Triangle>(); | 56 | private List<Triangle> _triangleBuffer = new List<Triangle>(); |
57 | 57 | ||
58 | public int TriangleCount { get { return _triangleBuffer.Count; } } | 58 | public int TriangleCount { get { return _triangleBuffer.Count; } } |
59 | public Triangle this[int index] { get { return _triangleBuffer[index]; } } | 59 | public Triangle this[int index] { get { return _triangleBuffer[index]; } } |
60 | 60 | ||
61 | public void ClearBuffer() | 61 | public void ClearBuffer() |
62 | { | 62 | { |
63 | _triangleBuffer.Clear(); | 63 | _triangleBuffer.Clear(); |
64 | } | 64 | } |
65 | 65 | ||
66 | #region ITriangleCallback Members | 66 | #region ITriangleCallback Members |
67 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) | 67 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) |
68 | { | 68 | { |
69 | Triangle tri = new Triangle(); | 69 | Triangle tri = new Triangle(); |
70 | tri.VertexA = triangle[0]; | 70 | tri.VertexA = triangle[0]; |
71 | tri.VertexB = triangle[1]; | 71 | tri.VertexB = triangle[1]; |
72 | tri.VertexC = triangle[2]; | 72 | tri.VertexC = triangle[2]; |
73 | tri.PartId = partID; | 73 | tri.PartId = partID; |
74 | tri.TriangleIndex = triangleIndex; | 74 | tri.TriangleIndex = triangleIndex; |
75 | 75 | ||
76 | _triangleBuffer.Add(tri); | 76 | _triangleBuffer.Add(tri); |
77 | } | 77 | } |
78 | #endregion | 78 | #endregion |
79 | } | 79 | } |
80 | } | 80 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs index ac892c2..938765b 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs | |||
@@ -1,33 +1,33 @@ | |||
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 | public interface ITriangleCallback | 29 | public interface ITriangleCallback |
30 | { | 30 | { |
31 | void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex); | 31 | void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex); |
32 | } | 32 | } |
33 | } | 33 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs index 6d0d7b5..52c08ed 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs | |||
@@ -1,136 +1,136 @@ | |||
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 | /// IndexedMesh indexes into existing vertex and index arrays, in a similar way OpenGL glDrawElements | 30 | /// IndexedMesh indexes into existing vertex and index arrays, in a similar way OpenGL glDrawElements |
31 | /// instead of the number of indices, we pass the number of triangles | 31 | /// instead of the number of indices, we pass the number of triangles |
32 | /// </summary> | 32 | /// </summary> |
33 | public struct IndexedMesh | 33 | public struct IndexedMesh |
34 | { | 34 | { |
35 | private int _numTriangles; | 35 | private int _numTriangles; |
36 | private int[] _triangleIndexBase; | 36 | private int[] _triangleIndexBase; |
37 | private int _triangleIndexStride; | 37 | private int _triangleIndexStride; |
38 | private int _numVertices; | 38 | private int _numVertices; |
39 | private Vector3[] _vertexBase; | 39 | private Vector3[] _vertexBase; |
40 | private int _vertexStride; | 40 | private int _vertexStride; |
41 | 41 | ||
42 | public IndexedMesh(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride) | 42 | public IndexedMesh(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride) |
43 | { | 43 | { |
44 | _numTriangles = numTriangleIndices; | 44 | _numTriangles = numTriangleIndices; |
45 | _triangleIndexBase = triangleIndexBase; | 45 | _triangleIndexBase = triangleIndexBase; |
46 | _triangleIndexStride = triangleIndexStride; | 46 | _triangleIndexStride = triangleIndexStride; |
47 | _vertexBase = vertexBase; | 47 | _vertexBase = vertexBase; |
48 | _numVertices = numVertices; | 48 | _numVertices = numVertices; |
49 | _vertexStride = vertexStride; | 49 | _vertexStride = vertexStride; |
50 | } | 50 | } |
51 | 51 | ||
52 | public IndexedMesh(int[] triangleIndexBase, Vector3[] vertexBase) | 52 | public IndexedMesh(int[] triangleIndexBase, Vector3[] vertexBase) |
53 | { | 53 | { |
54 | _numTriangles = triangleIndexBase.Length; | 54 | _numTriangles = triangleIndexBase.Length; |
55 | _triangleIndexBase = triangleIndexBase; | 55 | _triangleIndexBase = triangleIndexBase; |
56 | _triangleIndexStride = 32; | 56 | _triangleIndexStride = 32; |
57 | _vertexBase = vertexBase; | 57 | _vertexBase = vertexBase; |
58 | _numVertices = vertexBase.Length; | 58 | _numVertices = vertexBase.Length; |
59 | _vertexStride = 24; | 59 | _vertexStride = 24; |
60 | } | 60 | } |
61 | 61 | ||
62 | public int TriangleCount { get { return _numTriangles; } set { _numTriangles = value; } } | 62 | public int TriangleCount { get { return _numTriangles; } set { _numTriangles = value; } } |
63 | public int[] TriangleIndexBase { get { return _triangleIndexBase; } set { _triangleIndexBase = value; } } | 63 | public int[] TriangleIndexBase { get { return _triangleIndexBase; } set { _triangleIndexBase = value; } } |
64 | public int TriangleIndexStride { get { return _triangleIndexStride; } set { _triangleIndexStride = value; } } | 64 | public int TriangleIndexStride { get { return _triangleIndexStride; } set { _triangleIndexStride = value; } } |
65 | public int VertexCount { get { return _numVertices; } set { _numVertices = value; } } | 65 | public int VertexCount { get { return _numVertices; } set { _numVertices = value; } } |
66 | public Vector3[] VertexBase { get { return _vertexBase; } set { _vertexBase = value; } } | 66 | public Vector3[] VertexBase { get { return _vertexBase; } set { _vertexBase = value; } } |
67 | public int VertexStride { get { return _vertexStride; } set { _vertexStride = value; } } | 67 | public int VertexStride { get { return _vertexStride; } set { _vertexStride = value; } } |
68 | } | 68 | } |
69 | 69 | ||
70 | /// <summary> | 70 | /// <summary> |
71 | /// TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays. | 71 | /// TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays. |
72 | /// Additional meshes can be added using addIndexedMesh | 72 | /// Additional meshes can be added using addIndexedMesh |
73 | /// </summary> | 73 | /// </summary> |
74 | public class TriangleIndexVertexArray : StridingMeshInterface | 74 | public class TriangleIndexVertexArray : StridingMeshInterface |
75 | { | 75 | { |
76 | List<IndexedMesh> _indexedMeshes = new List<IndexedMesh>(); | 76 | List<IndexedMesh> _indexedMeshes = new List<IndexedMesh>(); |
77 | 77 | ||
78 | public TriangleIndexVertexArray() { } | 78 | public TriangleIndexVertexArray() { } |
79 | 79 | ||
80 | public TriangleIndexVertexArray(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride) | 80 | public TriangleIndexVertexArray(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride) |
81 | { | 81 | { |
82 | IndexedMesh mesh = new IndexedMesh(); | 82 | IndexedMesh mesh = new IndexedMesh(); |
83 | mesh.TriangleCount = numTriangleIndices; | 83 | mesh.TriangleCount = numTriangleIndices; |
84 | mesh.TriangleIndexBase = triangleIndexBase; | 84 | mesh.TriangleIndexBase = triangleIndexBase; |
85 | mesh.TriangleIndexStride = triangleIndexStride; | 85 | mesh.TriangleIndexStride = triangleIndexStride; |
86 | mesh.VertexBase = vertexBase; | 86 | mesh.VertexBase = vertexBase; |
87 | mesh.VertexCount = numVertices; | 87 | mesh.VertexCount = numVertices; |
88 | mesh.VertexStride = vertexStride; | 88 | mesh.VertexStride = vertexStride; |
89 | 89 | ||
90 | AddIndexedMesh(mesh); | 90 | AddIndexedMesh(mesh); |
91 | } | 91 | } |
92 | 92 | ||
93 | public TriangleIndexVertexArray(int[] triangleIndexBase, Vector3[] vertexBase) | 93 | public TriangleIndexVertexArray(int[] triangleIndexBase, Vector3[] vertexBase) |
94 | : this(triangleIndexBase.Length, triangleIndexBase, 32, vertexBase.Length, vertexBase, 24) { } | 94 | : this(triangleIndexBase.Length, triangleIndexBase, 32, vertexBase.Length, vertexBase, 24) { } |
95 | 95 | ||
96 | public void AddIndexedMesh(IndexedMesh indexedMesh) | 96 | public void AddIndexedMesh(IndexedMesh indexedMesh) |
97 | { | 97 | { |
98 | _indexedMeshes.Add(indexedMesh); | 98 | _indexedMeshes.Add(indexedMesh); |
99 | } | 99 | } |
100 | 100 | ||
101 | public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) | 101 | public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) |
102 | { | 102 | { |
103 | throw new Exception("The method or operation is not implemented."); | 103 | throw new Exception("The method or operation is not implemented."); |
104 | } | 104 | } |
105 | 105 | ||
106 | public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) | 106 | public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) |
107 | { | 107 | { |
108 | throw new Exception("The method or operation is not implemented."); | 108 | throw new Exception("The method or operation is not implemented."); |
109 | } | 109 | } |
110 | 110 | ||
111 | public override void UnLockVertexBase(int subpart) | 111 | public override void UnLockVertexBase(int subpart) |
112 | { | 112 | { |
113 | throw new Exception("The method or operation is not implemented."); | 113 | throw new Exception("The method or operation is not implemented."); |
114 | } | 114 | } |
115 | 115 | ||
116 | public override void UnLockReadOnlyVertexBase(int subpart) | 116 | public override void UnLockReadOnlyVertexBase(int subpart) |
117 | { | 117 | { |
118 | throw new Exception("The method or operation is not implemented."); | 118 | throw new Exception("The method or operation is not implemented."); |
119 | } | 119 | } |
120 | 120 | ||
121 | public override int SubPartsCount() | 121 | public override int SubPartsCount() |
122 | { | 122 | { |
123 | return _indexedMeshes.Count; | 123 | return _indexedMeshes.Count; |
124 | } | 124 | } |
125 | 125 | ||
126 | public override void PreallocateVertices(int numverts) | 126 | public override void PreallocateVertices(int numverts) |
127 | { | 127 | { |
128 | throw new Exception("The method or operation is not implemented."); | 128 | throw new Exception("The method or operation is not implemented."); |
129 | } | 129 | } |
130 | 130 | ||
131 | public override void PreallocateIndices(int numindices) | 131 | public override void PreallocateIndices(int numindices) |
132 | { | 132 | { |
133 | throw new Exception("The method or operation is not implemented."); | 133 | throw new Exception("The method or operation is not implemented."); |
134 | } | 134 | } |
135 | } | 135 | } |
136 | } | 136 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs index 1426355..d5b93ba 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs | |||
@@ -1,102 +1,102 @@ | |||
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 | 25 | ||
26 | using MonoXnaCompactMaths; | 26 | using MonoXnaCompactMaths; |
27 | 27 | ||
28 | namespace XnaDevRu.BulletX | 28 | namespace XnaDevRu.BulletX |
29 | { | 29 | { |
30 | class TriangleMesh : StridingMeshInterface | 30 | class TriangleMesh : StridingMeshInterface |
31 | { | 31 | { |
32 | int _numTriangles; | 32 | int _numTriangles; |
33 | List<Vector3> _verts; | 33 | List<Vector3> _verts; |
34 | 34 | ||
35 | public TriangleMesh() | 35 | public TriangleMesh() |
36 | { | 36 | { |
37 | _numTriangles = 0; | 37 | _numTriangles = 0; |
38 | _verts = new List<Vector3>(); | 38 | _verts = new List<Vector3>(); |
39 | } | 39 | } |
40 | 40 | ||
41 | void AddTriangle(Vector3 vertex0, Vector3 vertex1, Vector3 vertex2) | 41 | void AddTriangle(Vector3 vertex0, Vector3 vertex1, Vector3 vertex2) |
42 | { | 42 | { |
43 | _verts.Add(vertex0); | 43 | _verts.Add(vertex0); |
44 | _verts.Add(vertex1); | 44 | _verts.Add(vertex1); |
45 | _verts.Add(vertex2); | 45 | _verts.Add(vertex2); |
46 | _numTriangles++; | 46 | _numTriangles++; |
47 | } | 47 | } |
48 | 48 | ||
49 | public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) | 49 | public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) |
50 | { | 50 | { |
51 | verts = new List<Vector3>(); | 51 | verts = new List<Vector3>(); |
52 | for (int i = 0; i < 3; i++) | 52 | for (int i = 0; i < 3; i++) |
53 | { | 53 | { |
54 | verts.Add(_verts[subpart * 3 + i]); | 54 | verts.Add(_verts[subpart * 3 + i]); |
55 | } | 55 | } |
56 | indicies = new List<int>(); | 56 | indicies = new List<int>(); |
57 | indicies.Add(0); | 57 | indicies.Add(0); |
58 | indicies.Add(1); | 58 | indicies.Add(1); |
59 | indicies.Add(2); | 59 | indicies.Add(2); |
60 | numfaces = 1; | 60 | numfaces = 1; |
61 | } | 61 | } |
62 | 62 | ||
63 | public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) | 63 | public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) |
64 | { | 64 | { |
65 | verts = new List<Vector3>(); | 65 | verts = new List<Vector3>(); |
66 | for (int i = 0; i < 3; i++) | 66 | for (int i = 0; i < 3; i++) |
67 | { | 67 | { |
68 | verts.Add(_verts[subpart * 3 + i]); | 68 | verts.Add(_verts[subpart * 3 + i]); |
69 | } | 69 | } |
70 | indicies = new List<int>(); | 70 | indicies = new List<int>(); |
71 | indicies.Add(0); | 71 | indicies.Add(0); |
72 | indicies.Add(1); | 72 | indicies.Add(1); |
73 | indicies.Add(2); | 73 | indicies.Add(2); |
74 | numfaces = 1; | 74 | numfaces = 1; |
75 | } | 75 | } |
76 | 76 | ||
77 | public override void UnLockVertexBase(int subpart) | 77 | public override void UnLockVertexBase(int subpart) |
78 | { | 78 | { |
79 | 79 | ||
80 | } | 80 | } |
81 | 81 | ||
82 | public override void UnLockReadOnlyVertexBase(int subpart) | 82 | public override void UnLockReadOnlyVertexBase(int subpart) |
83 | { | 83 | { |
84 | 84 | ||
85 | } | 85 | } |
86 | 86 | ||
87 | public override int SubPartsCount() | 87 | public override int SubPartsCount() |
88 | { | 88 | { |
89 | return _numTriangles; | 89 | return _numTriangles; |
90 | } | 90 | } |
91 | 91 | ||
92 | public override void PreallocateVertices(int numverts) | 92 | public override void PreallocateVertices(int numverts) |
93 | { | 93 | { |
94 | 94 | ||
95 | } | 95 | } |
96 | 96 | ||
97 | public override void PreallocateIndices(int numindices) | 97 | public override void PreallocateIndices(int numindices) |
98 | { | 98 | { |
99 | 99 | ||
100 | } | 100 | } |
101 | } | 101 | } |
102 | } | 102 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs index bc8f09e..5f5a0cf 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs | |||
@@ -1,160 +1,160 @@ | |||
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 | /// Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles. | 30 | /// Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles. |
31 | /// </summary> | 31 | /// </summary> |
32 | public class TriangleMeshShape : ConcaveShape | 32 | public class TriangleMeshShape : ConcaveShape |
33 | { | 33 | { |
34 | private StridingMeshInterface _meshInterface; | 34 | private StridingMeshInterface _meshInterface; |
35 | private Vector3 _localAabbMin; | 35 | private Vector3 _localAabbMin; |
36 | private Vector3 _localAabbMax; | 36 | private Vector3 _localAabbMax; |
37 | 37 | ||
38 | public TriangleMeshShape(StridingMeshInterface meshInterface) | 38 | public TriangleMeshShape(StridingMeshInterface meshInterface) |
39 | { | 39 | { |
40 | this._meshInterface = meshInterface; | 40 | this._meshInterface = meshInterface; |
41 | RecalcLocalAabb(); | 41 | RecalcLocalAabb(); |
42 | } | 42 | } |
43 | 43 | ||
44 | protected StridingMeshInterface MeshInterface { get { return _meshInterface; } set { _meshInterface = value; } } | 44 | protected StridingMeshInterface MeshInterface { get { return _meshInterface; } set { _meshInterface = value; } } |
45 | protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } } | 45 | protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } } |
46 | protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } } | 46 | protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } } |
47 | 47 | ||
48 | public override BroadphaseNativeTypes ShapeType | 48 | public override BroadphaseNativeTypes ShapeType |
49 | { | 49 | { |
50 | get | 50 | get |
51 | { | 51 | { |
52 | return BroadphaseNativeTypes.TriangleMesh; | 52 | return BroadphaseNativeTypes.TriangleMesh; |
53 | } | 53 | } |
54 | } | 54 | } |
55 | 55 | ||
56 | public override Vector3 LocalScaling | 56 | public override Vector3 LocalScaling |
57 | { | 57 | { |
58 | get | 58 | get |
59 | { | 59 | { |
60 | return _meshInterface.Scaling; | 60 | return _meshInterface.Scaling; |
61 | } | 61 | } |
62 | set | 62 | set |
63 | { | 63 | { |
64 | _meshInterface.Scaling = value; | 64 | _meshInterface.Scaling = value; |
65 | } | 65 | } |
66 | } | 66 | } |
67 | 67 | ||
68 | public override string Name | 68 | public override string Name |
69 | { | 69 | { |
70 | get | 70 | get |
71 | { | 71 | { |
72 | return "TriangleMesh"; | 72 | return "TriangleMesh"; |
73 | } | 73 | } |
74 | } | 74 | } |
75 | 75 | ||
76 | public void RecalcLocalAabb() | 76 | public void RecalcLocalAabb() |
77 | { | 77 | { |
78 | { | 78 | { |
79 | Vector3 vec = new Vector3(); | 79 | Vector3 vec = new Vector3(); |
80 | vec.X = 1f; | 80 | vec.X = 1f; |
81 | Vector3 tmp = LocalGetSupportingVertex(vec); | 81 | Vector3 tmp = LocalGetSupportingVertex(vec); |
82 | _localAabbMax.X = tmp.X + CollisionMargin; | 82 | _localAabbMax.X = tmp.X + CollisionMargin; |
83 | vec.X = -1f; | 83 | vec.X = -1f; |
84 | tmp = LocalGetSupportingVertex(vec); | 84 | tmp = LocalGetSupportingVertex(vec); |
85 | _localAabbMin.X = tmp.X - CollisionMargin; | 85 | _localAabbMin.X = tmp.X - CollisionMargin; |
86 | } | 86 | } |
87 | { | 87 | { |
88 | Vector3 vec = new Vector3(); | 88 | Vector3 vec = new Vector3(); |
89 | vec.Y = 1f; | 89 | vec.Y = 1f; |
90 | Vector3 tmp = LocalGetSupportingVertex(vec); | 90 | Vector3 tmp = LocalGetSupportingVertex(vec); |
91 | _localAabbMax.Y = tmp.Y + CollisionMargin; | 91 | _localAabbMax.Y = tmp.Y + CollisionMargin; |
92 | vec.Y = -1f; | 92 | vec.Y = -1f; |
93 | tmp = LocalGetSupportingVertex(vec); | 93 | tmp = LocalGetSupportingVertex(vec); |
94 | _localAabbMin.Y = tmp.Y - CollisionMargin; | 94 | _localAabbMin.Y = tmp.Y - CollisionMargin; |
95 | } | 95 | } |
96 | { | 96 | { |
97 | Vector3 vec = new Vector3(); | 97 | Vector3 vec = new Vector3(); |
98 | vec.Z = 1f; | 98 | vec.Z = 1f; |
99 | Vector3 tmp = LocalGetSupportingVertex(vec); | 99 | Vector3 tmp = LocalGetSupportingVertex(vec); |
100 | _localAabbMax.Z = tmp.Z + CollisionMargin; | 100 | _localAabbMax.Z = tmp.Z + CollisionMargin; |
101 | vec.Z = -1f; | 101 | vec.Z = -1f; |
102 | tmp = LocalGetSupportingVertex(vec); | 102 | tmp = LocalGetSupportingVertex(vec); |
103 | _localAabbMin.Z = tmp.Z - CollisionMargin; | 103 | _localAabbMin.Z = tmp.Z - CollisionMargin; |
104 | } | 104 | } |
105 | } | 105 | } |
106 | 106 | ||
107 | public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) | 107 | public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) |
108 | { | 108 | { |
109 | LocalProcessAllTriangles(callback, aabbMax, aabbMax); | 109 | LocalProcessAllTriangles(callback, aabbMax, aabbMax); |
110 | } | 110 | } |
111 | 111 | ||
112 | protected void LocalProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) | 112 | protected void LocalProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) |
113 | { | 113 | { |
114 | FilteredCallback filterCallback = new FilteredCallback(callback, aabbMin, aabbMax); | 114 | FilteredCallback filterCallback = new FilteredCallback(callback, aabbMin, aabbMax); |
115 | _meshInterface.InternalProcessAllTriangles(filterCallback, aabbMin, aabbMax); | 115 | _meshInterface.InternalProcessAllTriangles(filterCallback, aabbMin, aabbMax); |
116 | } | 116 | } |
117 | 117 | ||
118 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | 118 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) |
119 | { | 119 | { |
120 | Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin); | 120 | Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin); |
121 | Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin); | 121 | Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin); |
122 | 122 | ||
123 | Matrix abs_b = MathHelper.Absolute(t); | 123 | Matrix abs_b = MathHelper.Absolute(t); |
124 | 124 | ||
125 | Vector3 center = MathHelper.MatrixToVector(t, localCenter); | 125 | Vector3 center = MathHelper.MatrixToVector(t, localCenter); |
126 | 126 | ||
127 | Vector3 extent = new Vector3(Vector3.Dot(new Vector3(abs_b.M11, abs_b.M12, abs_b.M13), localHalfExtents), | 127 | Vector3 extent = new Vector3(Vector3.Dot(new Vector3(abs_b.M11, abs_b.M12, abs_b.M13), localHalfExtents), |
128 | Vector3.Dot(new Vector3(abs_b.M21, abs_b.M22, abs_b.M23), localHalfExtents), | 128 | Vector3.Dot(new Vector3(abs_b.M21, abs_b.M22, abs_b.M23), localHalfExtents), |
129 | Vector3.Dot(new Vector3(abs_b.M31, abs_b.M32, abs_b.M33), localHalfExtents)); | 129 | Vector3.Dot(new Vector3(abs_b.M31, abs_b.M32, abs_b.M33), localHalfExtents)); |
130 | extent += new Vector3(Margin, Margin, Margin); | 130 | extent += new Vector3(Margin, Margin, Margin); |
131 | 131 | ||
132 | aabbMin = center - extent; | 132 | aabbMin = center - extent; |
133 | aabbMax = center + extent; | 133 | aabbMax = center + extent; |
134 | } | 134 | } |
135 | 135 | ||
136 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | 136 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) |
137 | { | 137 | { |
138 | inertia = new Vector3(); | 138 | inertia = new Vector3(); |
139 | //moving concave objects not supported | 139 | //moving concave objects not supported |
140 | BulletDebug.Assert(false); | 140 | BulletDebug.Assert(false); |
141 | } | 141 | } |
142 | 142 | ||
143 | public virtual Vector3 LocalGetSupportingVertex(Vector3 vec) | 143 | public virtual Vector3 LocalGetSupportingVertex(Vector3 vec) |
144 | { | 144 | { |
145 | Vector3 supportVertex; | 145 | Vector3 supportVertex; |
146 | Matrix ident = Matrix.Identity; | 146 | Matrix ident = Matrix.Identity; |
147 | SupportVertexCallback supportCallback = new SupportVertexCallback(vec, ident); | 147 | SupportVertexCallback supportCallback = new SupportVertexCallback(vec, ident); |
148 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | 148 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); |
149 | LocalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); | 149 | LocalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); |
150 | supportVertex = supportCallback.SupportVertexLocal; | 150 | supportVertex = supportCallback.SupportVertexLocal; |
151 | return supportVertex; | 151 | return supportVertex; |
152 | } | 152 | } |
153 | 153 | ||
154 | public virtual Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | 154 | public virtual Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) |
155 | { | 155 | { |
156 | BulletDebug.Assert(false); | 156 | BulletDebug.Assert(false); |
157 | return LocalGetSupportingVertex(vec); | 157 | return LocalGetSupportingVertex(vec); |
158 | } | 158 | } |
159 | } | 159 | } |
160 | } | 160 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs index 59ffad0..d5a17f3 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs | |||
@@ -1,187 +1,187 @@ | |||
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 | public class TriangleShape : PolyhedralConvexShape | 29 | public class TriangleShape : PolyhedralConvexShape |
30 | { | 30 | { |
31 | private Vector3[] _vertices = new Vector3[3]; | 31 | private Vector3[] _vertices = new Vector3[3]; |
32 | 32 | ||
33 | public TriangleShape(Vector3 pointA, Vector3 pointB, Vector3 pointC) | 33 | public TriangleShape(Vector3 pointA, Vector3 pointB, Vector3 pointC) |
34 | { | 34 | { |
35 | _vertices[0] = pointA; | 35 | _vertices[0] = pointA; |
36 | _vertices[1] = pointB; | 36 | _vertices[1] = pointB; |
37 | _vertices[2] = pointC; | 37 | _vertices[2] = pointC; |
38 | } | 38 | } |
39 | 39 | ||
40 | public override int PreferredPenetrationDirectionsCount | 40 | public override int PreferredPenetrationDirectionsCount |
41 | { | 41 | { |
42 | get | 42 | get |
43 | { | 43 | { |
44 | return 2; | 44 | return 2; |
45 | } | 45 | } |
46 | } | 46 | } |
47 | 47 | ||
48 | public Vector3[] Vertices | 48 | public Vector3[] Vertices |
49 | { | 49 | { |
50 | get | 50 | get |
51 | { | 51 | { |
52 | return _vertices; | 52 | return _vertices; |
53 | } | 53 | } |
54 | } | 54 | } |
55 | 55 | ||
56 | public override int VertexCount | 56 | public override int VertexCount |
57 | { | 57 | { |
58 | get | 58 | get |
59 | { | 59 | { |
60 | return 3; | 60 | return 3; |
61 | } | 61 | } |
62 | } | 62 | } |
63 | 63 | ||
64 | public override int EdgeCount | 64 | public override int EdgeCount |
65 | { | 65 | { |
66 | get | 66 | get |
67 | { | 67 | { |
68 | return 3; | 68 | return 3; |
69 | } | 69 | } |
70 | } | 70 | } |
71 | 71 | ||
72 | public override int PlaneCount | 72 | public override int PlaneCount |
73 | { | 73 | { |
74 | get | 74 | get |
75 | { | 75 | { |
76 | return 1; | 76 | return 1; |
77 | } | 77 | } |
78 | } | 78 | } |
79 | 79 | ||
80 | public override BroadphaseNativeTypes ShapeType | 80 | public override BroadphaseNativeTypes ShapeType |
81 | { | 81 | { |
82 | get | 82 | get |
83 | { | 83 | { |
84 | return BroadphaseNativeTypes.Triangle; | 84 | return BroadphaseNativeTypes.Triangle; |
85 | } | 85 | } |
86 | } | 86 | } |
87 | 87 | ||
88 | public override string Name | 88 | public override string Name |
89 | { | 89 | { |
90 | get | 90 | get |
91 | { | 91 | { |
92 | return "Triangle"; | 92 | return "Triangle"; |
93 | } | 93 | } |
94 | } | 94 | } |
95 | 95 | ||
96 | public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) | 96 | public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) |
97 | { | 97 | { |
98 | CalculateNormal(out penetrationVector); | 98 | CalculateNormal(out penetrationVector); |
99 | if (index != 0) | 99 | if (index != 0) |
100 | penetrationVector *= -1f; | 100 | penetrationVector *= -1f; |
101 | } | 101 | } |
102 | 102 | ||
103 | public virtual void GetPlaneEquation(int i, out Vector3 planeNormal, out Vector3 planeSupport) | 103 | public virtual void GetPlaneEquation(int i, out Vector3 planeNormal, out Vector3 planeSupport) |
104 | { | 104 | { |
105 | CalculateNormal(out planeNormal); | 105 | CalculateNormal(out planeNormal); |
106 | planeSupport = _vertices[0]; | 106 | planeSupport = _vertices[0]; |
107 | } | 107 | } |
108 | 108 | ||
109 | public void CalculateNormal(out Vector3 normal) | 109 | public void CalculateNormal(out Vector3 normal) |
110 | { | 110 | { |
111 | normal = Vector3.Normalize(Vector3.Cross(_vertices[1] - _vertices[0], _vertices[2] - _vertices[0])); | 111 | normal = Vector3.Normalize(Vector3.Cross(_vertices[1] - _vertices[0], _vertices[2] - _vertices[0])); |
112 | } | 112 | } |
113 | 113 | ||
114 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | 114 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) |
115 | { | 115 | { |
116 | Vector3 dots = new Vector3(Vector3.Dot(vec, _vertices[0]), Vector3.Dot(vec, _vertices[1]), Vector3.Dot(vec, _vertices[2])); | 116 | Vector3 dots = new Vector3(Vector3.Dot(vec, _vertices[0]), Vector3.Dot(vec, _vertices[1]), Vector3.Dot(vec, _vertices[2])); |
117 | return _vertices[MathHelper.MaxAxis(dots)]; | 117 | return _vertices[MathHelper.MaxAxis(dots)]; |
118 | } | 118 | } |
119 | 119 | ||
120 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | 120 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) |
121 | { | 121 | { |
122 | for (int i = 0; i < vectors.Length; i++) | 122 | for (int i = 0; i < vectors.Length; i++) |
123 | { | 123 | { |
124 | Vector3 dir = vectors[i]; | 124 | Vector3 dir = vectors[i]; |
125 | Vector3 dots = new Vector3(Vector3.Dot(dir, _vertices[0]), Vector3.Dot(dir, _vertices[1]), Vector3.Dot(dir, _vertices[2])); | 125 | Vector3 dots = new Vector3(Vector3.Dot(dir, _vertices[0]), Vector3.Dot(dir, _vertices[1]), Vector3.Dot(dir, _vertices[2])); |
126 | supportVerticesOut[i] = _vertices[MathHelper.MaxAxis(dots)]; | 126 | supportVerticesOut[i] = _vertices[MathHelper.MaxAxis(dots)]; |
127 | } | 127 | } |
128 | } | 128 | } |
129 | 129 | ||
130 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | 130 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) |
131 | { | 131 | { |
132 | inertia = new Vector3(); | 132 | inertia = new Vector3(); |
133 | BulletDebug.Assert(false); | 133 | BulletDebug.Assert(false); |
134 | } | 134 | } |
135 | 135 | ||
136 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) | 136 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) |
137 | { | 137 | { |
138 | GetVertex(i, out pa); | 138 | GetVertex(i, out pa); |
139 | GetVertex((i + 1) % 3, out pb); | 139 | GetVertex((i + 1) % 3, out pb); |
140 | } | 140 | } |
141 | 141 | ||
142 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | 142 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) |
143 | { | 143 | { |
144 | GetAabbSlow(t, out aabbMin, out aabbMax); | 144 | GetAabbSlow(t, out aabbMin, out aabbMax); |
145 | } | 145 | } |
146 | 146 | ||
147 | public override void GetVertex(int i, out Vector3 vtx) | 147 | public override void GetVertex(int i, out Vector3 vtx) |
148 | { | 148 | { |
149 | vtx = _vertices[i]; | 149 | vtx = _vertices[i]; |
150 | } | 150 | } |
151 | 151 | ||
152 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) | 152 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) |
153 | { | 153 | { |
154 | GetPlaneEquation(i, out planeNormal, out planeSupport); | 154 | GetPlaneEquation(i, out planeNormal, out planeSupport); |
155 | } | 155 | } |
156 | 156 | ||
157 | public override bool IsInside(Vector3 pt, float tolerance) | 157 | public override bool IsInside(Vector3 pt, float tolerance) |
158 | { | 158 | { |
159 | Vector3 normal; | 159 | Vector3 normal; |
160 | CalculateNormal(out normal); | 160 | CalculateNormal(out normal); |
161 | //distance to plane | 161 | //distance to plane |
162 | float dist = Vector3.Dot(pt, normal); | 162 | float dist = Vector3.Dot(pt, normal); |
163 | float planeconst = Vector3.Dot(_vertices[0], normal); | 163 | float planeconst = Vector3.Dot(_vertices[0], normal); |
164 | dist -= planeconst; | 164 | dist -= planeconst; |
165 | if (dist >= -tolerance && dist <= tolerance) | 165 | if (dist >= -tolerance && dist <= tolerance) |
166 | { | 166 | { |
167 | //inside check on edge-planes | 167 | //inside check on edge-planes |
168 | int i; | 168 | int i; |
169 | for (i = 0; i < 3; i++) | 169 | for (i = 0; i < 3; i++) |
170 | { | 170 | { |
171 | Vector3 pa, pb; | 171 | Vector3 pa, pb; |
172 | GetEdge(i, out pa, out pb); | 172 | GetEdge(i, out pa, out pb); |
173 | Vector3 edge = pb - pa; | 173 | Vector3 edge = pb - pa; |
174 | Vector3 edgeNormal = Vector3.Cross(edge, normal); | 174 | Vector3 edgeNormal = Vector3.Cross(edge, normal); |
175 | edgeNormal = Vector3.Normalize(edgeNormal); | 175 | edgeNormal = Vector3.Normalize(edgeNormal); |
176 | float distance = Vector3.Dot(pt, edgeNormal); | 176 | float distance = Vector3.Dot(pt, edgeNormal); |
177 | float edgeConst = Vector3.Dot(pa, edgeNormal); | 177 | float edgeConst = Vector3.Dot(pa, edgeNormal); |
178 | distance -= edgeConst; | 178 | distance -= edgeConst; |
179 | if (distance < -tolerance) | 179 | if (distance < -tolerance) |
180 | return false; | 180 | return false; |
181 | } | 181 | } |
182 | return true; | 182 | return true; |
183 | } | 183 | } |
184 | return false; | 184 | return false; |
185 | } | 185 | } |
186 | } | 186 | } |
187 | } | 187 | } |
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 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs index 9e88a7e..93501ed 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs | |||
@@ -1,73 +1,73 @@ | |||
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 | 29 | ||
30 | /// <summary> | 30 | /// <summary> |
31 | /// CastResult stores the closest result | 31 | /// CastResult stores the closest result |
32 | /// alternatively, add a callback method to decide about closest/all results | 32 | /// alternatively, add a callback method to decide about closest/all results |
33 | /// </summary> | 33 | /// </summary> |
34 | public class CastResult | 34 | public class CastResult |
35 | { | 35 | { |
36 | private Vector3 _normal; | 36 | private Vector3 _normal; |
37 | private float _fraction; | 37 | private float _fraction; |
38 | private Matrix _hitTransformA; | 38 | private Matrix _hitTransformA; |
39 | private Matrix _hitTransformB; | 39 | private Matrix _hitTransformB; |
40 | private IDebugDraw _debugDrawer; | 40 | private IDebugDraw _debugDrawer; |
41 | 41 | ||
42 | public CastResult() | 42 | public CastResult() |
43 | { | 43 | { |
44 | _fraction = 1e30f; | 44 | _fraction = 1e30f; |
45 | } | 45 | } |
46 | 46 | ||
47 | public Vector3 Normal { get { return _normal; } set { _normal = value; } } | 47 | public Vector3 Normal { get { return _normal; } set { _normal = value; } } |
48 | public float Fraction { get { return _fraction; } set { _fraction = value; } } | 48 | public float Fraction { get { return _fraction; } set { _fraction = value; } } |
49 | public Matrix HitTransformA { get { return _hitTransformA; } set { _hitTransformA = value; } } | 49 | public Matrix HitTransformA { get { return _hitTransformA; } set { _hitTransformA = value; } } |
50 | public Matrix HitTransformB { get { return _hitTransformB; } set { _hitTransformB = value; } } | 50 | public Matrix HitTransformB { get { return _hitTransformB; } set { _hitTransformB = value; } } |
51 | public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } } | 51 | public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } } |
52 | 52 | ||
53 | public virtual void DebugDraw(float fraction) { } | 53 | public virtual void DebugDraw(float fraction) { } |
54 | public virtual void DrawCoordSystem(Matrix trans) { } | 54 | public virtual void DrawCoordSystem(Matrix trans) { } |
55 | } | 55 | } |
56 | 56 | ||
57 | /// <summary> | 57 | /// <summary> |
58 | /// ConvexCast is an interface for Casting | 58 | /// ConvexCast is an interface for Casting |
59 | /// </summary> | 59 | /// </summary> |
60 | public interface IConvexCast | 60 | public interface IConvexCast |
61 | { | 61 | { |
62 | /// <summary> | 62 | /// <summary> |
63 | /// cast a convex against another convex object | 63 | /// cast a convex against another convex object |
64 | /// </summary> | 64 | /// </summary> |
65 | /// <param name="fromA"></param> | 65 | /// <param name="fromA"></param> |
66 | /// <param name="toA"></param> | 66 | /// <param name="toA"></param> |
67 | /// <param name="fromB"></param> | 67 | /// <param name="fromB"></param> |
68 | /// <param name="toB"></param> | 68 | /// <param name="toB"></param> |
69 | /// <param name="result"></param> | 69 | /// <param name="result"></param> |
70 | /// <returns></returns> | 70 | /// <returns></returns> |
71 | bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result); | 71 | bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result); |
72 | } | 72 | } |
73 | } | 73 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs index fae2557..5b07d9b 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs | |||
@@ -1,117 +1,117 @@ | |||
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 | public abstract class DiscreteCollisionDetectorInterface | 29 | public abstract class DiscreteCollisionDetectorInterface |
30 | { | 30 | { |
31 | public abstract class Result | 31 | public abstract class Result |
32 | { | 32 | { |
33 | public abstract void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB); | 33 | public abstract void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB); |
34 | public abstract void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth); | 34 | public abstract void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth); |
35 | } | 35 | } |
36 | 36 | ||
37 | public class ClosestPointInput | 37 | public class ClosestPointInput |
38 | { | 38 | { |
39 | private float _maximumDistanceSquared; | 39 | private float _maximumDistanceSquared; |
40 | private Matrix _transformA, _transformB; | 40 | private Matrix _transformA, _transformB; |
41 | 41 | ||
42 | #region Properties | 42 | #region Properties |
43 | public Matrix TransformB | 43 | public Matrix TransformB |
44 | { | 44 | { |
45 | get { return _transformB; } | 45 | get { return _transformB; } |
46 | set { _transformB = value; } | 46 | set { _transformB = value; } |
47 | } | 47 | } |
48 | 48 | ||
49 | public Matrix TransformA | 49 | public Matrix TransformA |
50 | { | 50 | { |
51 | get { return _transformA; } | 51 | get { return _transformA; } |
52 | set { _transformA = value; } | 52 | set { _transformA = value; } |
53 | } | 53 | } |
54 | 54 | ||
55 | public float MaximumDistanceSquared | 55 | public float MaximumDistanceSquared |
56 | { | 56 | { |
57 | get { return _maximumDistanceSquared; } | 57 | get { return _maximumDistanceSquared; } |
58 | set { _maximumDistanceSquared = value; } | 58 | set { _maximumDistanceSquared = value; } |
59 | } | 59 | } |
60 | #endregion | 60 | #endregion |
61 | 61 | ||
62 | public ClosestPointInput() | 62 | public ClosestPointInput() |
63 | { | 63 | { |
64 | _maximumDistanceSquared = 1e30f; | 64 | _maximumDistanceSquared = 1e30f; |
65 | } | 65 | } |
66 | } | 66 | } |
67 | 67 | ||
68 | public abstract void GetClosestPoints(ClosestPointInput input, Result output, IDebugDraw debugDraw); | 68 | public abstract void GetClosestPoints(ClosestPointInput input, Result output, IDebugDraw debugDraw); |
69 | } | 69 | } |
70 | 70 | ||
71 | public class StorageResult : DiscreteCollisionDetectorInterface.Result | 71 | public class StorageResult : DiscreteCollisionDetectorInterface.Result |
72 | { | 72 | { |
73 | private Vector3 _closestPointInB; | 73 | private Vector3 _closestPointInB; |
74 | private Vector3 _normalOnSurfaceB; | 74 | private Vector3 _normalOnSurfaceB; |
75 | private float _distance; //negative means penetration ! | 75 | private float _distance; //negative means penetration ! |
76 | 76 | ||
77 | #region Properties | 77 | #region Properties |
78 | 78 | ||
79 | public float Distance | 79 | public float Distance |
80 | { | 80 | { |
81 | get { return _distance; } | 81 | get { return _distance; } |
82 | set { _distance = value; } | 82 | set { _distance = value; } |
83 | } | 83 | } |
84 | public Vector3 NormalOnSurfaceB | 84 | public Vector3 NormalOnSurfaceB |
85 | { | 85 | { |
86 | get { return _normalOnSurfaceB; } | 86 | get { return _normalOnSurfaceB; } |
87 | set { _normalOnSurfaceB = value; } | 87 | set { _normalOnSurfaceB = value; } |
88 | } | 88 | } |
89 | public Vector3 ClosestPointInB | 89 | public Vector3 ClosestPointInB |
90 | { | 90 | { |
91 | get { return _closestPointInB; } | 91 | get { return _closestPointInB; } |
92 | set { _closestPointInB = value; } | 92 | set { _closestPointInB = value; } |
93 | } | 93 | } |
94 | 94 | ||
95 | #endregion | 95 | #endregion |
96 | 96 | ||
97 | public StorageResult() | 97 | public StorageResult() |
98 | { | 98 | { |
99 | _distance = 1e30f; | 99 | _distance = 1e30f; |
100 | } | 100 | } |
101 | 101 | ||
102 | public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) | 102 | public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) |
103 | { | 103 | { |
104 | if (depth < _distance) | 104 | if (depth < _distance) |
105 | { | 105 | { |
106 | _normalOnSurfaceB = normalOnBInWorld; | 106 | _normalOnSurfaceB = normalOnBInWorld; |
107 | _closestPointInB = pointInWorld; | 107 | _closestPointInB = pointInWorld; |
108 | _distance = depth; | 108 | _distance = depth; |
109 | } | 109 | } |
110 | } | 110 | } |
111 | 111 | ||
112 | public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1) | 112 | public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1) |
113 | { | 113 | { |
114 | throw new Exception("The method or operation is not implemented."); | 114 | throw new Exception("The method or operation is not implemented."); |
115 | } | 115 | } |
116 | } | 116 | } |
117 | } | 117 | } |
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 | ||
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 | /// 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 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs index 50ae7ab..a3064db 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs | |||
@@ -1,633 +1,633 @@ | |||
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 | /// GJK-EPA collision solver by Nathanael Presson | 30 | /// GJK-EPA collision solver by Nathanael Presson |
31 | /// Nov.2006 | 31 | /// Nov.2006 |
32 | /// </summary> | 32 | /// </summary> |
33 | public class GjkEpa | 33 | public class GjkEpa |
34 | { | 34 | { |
35 | //private static readonly int _precision = 1 /* U(sizeof(F) == 4)*/; | 35 | //private static readonly int _precision = 1 /* U(sizeof(F) == 4)*/; |
36 | 36 | ||
37 | private static readonly float _infinity = MathHelper.Infinity; | 37 | private static readonly float _infinity = MathHelper.Infinity; |
38 | //private static readonly float _pi = (float)Math.PI; | 38 | //private static readonly float _pi = (float)Math.PI; |
39 | private static readonly float _twoPi = (float)(Math.PI * 2); | 39 | private static readonly float _twoPi = (float)(Math.PI * 2); |
40 | 40 | ||
41 | private static readonly int _gjkMaxIterations = 128; | 41 | private static readonly int _gjkMaxIterations = 128; |
42 | private static readonly int _gjkHashSize = 1 << 6; | 42 | private static readonly int _gjkHashSize = 1 << 6; |
43 | private static readonly int _gjkHashMask = _gjkHashSize - 1; | 43 | private static readonly int _gjkHashMask = _gjkHashSize - 1; |
44 | private static readonly float _gjkInSimplexEpsilon = 0.0001f; | 44 | private static readonly float _gjkInSimplexEpsilon = 0.0001f; |
45 | private static readonly float _gjkSquareInSimplexEpsilon = _gjkInSimplexEpsilon * _gjkInSimplexEpsilon; | 45 | private static readonly float _gjkSquareInSimplexEpsilon = _gjkInSimplexEpsilon * _gjkInSimplexEpsilon; |
46 | 46 | ||
47 | private static readonly int _epaMaxIterations = 256; | 47 | private static readonly int _epaMaxIterations = 256; |
48 | private static readonly float _epaInFaceEpsilon = 0.01f; | 48 | private static readonly float _epaInFaceEpsilon = 0.01f; |
49 | private static readonly float _epaAccuracy = 0.001f; | 49 | private static readonly float _epaAccuracy = 0.001f; |
50 | 50 | ||
51 | public static float EpaAccuracy { get { return _epaAccuracy; } } | 51 | public static float EpaAccuracy { get { return _epaAccuracy; } } |
52 | 52 | ||
53 | private static float Abs(float v) { return (v < 0 ? -v : v); } | 53 | private static float Abs(float v) { return (v < 0 ? -v : v); } |
54 | private static float Sign(float v) { return (v < 0 ? -1 : 1); } | 54 | private static float Sign(float v) { return (v < 0 ? -1 : 1); } |
55 | 55 | ||
56 | static void Swap<T>(ref T a, ref T b) | 56 | static void Swap<T>(ref T a, ref T b) |
57 | { | 57 | { |
58 | T t = a; | 58 | T t = a; |
59 | a = b; | 59 | a = b; |
60 | b = t; | 60 | b = t; |
61 | } | 61 | } |
62 | 62 | ||
63 | public class Gjk | 63 | public class Gjk |
64 | { | 64 | { |
65 | public class MinkowskiVertice | 65 | public class MinkowskiVertice |
66 | { | 66 | { |
67 | private Vector3 _vertice; /* Minkowski vertice */ | 67 | private Vector3 _vertice; /* Minkowski vertice */ |
68 | private Vector3 _ray; /* Ray */ | 68 | private Vector3 _ray; /* Ray */ |
69 | 69 | ||
70 | public Vector3 Vertice { get { return _vertice; } set { _vertice = value; } } | 70 | public Vector3 Vertice { get { return _vertice; } set { _vertice = value; } } |
71 | public Vector3 Ray { get { return _ray; } set { _ray = value; } } | 71 | public Vector3 Ray { get { return _ray; } set { _ray = value; } } |
72 | } | 72 | } |
73 | public class He | 73 | public class He |
74 | { | 74 | { |
75 | private Vector3 _ray; | 75 | private Vector3 _ray; |
76 | private He _next; | 76 | private He _next; |
77 | 77 | ||
78 | public He Next { get { return _next; } set { _next = value; } } | 78 | public He Next { get { return _next; } set { _next = value; } } |
79 | public Vector3 Ray { get { return _ray; } set { _ray = value; } } | 79 | public Vector3 Ray { get { return _ray; } set { _ray = value; } } |
80 | } | 80 | } |
81 | 81 | ||
82 | private He[] _table = new He[_gjkHashSize]; | 82 | private He[] _table = new He[_gjkHashSize]; |
83 | private Matrix[] _wrotations = new Matrix[2]; | 83 | private Matrix[] _wrotations = new Matrix[2]; |
84 | private Vector3[] _positions = new Vector3[2]; | 84 | private Vector3[] _positions = new Vector3[2]; |
85 | private ConvexShape[] _shapes = new ConvexShape[2]; | 85 | private ConvexShape[] _shapes = new ConvexShape[2]; |
86 | private MinkowskiVertice[] _simplex = new MinkowskiVertice[5]; | 86 | private MinkowskiVertice[] _simplex = new MinkowskiVertice[5]; |
87 | private Vector3 _ray; | 87 | private Vector3 _ray; |
88 | private int _order; | 88 | private int _order; |
89 | private int _iterations; | 89 | private int _iterations; |
90 | private float _margin; | 90 | private float _margin; |
91 | private bool _failed; | 91 | private bool _failed; |
92 | 92 | ||
93 | public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, | 93 | public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, |
94 | Matrix wrotationB, Vector3 positionB, ConvexShape shapeB) | 94 | Matrix wrotationB, Vector3 positionB, ConvexShape shapeB) |
95 | : this(wrotationA, positionA, shapeA, wrotationB, positionB, shapeB, 0) { } | 95 | : this(wrotationA, positionA, shapeA, wrotationB, positionB, shapeB, 0) { } |
96 | 96 | ||
97 | public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, | 97 | public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, |
98 | Matrix wrotationB, Vector3 positionB, ConvexShape shapeB, | 98 | Matrix wrotationB, Vector3 positionB, ConvexShape shapeB, |
99 | float pmargin) | 99 | float pmargin) |
100 | { | 100 | { |
101 | for (int i = 0; i < _simplex.Length; i++) | 101 | for (int i = 0; i < _simplex.Length; i++) |
102 | _simplex[i] = new MinkowskiVertice(); | 102 | _simplex[i] = new MinkowskiVertice(); |
103 | 103 | ||
104 | for (int i = 0; i < _wrotations.Length; i++) | 104 | for (int i = 0; i < _wrotations.Length; i++) |
105 | _wrotations[i] = new Matrix(); | 105 | _wrotations[i] = new Matrix(); |
106 | 106 | ||
107 | for (int i = 0; i < _positions.Length; i++) | 107 | for (int i = 0; i < _positions.Length; i++) |
108 | _positions[i] = new Vector3(); | 108 | _positions[i] = new Vector3(); |
109 | 109 | ||
110 | _wrotations[0] = wrotationA; _positions[0] = positionA; | 110 | _wrotations[0] = wrotationA; _positions[0] = positionA; |
111 | _shapes[0] = shapeA; | 111 | _shapes[0] = shapeA; |
112 | _wrotations[0].Translation = Vector3.Zero; | 112 | _wrotations[0].Translation = Vector3.Zero; |
113 | _wrotations[1] = wrotationB; _positions[1] = positionB; | 113 | _wrotations[1] = wrotationB; _positions[1] = positionB; |
114 | _shapes[1] = shapeB; | 114 | _shapes[1] = shapeB; |
115 | _wrotations[1].Translation = Vector3.Zero; | 115 | _wrotations[1].Translation = Vector3.Zero; |
116 | //sablock = sa->BeginBlock(); | 116 | //sablock = sa->BeginBlock(); |
117 | _margin = pmargin; | 117 | _margin = pmargin; |
118 | _failed = false; | 118 | _failed = false; |
119 | } | 119 | } |
120 | 120 | ||
121 | public bool Failed { get { return _failed; } } | 121 | public bool Failed { get { return _failed; } } |
122 | public int Iterations { get { return _iterations; } } | 122 | public int Iterations { get { return _iterations; } } |
123 | public int Order { get { return _order; } } | 123 | public int Order { get { return _order; } } |
124 | public MinkowskiVertice[] Simplex { get { return _simplex; } } | 124 | public MinkowskiVertice[] Simplex { get { return _simplex; } } |
125 | 125 | ||
126 | public int Hash(Vector3 v) | 126 | public int Hash(Vector3 v) |
127 | { | 127 | { |
128 | int h = ((int)(v.X * 15461) ^ (int)(v.Y * 83003) ^ (int)(v.Z * 15473)); | 128 | int h = ((int)(v.X * 15461) ^ (int)(v.Y * 83003) ^ (int)(v.Z * 15473)); |
129 | return (h * 169639) & _gjkHashMask; | 129 | return (h * 169639) & _gjkHashMask; |
130 | } | 130 | } |
131 | 131 | ||
132 | public bool FetchSupport() | 132 | public bool FetchSupport() |
133 | { | 133 | { |
134 | int h = Hash(_ray); | 134 | int h = Hash(_ray); |
135 | He e = _table[h]; | 135 | He e = _table[h]; |
136 | while (e != null) | 136 | while (e != null) |
137 | { | 137 | { |
138 | if (e.Ray == _ray) | 138 | if (e.Ray == _ray) |
139 | { | 139 | { |
140 | --_order; | 140 | --_order; |
141 | return (false); | 141 | return (false); |
142 | } | 142 | } |
143 | else e = e.Next; | 143 | else e = e.Next; |
144 | } | 144 | } |
145 | e = new He(); | 145 | e = new He(); |
146 | e.Ray = _ray; | 146 | e.Ray = _ray; |
147 | e.Next = _table[h]; | 147 | e.Next = _table[h]; |
148 | _table[h] = e; | 148 | _table[h] = e; |
149 | Support(_ray, ref _simplex[++_order]); | 149 | Support(_ray, ref _simplex[++_order]); |
150 | return (Vector3.Dot(_ray, _simplex[_order].Vertice) > 0); | 150 | return (Vector3.Dot(_ray, _simplex[_order].Vertice) > 0); |
151 | } | 151 | } |
152 | 152 | ||
153 | public Vector3 LocalSupport(Vector3 d, int i) | 153 | public Vector3 LocalSupport(Vector3 d, int i) |
154 | { | 154 | { |
155 | Matrix m = _wrotations[i]; | 155 | Matrix m = _wrotations[i]; |
156 | m.Translation = Vector3.Zero; | 156 | m.Translation = Vector3.Zero; |
157 | Vector3 vtx = Vector3.TransformNormal(d, m); | 157 | Vector3 vtx = Vector3.TransformNormal(d, m); |
158 | Vector3 result = MathHelper.MatrixToVector(_wrotations[i], _shapes[i].LocalGetSupportingVertex(vtx)); | 158 | Vector3 result = MathHelper.MatrixToVector(_wrotations[i], _shapes[i].LocalGetSupportingVertex(vtx)); |
159 | return (result + _positions[i]); | 159 | return (result + _positions[i]); |
160 | } | 160 | } |
161 | 161 | ||
162 | public void Support(Vector3 d, ref MinkowskiVertice v) | 162 | public void Support(Vector3 d, ref MinkowskiVertice v) |
163 | { | 163 | { |
164 | v.Ray = d; | 164 | v.Ray = d; |
165 | v.Vertice = LocalSupport(d, 0) - LocalSupport(-d, 1) + d * _margin; | 165 | v.Vertice = LocalSupport(d, 0) - LocalSupport(-d, 1) + d * _margin; |
166 | } | 166 | } |
167 | 167 | ||
168 | public bool SolveSimplex2(Vector3 ao, Vector3 ab) | 168 | public bool SolveSimplex2(Vector3 ao, Vector3 ab) |
169 | { | 169 | { |
170 | if (Vector3.Dot(ab, ao) >= 0) | 170 | if (Vector3.Dot(ab, ao) >= 0) |
171 | { | 171 | { |
172 | Vector3 cabo = Vector3.Cross(ab, ao); | 172 | Vector3 cabo = Vector3.Cross(ab, ao); |
173 | if (cabo.LengthSquared() > _gjkSquareInSimplexEpsilon) | 173 | if (cabo.LengthSquared() > _gjkSquareInSimplexEpsilon) |
174 | { _ray = Vector3.Cross(cabo, ab); } | 174 | { _ray = Vector3.Cross(cabo, ab); } |
175 | else | 175 | else |
176 | { return true; } | 176 | { return true; } |
177 | } | 177 | } |
178 | else | 178 | else |
179 | { | 179 | { |
180 | _order = 0; | 180 | _order = 0; |
181 | _simplex[0].Ray = _simplex[1].Ray; | 181 | _simplex[0].Ray = _simplex[1].Ray; |
182 | _simplex[0].Vertice = _simplex[1].Vertice; | 182 | _simplex[0].Vertice = _simplex[1].Vertice; |
183 | 183 | ||
184 | _ray = ao; | 184 | _ray = ao; |
185 | } | 185 | } |
186 | return false; | 186 | return false; |
187 | } | 187 | } |
188 | 188 | ||
189 | public bool SolveSimplex3(Vector3 ao, Vector3 ab, Vector3 ac) | 189 | public bool SolveSimplex3(Vector3 ao, Vector3 ab, Vector3 ac) |
190 | { | 190 | { |
191 | return (SolveSimplex3a(ao, ab, ac, Vector3.Cross(ab, ac))); | 191 | return (SolveSimplex3a(ao, ab, ac, Vector3.Cross(ab, ac))); |
192 | } | 192 | } |
193 | 193 | ||
194 | public bool SolveSimplex3a(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 cabc) | 194 | public bool SolveSimplex3a(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 cabc) |
195 | { | 195 | { |
196 | if ((Vector3.Dot(Vector3.Cross(cabc, ab), ao)) < -_gjkInSimplexEpsilon) | 196 | if ((Vector3.Dot(Vector3.Cross(cabc, ab), ao)) < -_gjkInSimplexEpsilon) |
197 | { | 197 | { |
198 | _order = 1; | 198 | _order = 1; |
199 | _simplex[0].Vertice = _simplex[1].Vertice; | 199 | _simplex[0].Vertice = _simplex[1].Vertice; |
200 | _simplex[0].Ray = _simplex[1].Ray; | 200 | _simplex[0].Ray = _simplex[1].Ray; |
201 | 201 | ||
202 | _simplex[1].Vertice = _simplex[2].Vertice; | 202 | _simplex[1].Vertice = _simplex[2].Vertice; |
203 | _simplex[1].Ray = _simplex[2].Ray; | 203 | _simplex[1].Ray = _simplex[2].Ray; |
204 | 204 | ||
205 | return (SolveSimplex2(ao, ab)); | 205 | return (SolveSimplex2(ao, ab)); |
206 | } | 206 | } |
207 | else if (Vector3.Dot(Vector3.Cross(cabc, ac), ao) > +_gjkInSimplexEpsilon) | 207 | else if (Vector3.Dot(Vector3.Cross(cabc, ac), ao) > +_gjkInSimplexEpsilon) |
208 | { | 208 | { |
209 | _order = 1; | 209 | _order = 1; |
210 | _simplex[1].Vertice = _simplex[2].Vertice; | 210 | _simplex[1].Vertice = _simplex[2].Vertice; |
211 | _simplex[1].Ray = _simplex[2].Ray; | 211 | _simplex[1].Ray = _simplex[2].Ray; |
212 | 212 | ||
213 | return (SolveSimplex2(ao, ac)); | 213 | return (SolveSimplex2(ao, ac)); |
214 | } | 214 | } |
215 | else | 215 | else |
216 | { | 216 | { |
217 | float d = Vector3.Dot(cabc, ao); | 217 | float d = Vector3.Dot(cabc, ao); |
218 | if (Abs(d) > _gjkInSimplexEpsilon) | 218 | if (Abs(d) > _gjkInSimplexEpsilon) |
219 | { | 219 | { |
220 | if (d > 0) | 220 | if (d > 0) |
221 | { _ray = cabc; } | 221 | { _ray = cabc; } |
222 | else | 222 | else |
223 | { _ray = -cabc; Swap<MinkowskiVertice>(ref _simplex[0], ref _simplex[1]); } | 223 | { _ray = -cabc; Swap<MinkowskiVertice>(ref _simplex[0], ref _simplex[1]); } |
224 | return (false); | 224 | return (false); |
225 | } | 225 | } |
226 | else return (true); | 226 | else return (true); |
227 | } | 227 | } |
228 | } | 228 | } |
229 | 229 | ||
230 | public bool SolveSimplex4(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 ad) | 230 | public bool SolveSimplex4(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 ad) |
231 | { | 231 | { |
232 | Vector3 crs; | 232 | Vector3 crs; |
233 | if (Vector3.Dot((crs = Vector3.Cross(ab, ac)), ao) > _gjkInSimplexEpsilon) | 233 | if (Vector3.Dot((crs = Vector3.Cross(ab, ac)), ao) > _gjkInSimplexEpsilon) |
234 | { | 234 | { |
235 | _order = 2; | 235 | _order = 2; |
236 | _simplex[0].Vertice = _simplex[1].Vertice; | 236 | _simplex[0].Vertice = _simplex[1].Vertice; |
237 | _simplex[0].Ray = _simplex[1].Ray; | 237 | _simplex[0].Ray = _simplex[1].Ray; |
238 | 238 | ||
239 | _simplex[1].Vertice = _simplex[2].Vertice; | 239 | _simplex[1].Vertice = _simplex[2].Vertice; |
240 | _simplex[1].Ray = _simplex[2].Ray; | 240 | _simplex[1].Ray = _simplex[2].Ray; |
241 | 241 | ||
242 | _simplex[2].Vertice = _simplex[3].Vertice; | 242 | _simplex[2].Vertice = _simplex[3].Vertice; |
243 | _simplex[2].Ray = _simplex[3].Ray; | 243 | _simplex[2].Ray = _simplex[3].Ray; |
244 | 244 | ||
245 | return (SolveSimplex3a(ao, ab, ac, crs)); | 245 | return (SolveSimplex3a(ao, ab, ac, crs)); |
246 | } | 246 | } |
247 | else if (Vector3.Dot((crs = Vector3.Cross(ac, ad)), ao) > _gjkInSimplexEpsilon) | 247 | else if (Vector3.Dot((crs = Vector3.Cross(ac, ad)), ao) > _gjkInSimplexEpsilon) |
248 | { | 248 | { |
249 | _order = 2; | 249 | _order = 2; |
250 | _simplex[2].Vertice = _simplex[3].Vertice; | 250 | _simplex[2].Vertice = _simplex[3].Vertice; |
251 | _simplex[2].Ray = _simplex[3].Ray; | 251 | _simplex[2].Ray = _simplex[3].Ray; |
252 | 252 | ||
253 | return (SolveSimplex3a(ao, ac, ad, crs)); | 253 | return (SolveSimplex3a(ao, ac, ad, crs)); |
254 | } | 254 | } |
255 | else if (Vector3.Dot((crs = Vector3.Cross(ad, ab)), ao) > _gjkInSimplexEpsilon) | 255 | else if (Vector3.Dot((crs = Vector3.Cross(ad, ab)), ao) > _gjkInSimplexEpsilon) |
256 | { | 256 | { |
257 | _order = 2; | 257 | _order = 2; |
258 | 258 | ||
259 | _simplex[1].Vertice = _simplex[0].Vertice; | 259 | _simplex[1].Vertice = _simplex[0].Vertice; |
260 | _simplex[1].Ray = _simplex[0].Ray; | 260 | _simplex[1].Ray = _simplex[0].Ray; |
261 | 261 | ||
262 | _simplex[0].Vertice = _simplex[2].Vertice; | 262 | _simplex[0].Vertice = _simplex[2].Vertice; |
263 | _simplex[0].Ray = _simplex[2].Ray; | 263 | _simplex[0].Ray = _simplex[2].Ray; |
264 | 264 | ||
265 | _simplex[2].Vertice = _simplex[3].Vertice; | 265 | _simplex[2].Vertice = _simplex[3].Vertice; |
266 | _simplex[2].Ray = _simplex[3].Ray; | 266 | _simplex[2].Ray = _simplex[3].Ray; |
267 | 267 | ||
268 | return (SolveSimplex3a(ao, ad, ab, crs)); | 268 | return (SolveSimplex3a(ao, ad, ab, crs)); |
269 | } | 269 | } |
270 | else return (true); | 270 | else return (true); |
271 | } | 271 | } |
272 | 272 | ||
273 | public bool SearchOrigin() | 273 | public bool SearchOrigin() |
274 | { | 274 | { |
275 | return SearchOrigin(new Vector3(1, 0, 0)); | 275 | return SearchOrigin(new Vector3(1, 0, 0)); |
276 | } | 276 | } |
277 | 277 | ||
278 | public bool SearchOrigin(Vector3 initray) | 278 | public bool SearchOrigin(Vector3 initray) |
279 | { | 279 | { |
280 | _iterations = 0; | 280 | _iterations = 0; |
281 | unchecked | 281 | unchecked |
282 | { | 282 | { |
283 | _order = (int)(-1); | 283 | _order = (int)(-1); |
284 | } | 284 | } |
285 | _failed = false; | 285 | _failed = false; |
286 | _ray = Vector3.Normalize(initray); | 286 | _ray = Vector3.Normalize(initray); |
287 | 287 | ||
288 | //ClearMemory(table, sizeof(void*) * GJK_hashsize); | 288 | //ClearMemory(table, sizeof(void*) * GJK_hashsize); |
289 | for (int i = 0; i < _table.Length; i++) | 289 | for (int i = 0; i < _table.Length; i++) |
290 | _table[i] = null; | 290 | _table[i] = null; |
291 | FetchSupport(); | 291 | FetchSupport(); |
292 | _ray = -_simplex[0].Vertice; | 292 | _ray = -_simplex[0].Vertice; |
293 | for (; _iterations < _gjkMaxIterations; ++_iterations) | 293 | for (; _iterations < _gjkMaxIterations; ++_iterations) |
294 | { | 294 | { |
295 | float rl = _ray.Length(); | 295 | float rl = _ray.Length(); |
296 | _ray /= rl > 0 ? rl : 1; | 296 | _ray /= rl > 0 ? rl : 1; |
297 | if (FetchSupport()) | 297 | if (FetchSupport()) |
298 | { | 298 | { |
299 | bool found = (false); | 299 | bool found = (false); |
300 | switch (_order) | 300 | switch (_order) |
301 | { | 301 | { |
302 | case 1: found = SolveSimplex2(-_simplex[1].Vertice, _simplex[0].Vertice - _simplex[1].Vertice); break; | 302 | case 1: found = SolveSimplex2(-_simplex[1].Vertice, _simplex[0].Vertice - _simplex[1].Vertice); break; |
303 | case 2: found = SolveSimplex3(-_simplex[2].Vertice, _simplex[1].Vertice - _simplex[2].Vertice, _simplex[0].Vertice - _simplex[2].Vertice); break; | 303 | case 2: found = SolveSimplex3(-_simplex[2].Vertice, _simplex[1].Vertice - _simplex[2].Vertice, _simplex[0].Vertice - _simplex[2].Vertice); break; |
304 | case 3: found = SolveSimplex4(-_simplex[3].Vertice, _simplex[2].Vertice - _simplex[3].Vertice, _simplex[1].Vertice - _simplex[3].Vertice, _simplex[0].Vertice - _simplex[3].Vertice); break; | 304 | case 3: found = SolveSimplex4(-_simplex[3].Vertice, _simplex[2].Vertice - _simplex[3].Vertice, _simplex[1].Vertice - _simplex[3].Vertice, _simplex[0].Vertice - _simplex[3].Vertice); break; |
305 | } | 305 | } |
306 | if (found) return (true); | 306 | if (found) return (true); |
307 | } | 307 | } |
308 | else return (false); | 308 | else return (false); |
309 | } | 309 | } |
310 | _failed = true; | 310 | _failed = true; |
311 | return (false); | 311 | return (false); |
312 | } | 312 | } |
313 | 313 | ||
314 | public bool EncloseOrigin() | 314 | public bool EncloseOrigin() |
315 | { | 315 | { |
316 | switch (_order) | 316 | switch (_order) |
317 | { | 317 | { |
318 | /* Point */ | 318 | /* Point */ |
319 | case 0: break; | 319 | case 0: break; |
320 | /* Line */ | 320 | /* Line */ |
321 | case 1: | 321 | case 1: |
322 | Vector3 ab = _simplex[1].Vertice - _simplex[0].Vertice; | 322 | Vector3 ab = _simplex[1].Vertice - _simplex[0].Vertice; |
323 | Vector3[] b ={ Vector3.Cross(ab, new Vector3(1, 0, 0)), | 323 | Vector3[] b ={ Vector3.Cross(ab, new Vector3(1, 0, 0)), |
324 | Vector3.Cross(ab, new Vector3(0, 1, 0)), | 324 | Vector3.Cross(ab, new Vector3(0, 1, 0)), |
325 | Vector3.Cross(ab, new Vector3(0, 0, 1)) }; | 325 | Vector3.Cross(ab, new Vector3(0, 0, 1)) }; |
326 | float[] m ={ b[0].LengthSquared(), b[1].LengthSquared(), b[2].LengthSquared() }; | 326 | float[] m ={ b[0].LengthSquared(), b[1].LengthSquared(), b[2].LengthSquared() }; |
327 | Matrix r = Matrix.CreateFromQuaternion(new Quaternion(Vector3.Normalize(ab), _twoPi / 3)); | 327 | Matrix r = Matrix.CreateFromQuaternion(new Quaternion(Vector3.Normalize(ab), _twoPi / 3)); |
328 | Vector3 w = b[m[0] > m[1] ? m[0] > m[2] ? 0 : 2 : m[1] > m[2] ? 1 : 2]; | 328 | Vector3 w = b[m[0] > m[1] ? m[0] > m[2] ? 0 : 2 : m[1] > m[2] ? 1 : 2]; |
329 | Support(Vector3.Normalize(w), ref _simplex[4]); w = Vector3.TransformNormal(w, r); | 329 | Support(Vector3.Normalize(w), ref _simplex[4]); w = Vector3.TransformNormal(w, r); |
330 | Support(Vector3.Normalize(w), ref _simplex[2]); w = Vector3.TransformNormal(w, r); | 330 | Support(Vector3.Normalize(w), ref _simplex[2]); w = Vector3.TransformNormal(w, r); |
331 | Support(Vector3.Normalize(w), ref _simplex[3]); w = Vector3.TransformNormal(w, r); | 331 | Support(Vector3.Normalize(w), ref _simplex[3]); w = Vector3.TransformNormal(w, r); |
332 | _order = 4; | 332 | _order = 4; |
333 | return true; | 333 | return true; |
334 | /* Triangle */ | 334 | /* Triangle */ |
335 | case 2: | 335 | case 2: |
336 | Vector3 n = Vector3.Normalize(Vector3.Cross(_simplex[1].Vertice - _simplex[0].Vertice, _simplex[2].Vertice - _simplex[0].Vertice)); | 336 | Vector3 n = Vector3.Normalize(Vector3.Cross(_simplex[1].Vertice - _simplex[0].Vertice, _simplex[2].Vertice - _simplex[0].Vertice)); |
337 | Support(n, ref _simplex[3]); | 337 | Support(n, ref _simplex[3]); |
338 | Support(-n, ref _simplex[4]); | 338 | Support(-n, ref _simplex[4]); |
339 | _order = 4; | 339 | _order = 4; |
340 | return true; | 340 | return true; |
341 | /* Tetrahedron */ | 341 | /* Tetrahedron */ |
342 | case 3: return (true); | 342 | case 3: return (true); |
343 | /* Hexahedron */ | 343 | /* Hexahedron */ |
344 | case 4: return (true); | 344 | case 4: return (true); |
345 | } | 345 | } |
346 | return (false); | 346 | return (false); |
347 | } | 347 | } |
348 | } | 348 | } |
349 | 349 | ||
350 | public class Epa | 350 | public class Epa |
351 | { | 351 | { |
352 | public class Face | 352 | public class Face |
353 | { | 353 | { |
354 | public Gjk.MinkowskiVertice[] _vertices = new Gjk.MinkowskiVertice[3]; | 354 | public Gjk.MinkowskiVertice[] _vertices = new Gjk.MinkowskiVertice[3]; |
355 | public Face[] _faces = new Face[3]; | 355 | public Face[] _faces = new Face[3]; |
356 | public int[] _e = new int[3]; | 356 | public int[] _e = new int[3]; |
357 | public Vector3 _n; | 357 | public Vector3 _n; |
358 | public float _d; | 358 | public float _d; |
359 | public int _mark; | 359 | public int _mark; |
360 | public Face _prev; | 360 | public Face _prev; |
361 | public Face _next; | 361 | public Face _next; |
362 | } | 362 | } |
363 | 363 | ||
364 | private Gjk _gjk; | 364 | private Gjk _gjk; |
365 | private Face _root; | 365 | private Face _root; |
366 | private int _nfaces; | 366 | private int _nfaces; |
367 | private int _iterations; | 367 | private int _iterations; |
368 | private Vector3[,] _features = new Vector3[2, 3]; | 368 | private Vector3[,] _features = new Vector3[2, 3]; |
369 | private Vector3[] _nearest = new Vector3[2]; | 369 | private Vector3[] _nearest = new Vector3[2]; |
370 | private Vector3 _normal; | 370 | private Vector3 _normal; |
371 | private float _depth; | 371 | private float _depth; |
372 | private bool _failed; | 372 | private bool _failed; |
373 | 373 | ||
374 | public Epa(Gjk gjk) | 374 | public Epa(Gjk gjk) |
375 | { | 375 | { |
376 | this._gjk = gjk; | 376 | this._gjk = gjk; |
377 | } | 377 | } |
378 | 378 | ||
379 | public bool Failed { get { return _failed; } } | 379 | public bool Failed { get { return _failed; } } |
380 | public int Iterations { get { return _iterations; } } | 380 | public int Iterations { get { return _iterations; } } |
381 | public Vector3 Normal { get { return _normal; } } | 381 | public Vector3 Normal { get { return _normal; } } |
382 | public Vector3[] Nearest { get { return _nearest; } } | 382 | public Vector3[] Nearest { get { return _nearest; } } |
383 | 383 | ||
384 | public Vector3 GetCoordinates(Face face) | 384 | public Vector3 GetCoordinates(Face face) |
385 | { | 385 | { |
386 | Vector3 o = face._n * -face._d; | 386 | Vector3 o = face._n * -face._d; |
387 | float[] a ={ Vector3.Cross(face._vertices[0].Vertice - o, face._vertices[1].Vertice - o).Length(), | 387 | float[] a ={ Vector3.Cross(face._vertices[0].Vertice - o, face._vertices[1].Vertice - o).Length(), |
388 | Vector3.Cross(face._vertices[1].Vertice - o, face._vertices[2].Vertice - o).Length(), | 388 | Vector3.Cross(face._vertices[1].Vertice - o, face._vertices[2].Vertice - o).Length(), |
389 | Vector3.Cross(face._vertices[2].Vertice - o, face._vertices[0].Vertice - o).Length()}; | 389 | Vector3.Cross(face._vertices[2].Vertice - o, face._vertices[0].Vertice - o).Length()}; |
390 | float sm = a[0] + a[1] + a[2]; | 390 | float sm = a[0] + a[1] + a[2]; |
391 | return (new Vector3(a[1], a[2], a[0]) / (sm > 0 ? sm : 1)); | 391 | return (new Vector3(a[1], a[2], a[0]) / (sm > 0 ? sm : 1)); |
392 | } | 392 | } |
393 | 393 | ||
394 | public Face FindBest() | 394 | public Face FindBest() |
395 | { | 395 | { |
396 | Face bf = null; | 396 | Face bf = null; |
397 | if (_root != null) | 397 | if (_root != null) |
398 | { | 398 | { |
399 | Face cf = _root; | 399 | Face cf = _root; |
400 | float bd = _infinity; | 400 | float bd = _infinity; |
401 | do | 401 | do |
402 | { | 402 | { |
403 | if (cf._d < bd) { bd = cf._d; bf = cf; } | 403 | if (cf._d < bd) { bd = cf._d; bf = cf; } |
404 | } while (null != (cf = cf._next)); | 404 | } while (null != (cf = cf._next)); |
405 | } | 405 | } |
406 | return bf; | 406 | return bf; |
407 | } | 407 | } |
408 | 408 | ||
409 | public bool Set(ref Face f, Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) | 409 | public bool Set(ref Face f, Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) |
410 | { | 410 | { |
411 | Vector3 nrm = Vector3.Cross(b.Vertice - a.Vertice, c.Vertice - a.Vertice); | 411 | Vector3 nrm = Vector3.Cross(b.Vertice - a.Vertice, c.Vertice - a.Vertice); |
412 | float len = nrm.Length(); | 412 | float len = nrm.Length(); |
413 | bool valid = (Vector3.Dot(Vector3.Cross(a.Vertice, b.Vertice), nrm) >= -_epaInFaceEpsilon && | 413 | bool valid = (Vector3.Dot(Vector3.Cross(a.Vertice, b.Vertice), nrm) >= -_epaInFaceEpsilon && |
414 | Vector3.Dot(Vector3.Cross(b.Vertice, c.Vertice), nrm) >= -_epaInFaceEpsilon && | 414 | Vector3.Dot(Vector3.Cross(b.Vertice, c.Vertice), nrm) >= -_epaInFaceEpsilon && |
415 | Vector3.Dot(Vector3.Cross(c.Vertice, a.Vertice), nrm) >= -_epaInFaceEpsilon); | 415 | Vector3.Dot(Vector3.Cross(c.Vertice, a.Vertice), nrm) >= -_epaInFaceEpsilon); |
416 | f._vertices[0] = a; | 416 | f._vertices[0] = a; |
417 | f._vertices[1] = b; | 417 | f._vertices[1] = b; |
418 | f._vertices[2] = c; | 418 | f._vertices[2] = c; |
419 | f._mark = 0; | 419 | f._mark = 0; |
420 | f._n = nrm / (len > 0 ? len : _infinity); | 420 | f._n = nrm / (len > 0 ? len : _infinity); |
421 | f._d = Max(0, -Vector3.Dot(f._n, a.Vertice)); | 421 | f._d = Max(0, -Vector3.Dot(f._n, a.Vertice)); |
422 | return valid; | 422 | return valid; |
423 | } | 423 | } |
424 | 424 | ||
425 | public Face NewFace(Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) | 425 | public Face NewFace(Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) |
426 | { | 426 | { |
427 | Face pf = new Face(); | 427 | Face pf = new Face(); |
428 | if (Set(ref pf, a, b, c)) | 428 | if (Set(ref pf, a, b, c)) |
429 | { | 429 | { |
430 | if (_root != null) _root._prev = pf; | 430 | if (_root != null) _root._prev = pf; |
431 | pf._prev = null; | 431 | pf._prev = null; |
432 | pf._next = _root; | 432 | pf._next = _root; |
433 | _root = pf; | 433 | _root = pf; |
434 | ++_nfaces; | 434 | ++_nfaces; |
435 | } | 435 | } |
436 | else | 436 | else |
437 | { | 437 | { |
438 | pf._prev = pf._next = null; | 438 | pf._prev = pf._next = null; |
439 | } | 439 | } |
440 | return (pf); | 440 | return (pf); |
441 | } | 441 | } |
442 | 442 | ||
443 | public void Detach(ref Face face) | 443 | public void Detach(ref Face face) |
444 | { | 444 | { |
445 | if (face._prev != null || face._next != null) | 445 | if (face._prev != null || face._next != null) |
446 | { | 446 | { |
447 | --_nfaces; | 447 | --_nfaces; |
448 | if (face == _root) | 448 | if (face == _root) |
449 | { | 449 | { |
450 | _root = face._next; | 450 | _root = face._next; |
451 | _root._prev = null; | 451 | _root._prev = null; |
452 | } | 452 | } |
453 | else | 453 | else |
454 | { | 454 | { |
455 | if (face._next == null) | 455 | if (face._next == null) |
456 | { | 456 | { |
457 | face._prev._next = null; | 457 | face._prev._next = null; |
458 | } | 458 | } |
459 | else | 459 | else |
460 | { | 460 | { |
461 | face._prev._next = face._next; | 461 | face._prev._next = face._next; |
462 | face._next._prev = face._prev; | 462 | face._next._prev = face._prev; |
463 | } | 463 | } |
464 | } | 464 | } |
465 | face._prev = face._next = null; | 465 | face._prev = face._next = null; |
466 | } | 466 | } |
467 | } | 467 | } |
468 | 468 | ||
469 | public void Link(ref Face f0, int e0, ref Face f1, int e1) | 469 | public void Link(ref Face f0, int e0, ref Face f1, int e1) |
470 | { | 470 | { |
471 | f0._faces[e0] = f1; f1._e[e1] = e0; | 471 | f0._faces[e0] = f1; f1._e[e1] = e0; |
472 | f1._faces[e1] = f0; f0._e[e0] = e1; | 472 | f1._faces[e1] = f0; f0._e[e0] = e1; |
473 | } | 473 | } |
474 | 474 | ||
475 | public Gjk.MinkowskiVertice Support(Vector3 w) | 475 | public Gjk.MinkowskiVertice Support(Vector3 w) |
476 | { | 476 | { |
477 | Gjk.MinkowskiVertice v = new Gjk.MinkowskiVertice(); | 477 | Gjk.MinkowskiVertice v = new Gjk.MinkowskiVertice(); |
478 | _gjk.Support(w, ref v); | 478 | _gjk.Support(w, ref v); |
479 | return v; | 479 | return v; |
480 | } | 480 | } |
481 | 481 | ||
482 | private static int[] mod3 ={ 0, 1, 2, 0, 1 }; | 482 | private static int[] mod3 ={ 0, 1, 2, 0, 1 }; |
483 | 483 | ||
484 | public int BuildHorizon(int markid, Gjk.MinkowskiVertice w, ref Face f, int e, ref Face cf, ref Face ff) | 484 | public int BuildHorizon(int markid, Gjk.MinkowskiVertice w, ref Face f, int e, ref Face cf, ref Face ff) |
485 | { | 485 | { |
486 | int ne = (0); | 486 | int ne = (0); |
487 | if (f._mark != markid) | 487 | if (f._mark != markid) |
488 | { | 488 | { |
489 | int e1 = (mod3[e + 1]); | 489 | int e1 = (mod3[e + 1]); |
490 | if ((Vector3.Dot(f._n, w.Vertice) + f._d) > 0) | 490 | if ((Vector3.Dot(f._n, w.Vertice) + f._d) > 0) |
491 | { | 491 | { |
492 | Face nf = NewFace(f._vertices[e1], f._vertices[e], w); | 492 | Face nf = NewFace(f._vertices[e1], f._vertices[e], w); |
493 | Link(ref nf, 0, ref f, e); | 493 | Link(ref nf, 0, ref f, e); |
494 | if (cf != null) Link(ref cf, 1, ref nf, 2); else ff = nf; | 494 | if (cf != null) Link(ref cf, 1, ref nf, 2); else ff = nf; |
495 | cf = nf; ne = 1; | 495 | cf = nf; ne = 1; |
496 | } | 496 | } |
497 | else | 497 | else |
498 | { | 498 | { |
499 | int e2 = (mod3[e + 2]); | 499 | int e2 = (mod3[e + 2]); |
500 | Detach(ref f); | 500 | Detach(ref f); |
501 | f._mark = markid; | 501 | f._mark = markid; |
502 | ne += BuildHorizon(markid, w, ref f._faces[e1], f._e[e1], ref cf, ref ff); | 502 | ne += BuildHorizon(markid, w, ref f._faces[e1], f._e[e1], ref cf, ref ff); |
503 | ne += BuildHorizon(markid, w, ref f._faces[e2], f._e[e2], ref cf, ref ff); | 503 | ne += BuildHorizon(markid, w, ref f._faces[e2], f._e[e2], ref cf, ref ff); |
504 | } | 504 | } |
505 | } | 505 | } |
506 | return (ne); | 506 | return (ne); |
507 | } | 507 | } |
508 | 508 | ||
509 | public float EvaluatePD() | 509 | public float EvaluatePD() |
510 | { | 510 | { |
511 | return EvaluatePD(_epaAccuracy); | 511 | return EvaluatePD(_epaAccuracy); |
512 | } | 512 | } |
513 | 513 | ||
514 | private int[,] fidx; | 514 | private int[,] fidx; |
515 | private int[,] eidx; | 515 | private int[,] eidx; |
516 | 516 | ||
517 | public float EvaluatePD(float accuracy) | 517 | public float EvaluatePD(float accuracy) |
518 | { | 518 | { |
519 | //Block* sablock = sa->BeginBlock(); | 519 | //Block* sablock = sa->BeginBlock(); |
520 | Face bestface = null; | 520 | Face bestface = null; |
521 | int markid = 1; | 521 | int markid = 1; |
522 | _depth = -_infinity; | 522 | _depth = -_infinity; |
523 | _normal = new Vector3(); | 523 | _normal = new Vector3(); |
524 | _root = null; | 524 | _root = null; |
525 | _nfaces = 0; | 525 | _nfaces = 0; |
526 | _iterations = 0; | 526 | _iterations = 0; |
527 | _failed = false; | 527 | _failed = false; |
528 | /* Prepare hull */ | 528 | /* Prepare hull */ |
529 | if (_gjk.EncloseOrigin()) | 529 | if (_gjk.EncloseOrigin()) |
530 | { | 530 | { |
531 | int nfidx = 0; | 531 | int nfidx = 0; |
532 | int neidx = 0; | 532 | int neidx = 0; |
533 | Gjk.MinkowskiVertice[] basemkv = new Gjk.MinkowskiVertice[5]; | 533 | Gjk.MinkowskiVertice[] basemkv = new Gjk.MinkowskiVertice[5]; |
534 | Face[] basefaces = new Face[6]; | 534 | Face[] basefaces = new Face[6]; |
535 | switch (_gjk.Order) | 535 | switch (_gjk.Order) |
536 | { | 536 | { |
537 | /* Tetrahedron */ | 537 | /* Tetrahedron */ |
538 | case 3: | 538 | case 3: |
539 | { | 539 | { |
540 | fidx = new int[,] { { 2, 1, 0 }, { 3, 0, 1 }, { 3, 1, 2 }, { 3, 2, 0 } }; | 540 | fidx = new int[,] { { 2, 1, 0 }, { 3, 0, 1 }, { 3, 1, 2 }, { 3, 2, 0 } }; |
541 | eidx = new int[,] { { 0, 0, 2, 1 }, { 0, 1, 1, 1 }, { 0, 2, 3, 1 }, { 1, 0, 3, 2 }, { 2, 0, 1, 2 }, { 3, 0, 2, 2 } }; | 541 | eidx = new int[,] { { 0, 0, 2, 1 }, { 0, 1, 1, 1 }, { 0, 2, 3, 1 }, { 1, 0, 3, 2 }, { 2, 0, 1, 2 }, { 3, 0, 2, 2 } }; |
542 | nfidx = 4; neidx = 6; | 542 | nfidx = 4; neidx = 6; |
543 | } break; | 543 | } break; |
544 | /* Hexahedron */ | 544 | /* Hexahedron */ |
545 | case 4: | 545 | case 4: |
546 | { | 546 | { |
547 | fidx = new int[,] { { 2, 0, 4 }, { 4, 1, 2 }, { 1, 4, 0 }, { 0, 3, 1 }, { 0, 2, 3 }, { 1, 3, 2 } }; | 547 | fidx = new int[,] { { 2, 0, 4 }, { 4, 1, 2 }, { 1, 4, 0 }, { 0, 3, 1 }, { 0, 2, 3 }, { 1, 3, 2 } }; |
548 | eidx = new int[,] { { 0, 0, 4, 0 }, { 0, 1, 2, 1 }, { 0, 2, 1, 2 }, { 1, 1, 5, 2 }, { 1, 0, 2, 0 }, { 2, 2, 3, 2 }, { 3, 1, 5, 0 }, { 3, 0, 4, 2 }, { 5, 1, 4, 1 } }; | 548 | eidx = new int[,] { { 0, 0, 4, 0 }, { 0, 1, 2, 1 }, { 0, 2, 1, 2 }, { 1, 1, 5, 2 }, { 1, 0, 2, 0 }, { 2, 2, 3, 2 }, { 3, 1, 5, 0 }, { 3, 0, 4, 2 }, { 5, 1, 4, 1 } }; |
549 | nfidx = 6; neidx = 9; | 549 | nfidx = 6; neidx = 9; |
550 | } break; | 550 | } break; |
551 | } | 551 | } |
552 | int i; | 552 | int i; |
553 | 553 | ||
554 | for (i = 0; i <= _gjk.Order; ++i) | 554 | for (i = 0; i <= _gjk.Order; ++i) |
555 | { | 555 | { |
556 | //basemkv[i] = (GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv)); | 556 | //basemkv[i] = (GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv)); |
557 | basemkv[i] = new Gjk.MinkowskiVertice(); | 557 | basemkv[i] = new Gjk.MinkowskiVertice(); |
558 | basemkv[i].Vertice = _gjk.Simplex[i].Vertice; | 558 | basemkv[i].Vertice = _gjk.Simplex[i].Vertice; |
559 | basemkv[i].Ray = _gjk.Simplex[i].Ray; | 559 | basemkv[i].Ray = _gjk.Simplex[i].Ray; |
560 | } | 560 | } |
561 | for (i = 0; i < nfidx; ++i) | 561 | for (i = 0; i < nfidx; ++i) |
562 | { | 562 | { |
563 | basefaces[i] = NewFace(basemkv[fidx[i, 0]], basemkv[fidx[i, 1]], basemkv[fidx[i, 2]]); | 563 | basefaces[i] = NewFace(basemkv[fidx[i, 0]], basemkv[fidx[i, 1]], basemkv[fidx[i, 2]]); |
564 | } | 564 | } |
565 | for (i = 0; i < neidx; ++i) | 565 | for (i = 0; i < neidx; ++i) |
566 | { | 566 | { |
567 | Link(ref basefaces[eidx[i, 0]], eidx[i, 1], ref basefaces[eidx[i, 2]], eidx[i, 3]); | 567 | Link(ref basefaces[eidx[i, 0]], eidx[i, 1], ref basefaces[eidx[i, 2]], eidx[i, 3]); |
568 | } | 568 | } |
569 | } | 569 | } |
570 | if (0 == _nfaces) | 570 | if (0 == _nfaces) |
571 | { | 571 | { |
572 | return _depth; | 572 | return _depth; |
573 | } | 573 | } |
574 | /* Expand hull */ | 574 | /* Expand hull */ |
575 | for (; _iterations < _epaMaxIterations; ++_iterations) | 575 | for (; _iterations < _epaMaxIterations; ++_iterations) |
576 | { | 576 | { |
577 | Face bf = FindBest(); | 577 | Face bf = FindBest(); |
578 | if (bf != null) | 578 | if (bf != null) |
579 | { | 579 | { |
580 | Gjk.MinkowskiVertice w = Support(-bf._n); | 580 | Gjk.MinkowskiVertice w = Support(-bf._n); |
581 | float d = Vector3.Dot(bf._n, w.Vertice) + bf._d; | 581 | float d = Vector3.Dot(bf._n, w.Vertice) + bf._d; |
582 | bestface = bf; | 582 | bestface = bf; |
583 | if (d < -accuracy) | 583 | if (d < -accuracy) |
584 | { | 584 | { |
585 | Face cf = null; | 585 | Face cf = null; |
586 | Face ff = null; | 586 | Face ff = null; |
587 | int nf = 0; | 587 | int nf = 0; |
588 | Detach(ref bf); | 588 | Detach(ref bf); |
589 | bf._mark = ++markid; | 589 | bf._mark = ++markid; |
590 | for (int i = 0; i < 3; ++i) | 590 | for (int i = 0; i < 3; ++i) |
591 | { | 591 | { |
592 | nf += BuildHorizon(markid, w, ref bf._faces[i], bf._e[i], ref cf, ref ff); | 592 | nf += BuildHorizon(markid, w, ref bf._faces[i], bf._e[i], ref cf, ref ff); |
593 | } | 593 | } |
594 | if (nf <= 2) { break; } | 594 | if (nf <= 2) { break; } |
595 | Link(ref cf, 1, ref ff, 2); | 595 | Link(ref cf, 1, ref ff, 2); |
596 | } | 596 | } |
597 | else break; | 597 | else break; |
598 | } | 598 | } |
599 | else break; | 599 | else break; |
600 | } | 600 | } |
601 | /* Extract contact */ | 601 | /* Extract contact */ |
602 | if (bestface != null) | 602 | if (bestface != null) |
603 | { | 603 | { |
604 | Vector3 b = GetCoordinates(bestface); | 604 | Vector3 b = GetCoordinates(bestface); |
605 | _normal = bestface._n; | 605 | _normal = bestface._n; |
606 | _depth = Max(0, bestface._d); | 606 | _depth = Max(0, bestface._d); |
607 | for (int i = 0; i < 2; ++i) | 607 | for (int i = 0; i < 2; ++i) |
608 | { | 608 | { |
609 | float s = i != 0 ? -1 : 1; | 609 | float s = i != 0 ? -1 : 1; |
610 | for (int j = 0; j < 3; ++j) | 610 | for (int j = 0; j < 3; ++j) |
611 | { | 611 | { |
612 | _features[i, j] = _gjk.LocalSupport(s * bestface._vertices[j].Ray, i); | 612 | _features[i, j] = _gjk.LocalSupport(s * bestface._vertices[j].Ray, i); |
613 | } | 613 | } |
614 | } | 614 | } |
615 | _nearest[0] = _features[0, 0] * b.X + _features[0, 1] * b.Y + _features[0, 2] * b.Z; | 615 | _nearest[0] = _features[0, 0] * b.X + _features[0, 1] * b.Y + _features[0, 2] * b.Z; |
616 | _nearest[1] = _features[1, 0] * b.X + _features[1, 1] * b.Y + _features[1, 2] * b.Z; | 616 | _nearest[1] = _features[1, 0] * b.X + _features[1, 1] * b.Y + _features[1, 2] * b.Z; |
617 | } | 617 | } |
618 | else _failed = true; | 618 | else _failed = true; |
619 | return _depth; | 619 | return _depth; |
620 | } | 620 | } |
621 | 621 | ||
622 | private float Max(float a, float b) | 622 | private float Max(float a, float b) |
623 | { | 623 | { |
624 | return (a > b ? a : b); | 624 | return (a > b ? a : b); |
625 | } | 625 | } |
626 | 626 | ||
627 | private float Min(float a, float b) | 627 | private float Min(float a, float b) |
628 | { | 628 | { |
629 | return (a < b ? a : b); | 629 | return (a < b ? a : b); |
630 | } | 630 | } |
631 | } | 631 | } |
632 | } | 632 | } |
633 | } | 633 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs index 739b4fc..551449f 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs | |||
@@ -1,56 +1,56 @@ | |||
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 | /// EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to | 30 | /// EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to |
31 | /// calculate the penetration depth between two convex shapes. | 31 | /// calculate the penetration depth between two convex shapes. |
32 | /// </summary> | 32 | /// </summary> |
33 | public class GjkEpaPenetrationDepthSolver : IConvexPenetrationDepthSolver | 33 | public class GjkEpaPenetrationDepthSolver : IConvexPenetrationDepthSolver |
34 | { | 34 | { |
35 | public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, ConvexShape convexA, ConvexShape convexB, Matrix transformA, Matrix transformB, Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, IDebugDraw debugDraw) | 35 | public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, ConvexShape convexA, ConvexShape convexB, Matrix transformA, Matrix transformB, Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, IDebugDraw debugDraw) |
36 | { | 36 | { |
37 | float radialmargin = 0; | 37 | float radialmargin = 0; |
38 | 38 | ||
39 | GjkEpaSolver.Results results; | 39 | GjkEpaSolver.Results results; |
40 | if (GjkEpaSolver.Collide(convexA, transformA, | 40 | if (GjkEpaSolver.Collide(convexA, transformA, |
41 | convexB, transformB, | 41 | convexB, transformB, |
42 | radialmargin, out results)) | 42 | radialmargin, out results)) |
43 | { | 43 | { |
44 | // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); | 44 | // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); |
45 | //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); | 45 | //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); |
46 | ptrA = results.Witnesses[0]; | 46 | ptrA = results.Witnesses[0]; |
47 | ptrB = results.Witnesses[1]; | 47 | ptrB = results.Witnesses[1]; |
48 | return true; | 48 | return true; |
49 | } | 49 | } |
50 | ptrA = new Vector3(); | 50 | ptrA = new Vector3(); |
51 | ptrB = new Vector3(); | 51 | ptrB = new Vector3(); |
52 | 52 | ||
53 | return false; | 53 | return false; |
54 | } | 54 | } |
55 | } | 55 | } |
56 | } | 56 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs index aa9d61e..20af192 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs | |||
@@ -1,101 +1,101 @@ | |||
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 | /// GjkEpaSolver contributed under zlib by Nathanael Presson | 30 | /// GjkEpaSolver contributed under zlib by Nathanael Presson |
31 | /// </summary> | 31 | /// </summary> |
32 | public class GjkEpaSolver | 32 | public class GjkEpaSolver |
33 | { | 33 | { |
34 | public struct Results | 34 | public struct Results |
35 | { | 35 | { |
36 | public enum Status | 36 | public enum Status |
37 | { | 37 | { |
38 | Separated, /* Shapes doesnt penetrate */ | 38 | Separated, /* Shapes doesnt penetrate */ |
39 | Penetrating, /* Shapes are penetrating */ | 39 | Penetrating, /* Shapes are penetrating */ |
40 | GjkFailed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ | 40 | GjkFailed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ |
41 | EpaFailed, /* EPA phase fail, bigger problem, need to save parameters, and debug */ | 41 | EpaFailed, /* EPA phase fail, bigger problem, need to save parameters, and debug */ |
42 | } | 42 | } |
43 | 43 | ||
44 | private Vector3[] _witnesses; | 44 | private Vector3[] _witnesses; |
45 | private Vector3 _normal; | 45 | private Vector3 _normal; |
46 | private float _depth; | 46 | private float _depth; |
47 | private int _epaIterations; | 47 | private int _epaIterations; |
48 | private int _gjkIterations; | 48 | private int _gjkIterations; |
49 | private Status _status; | 49 | private Status _status; |
50 | 50 | ||
51 | public Vector3[] Witnesses { get { return _witnesses; } set { _witnesses = value; } } | 51 | public Vector3[] Witnesses { get { return _witnesses; } set { _witnesses = value; } } |
52 | public Vector3 Normal { get { return _normal; } set { _normal = value; } } | 52 | public Vector3 Normal { get { return _normal; } set { _normal = value; } } |
53 | public float Depth { get { return _depth; } set { _depth = value; } } | 53 | public float Depth { get { return _depth; } set { _depth = value; } } |
54 | public int EpaIterations { get { return _epaIterations; } set { _epaIterations = value; } } | 54 | public int EpaIterations { get { return _epaIterations; } set { _epaIterations = value; } } |
55 | public int GjkIterations { get { return _gjkIterations; } set { _gjkIterations = value; } } | 55 | public int GjkIterations { get { return _gjkIterations; } set { _gjkIterations = value; } } |
56 | public Status ResultStatus { get { return _status; } set { _status = value; } } | 56 | public Status ResultStatus { get { return _status; } set { _status = value; } } |
57 | } | 57 | } |
58 | 58 | ||
59 | public static bool Collide(ConvexShape shapeA, Matrix wtrsA, | 59 | public static bool Collide(ConvexShape shapeA, Matrix wtrsA, |
60 | ConvexShape shapeB, Matrix wtrsB, | 60 | ConvexShape shapeB, Matrix wtrsB, |
61 | float radialmargin, | 61 | float radialmargin, |
62 | out Results results) | 62 | out Results results) |
63 | { | 63 | { |
64 | /* Initialize */ | 64 | /* Initialize */ |
65 | results = new Results(); | 65 | results = new Results(); |
66 | results.Witnesses = new Vector3[2]; | 66 | results.Witnesses = new Vector3[2]; |
67 | results.Witnesses[0] = | 67 | results.Witnesses[0] = |
68 | results.Witnesses[1] = | 68 | results.Witnesses[1] = |
69 | results.Normal = new Vector3(); | 69 | results.Normal = new Vector3(); |
70 | results.Depth = 0; | 70 | results.Depth = 0; |
71 | results.ResultStatus = Results.Status.Separated; | 71 | results.ResultStatus = Results.Status.Separated; |
72 | results.EpaIterations = 0; | 72 | results.EpaIterations = 0; |
73 | results.GjkIterations = 0; | 73 | results.GjkIterations = 0; |
74 | /* Use GJK to locate origin */ | 74 | /* Use GJK to locate origin */ |
75 | GjkEpa.Gjk gjk = new GjkEpa.Gjk(wtrsA, wtrsA.Translation, shapeA, | 75 | GjkEpa.Gjk gjk = new GjkEpa.Gjk(wtrsA, wtrsA.Translation, shapeA, |
76 | wtrsB, wtrsB.Translation, shapeB, | 76 | wtrsB, wtrsB.Translation, shapeB, |
77 | radialmargin + GjkEpa.EpaAccuracy); | 77 | radialmargin + GjkEpa.EpaAccuracy); |
78 | bool collide = gjk.SearchOrigin(); | 78 | bool collide = gjk.SearchOrigin(); |
79 | results.GjkIterations = gjk.Iterations + 1; | 79 | results.GjkIterations = gjk.Iterations + 1; |
80 | if (collide) | 80 | if (collide) |
81 | { | 81 | { |
82 | /* Then EPA for penetration depth */ | 82 | /* Then EPA for penetration depth */ |
83 | GjkEpa.Epa epa = new GjkEpa.Epa(gjk); | 83 | GjkEpa.Epa epa = new GjkEpa.Epa(gjk); |
84 | float pd = epa.EvaluatePD(); | 84 | float pd = epa.EvaluatePD(); |
85 | results.EpaIterations = epa.Iterations + 1; | 85 | results.EpaIterations = epa.Iterations + 1; |
86 | if (pd > 0) | 86 | if (pd > 0) |
87 | { | 87 | { |
88 | results.ResultStatus = Results.Status.Penetrating; | 88 | results.ResultStatus = Results.Status.Penetrating; |
89 | results.Normal = epa.Normal; | 89 | results.Normal = epa.Normal; |
90 | results.Depth = pd; | 90 | results.Depth = pd; |
91 | results.Witnesses[0] = epa.Nearest[0]; | 91 | results.Witnesses[0] = epa.Nearest[0]; |
92 | results.Witnesses[1] = epa.Nearest[1]; | 92 | results.Witnesses[1] = epa.Nearest[1]; |
93 | return true; | 93 | return true; |
94 | } | 94 | } |
95 | else { if (epa.Failed) results.ResultStatus = Results.Status.EpaFailed; } | 95 | else { if (epa.Failed) results.ResultStatus = Results.Status.EpaFailed; } |
96 | } | 96 | } |
97 | else { if (gjk.Failed) results.ResultStatus = Results.Status.GjkFailed; } | 97 | else { if (gjk.Failed) results.ResultStatus = Results.Status.GjkFailed; } |
98 | return false; | 98 | return false; |
99 | } | 99 | } |
100 | } | 100 | } |
101 | } | 101 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs index 0831ff5..7557ed9 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs | |||
@@ -1,343 +1,343 @@ | |||
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 | public class GjkPairDetector : DiscreteCollisionDetectorInterface | 29 | public class GjkPairDetector : DiscreteCollisionDetectorInterface |
30 | { | 30 | { |
31 | private Vector3 _cachedSeparatingAxis; | 31 | private Vector3 _cachedSeparatingAxis; |
32 | private IConvexPenetrationDepthSolver _penetrationDepthSolver; | 32 | private IConvexPenetrationDepthSolver _penetrationDepthSolver; |
33 | private ISimplexSolver _simplexSolver; | 33 | private ISimplexSolver _simplexSolver; |
34 | private ConvexShape _minkowskiA, _minkowskiB; | 34 | private ConvexShape _minkowskiA, _minkowskiB; |
35 | private bool _ignoreMargin; | 35 | private bool _ignoreMargin; |
36 | 36 | ||
37 | private int _lastUsedMethod; | 37 | private int _lastUsedMethod; |
38 | private int _currentIteration; | 38 | private int _currentIteration; |
39 | private int _degenerateSimplex; | 39 | private int _degenerateSimplex; |
40 | private int _catchDegeneracies; | 40 | private int _catchDegeneracies; |
41 | 41 | ||
42 | private static int _numDeepPenetrationChecks = 0; | 42 | private static int _numDeepPenetrationChecks = 0; |
43 | private static int _numGjkChecks = 0; | 43 | private static int _numGjkChecks = 0; |
44 | 44 | ||
45 | private const float RelativeError2 = 1.0e-6f; | 45 | private const float RelativeError2 = 1.0e-6f; |
46 | 46 | ||
47 | #region Properties | 47 | #region Properties |
48 | public int LastUsedMethod | 48 | public int LastUsedMethod |
49 | { | 49 | { |
50 | get { return _lastUsedMethod; } | 50 | get { return _lastUsedMethod; } |
51 | set { _lastUsedMethod = value; } | 51 | set { _lastUsedMethod = value; } |
52 | } | 52 | } |
53 | 53 | ||
54 | public int CurrentIteration | 54 | public int CurrentIteration |
55 | { | 55 | { |
56 | get { return _currentIteration; } | 56 | get { return _currentIteration; } |
57 | set { _currentIteration = value; } | 57 | set { _currentIteration = value; } |
58 | } | 58 | } |
59 | 59 | ||
60 | public int DegenerateSimplex | 60 | public int DegenerateSimplex |
61 | { | 61 | { |
62 | get { return _degenerateSimplex; } | 62 | get { return _degenerateSimplex; } |
63 | set { _degenerateSimplex = value; } | 63 | set { _degenerateSimplex = value; } |
64 | } | 64 | } |
65 | 65 | ||
66 | public int CatchDegeneracies | 66 | public int CatchDegeneracies |
67 | { | 67 | { |
68 | get { return _catchDegeneracies; } | 68 | get { return _catchDegeneracies; } |
69 | set { _catchDegeneracies = value; } | 69 | set { _catchDegeneracies = value; } |
70 | } | 70 | } |
71 | 71 | ||
72 | public static int DeepPenetrationChecksCount { get { return _numDeepPenetrationChecks; } } | 72 | public static int DeepPenetrationChecksCount { get { return _numDeepPenetrationChecks; } } |
73 | public static int GjkChecksCount { get { return _numGjkChecks; } } | 73 | public static int GjkChecksCount { get { return _numGjkChecks; } } |
74 | #endregion | 74 | #endregion |
75 | 75 | ||
76 | public GjkPairDetector(ConvexShape objectA, ConvexShape objectB, | 76 | public GjkPairDetector(ConvexShape objectA, ConvexShape objectB, |
77 | ISimplexSolver simplexSolver, | 77 | ISimplexSolver simplexSolver, |
78 | IConvexPenetrationDepthSolver penetrationDepthSolver) | 78 | IConvexPenetrationDepthSolver penetrationDepthSolver) |
79 | { | 79 | { |
80 | _cachedSeparatingAxis = new Vector3(0, 0, 1); | 80 | _cachedSeparatingAxis = new Vector3(0, 0, 1); |
81 | 81 | ||
82 | _penetrationDepthSolver = penetrationDepthSolver; | 82 | _penetrationDepthSolver = penetrationDepthSolver; |
83 | _simplexSolver = simplexSolver; | 83 | _simplexSolver = simplexSolver; |
84 | _minkowskiA = objectA; | 84 | _minkowskiA = objectA; |
85 | _minkowskiB = objectB; | 85 | _minkowskiB = objectB; |
86 | _ignoreMargin = false; | 86 | _ignoreMargin = false; |
87 | _lastUsedMethod = -1; | 87 | _lastUsedMethod = -1; |
88 | _catchDegeneracies = 1; | 88 | _catchDegeneracies = 1; |
89 | } | 89 | } |
90 | 90 | ||
91 | public void setMinkowskiA(ConvexShape minkA) | 91 | public void setMinkowskiA(ConvexShape minkA) |
92 | { | 92 | { |
93 | _minkowskiA = minkA; | 93 | _minkowskiA = minkA; |
94 | } | 94 | } |
95 | 95 | ||
96 | public void setMinkowskiB(ConvexShape minkB) | 96 | public void setMinkowskiB(ConvexShape minkB) |
97 | { | 97 | { |
98 | _minkowskiB = minkB; | 98 | _minkowskiB = minkB; |
99 | } | 99 | } |
100 | public void setCachedSeperatingAxis(Vector3 seperatingAxis) | 100 | public void setCachedSeperatingAxis(Vector3 seperatingAxis) |
101 | { | 101 | { |
102 | _cachedSeparatingAxis = seperatingAxis; | 102 | _cachedSeparatingAxis = seperatingAxis; |
103 | } | 103 | } |
104 | 104 | ||
105 | public void setPenetrationDepthSolver(IConvexPenetrationDepthSolver penetrationDepthSolver) | 105 | public void setPenetrationDepthSolver(IConvexPenetrationDepthSolver penetrationDepthSolver) |
106 | { | 106 | { |
107 | this._penetrationDepthSolver = penetrationDepthSolver; | 107 | this._penetrationDepthSolver = penetrationDepthSolver; |
108 | } | 108 | } |
109 | 109 | ||
110 | public void setIgnoreMargin(bool ignoreMargin) | 110 | public void setIgnoreMargin(bool ignoreMargin) |
111 | { | 111 | { |
112 | this._ignoreMargin = ignoreMargin; | 112 | this._ignoreMargin = ignoreMargin; |
113 | } | 113 | } |
114 | 114 | ||
115 | public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw) | 115 | public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw) |
116 | { | 116 | { |
117 | float distance = 0; | 117 | float distance = 0; |
118 | 118 | ||
119 | Vector3 normalInB = new Vector3(); | 119 | Vector3 normalInB = new Vector3(); |
120 | Vector3 pointOnA = new Vector3(), pointOnB = new Vector3(); | 120 | Vector3 pointOnA = new Vector3(), pointOnB = new Vector3(); |
121 | 121 | ||
122 | Matrix localTransA = input.TransformA; | 122 | Matrix localTransA = input.TransformA; |
123 | Matrix localTransB = input.TransformB; | 123 | Matrix localTransB = input.TransformB; |
124 | 124 | ||
125 | Vector3 positionOffset = (localTransA.Translation + localTransB.Translation) * 0.5f; | 125 | Vector3 positionOffset = (localTransA.Translation + localTransB.Translation) * 0.5f; |
126 | localTransA.Translation -= positionOffset; | 126 | localTransA.Translation -= positionOffset; |
127 | localTransB.Translation -= positionOffset; | 127 | localTransB.Translation -= positionOffset; |
128 | 128 | ||
129 | float marginA = _minkowskiA.Margin; | 129 | float marginA = _minkowskiA.Margin; |
130 | float marginB = _minkowskiB.Margin; | 130 | float marginB = _minkowskiB.Margin; |
131 | 131 | ||
132 | _numGjkChecks++; | 132 | _numGjkChecks++; |
133 | 133 | ||
134 | if (_ignoreMargin) | 134 | if (_ignoreMargin) |
135 | { | 135 | { |
136 | marginA = 0; | 136 | marginA = 0; |
137 | marginB = 0; | 137 | marginB = 0; |
138 | } | 138 | } |
139 | 139 | ||
140 | _currentIteration = 0; | 140 | _currentIteration = 0; |
141 | 141 | ||
142 | int gjkMaxIter = 1000; | 142 | int gjkMaxIter = 1000; |
143 | _cachedSeparatingAxis = new Vector3(0, 1, 0); | 143 | _cachedSeparatingAxis = new Vector3(0, 1, 0); |
144 | 144 | ||
145 | bool isValid = false; | 145 | bool isValid = false; |
146 | bool checkSimplex = false; | 146 | bool checkSimplex = false; |
147 | bool checkPenetration = true; | 147 | bool checkPenetration = true; |
148 | _degenerateSimplex = 0; | 148 | _degenerateSimplex = 0; |
149 | 149 | ||
150 | _lastUsedMethod = -1; | 150 | _lastUsedMethod = -1; |
151 | 151 | ||
152 | { | 152 | { |
153 | float squaredDistance = MathHelper.Infinity; | 153 | float squaredDistance = MathHelper.Infinity; |
154 | float delta = 0; | 154 | float delta = 0; |
155 | 155 | ||
156 | float margin = marginA + marginB; | 156 | float margin = marginA + marginB; |
157 | 157 | ||
158 | _simplexSolver.Reset(); | 158 | _simplexSolver.Reset(); |
159 | 159 | ||
160 | while (true) | 160 | while (true) |
161 | { | 161 | { |
162 | Matrix transABasis = input.TransformA; | 162 | Matrix transABasis = input.TransformA; |
163 | transABasis.Translation = Vector3.Zero; | 163 | transABasis.Translation = Vector3.Zero; |
164 | 164 | ||
165 | Matrix transBBasis = input.TransformB; | 165 | Matrix transBBasis = input.TransformB; |
166 | transBBasis.Translation = Vector3.Zero; | 166 | transBBasis.Translation = Vector3.Zero; |
167 | 167 | ||
168 | Vector3 seperatingAxisInA = Vector3.TransformNormal(-_cachedSeparatingAxis, transABasis); | 168 | Vector3 seperatingAxisInA = Vector3.TransformNormal(-_cachedSeparatingAxis, transABasis); |
169 | Vector3 seperatingAxisInB = Vector3.TransformNormal(_cachedSeparatingAxis, transBBasis); | 169 | Vector3 seperatingAxisInB = Vector3.TransformNormal(_cachedSeparatingAxis, transBBasis); |
170 | 170 | ||
171 | Vector3 pInA = _minkowskiA.LocalGetSupportingVertexWithoutMargin(seperatingAxisInA); | 171 | Vector3 pInA = _minkowskiA.LocalGetSupportingVertexWithoutMargin(seperatingAxisInA); |
172 | Vector3 qInB = _minkowskiB.LocalGetSupportingVertexWithoutMargin(seperatingAxisInB); | 172 | Vector3 qInB = _minkowskiB.LocalGetSupportingVertexWithoutMargin(seperatingAxisInB); |
173 | Vector3 pWorld = MathHelper.MatrixToVector(localTransA, pInA); | 173 | Vector3 pWorld = MathHelper.MatrixToVector(localTransA, pInA); |
174 | Vector3 qWorld = MathHelper.MatrixToVector(localTransB, qInB); | 174 | Vector3 qWorld = MathHelper.MatrixToVector(localTransB, qInB); |
175 | 175 | ||
176 | Vector3 w = pWorld - qWorld; | 176 | Vector3 w = pWorld - qWorld; |
177 | delta = Vector3.Dot(_cachedSeparatingAxis, w); | 177 | delta = Vector3.Dot(_cachedSeparatingAxis, w); |
178 | 178 | ||
179 | if ((delta > 0.0) && (delta * delta > squaredDistance * input.MaximumDistanceSquared)) | 179 | if ((delta > 0.0) && (delta * delta > squaredDistance * input.MaximumDistanceSquared)) |
180 | { | 180 | { |
181 | checkPenetration = false; | 181 | checkPenetration = false; |
182 | break; | 182 | break; |
183 | } | 183 | } |
184 | 184 | ||
185 | if (_simplexSolver.InSimplex(w)) | 185 | if (_simplexSolver.InSimplex(w)) |
186 | { | 186 | { |
187 | _degenerateSimplex = 1; | 187 | _degenerateSimplex = 1; |
188 | checkSimplex = true; | 188 | checkSimplex = true; |
189 | break; | 189 | break; |
190 | } | 190 | } |
191 | 191 | ||
192 | float f0 = squaredDistance - delta; | 192 | float f0 = squaredDistance - delta; |
193 | float f1 = squaredDistance * RelativeError2; | 193 | float f1 = squaredDistance * RelativeError2; |
194 | 194 | ||
195 | if (f0 <= f1) | 195 | if (f0 <= f1) |
196 | { | 196 | { |
197 | if (f0 <= 0.0f) | 197 | if (f0 <= 0.0f) |
198 | { | 198 | { |
199 | _degenerateSimplex = 2; | 199 | _degenerateSimplex = 2; |
200 | } | 200 | } |
201 | 201 | ||
202 | checkSimplex = true; | 202 | checkSimplex = true; |
203 | break; | 203 | break; |
204 | } | 204 | } |
205 | 205 | ||
206 | _simplexSolver.AddVertex(w, pWorld, qWorld); | 206 | _simplexSolver.AddVertex(w, pWorld, qWorld); |
207 | 207 | ||
208 | if (!_simplexSolver.Closest(out _cachedSeparatingAxis)) | 208 | if (!_simplexSolver.Closest(out _cachedSeparatingAxis)) |
209 | { | 209 | { |
210 | _degenerateSimplex = 3; | 210 | _degenerateSimplex = 3; |
211 | checkSimplex = true; | 211 | checkSimplex = true; |
212 | break; | 212 | break; |
213 | } | 213 | } |
214 | 214 | ||
215 | float previouseSquaredDistance = squaredDistance; | 215 | float previouseSquaredDistance = squaredDistance; |
216 | squaredDistance = _cachedSeparatingAxis.LengthSquared(); | 216 | squaredDistance = _cachedSeparatingAxis.LengthSquared(); |
217 | 217 | ||
218 | if (previouseSquaredDistance - squaredDistance <= MathHelper.Epsilon * previouseSquaredDistance) | 218 | if (previouseSquaredDistance - squaredDistance <= MathHelper.Epsilon * previouseSquaredDistance) |
219 | { | 219 | { |
220 | _simplexSolver.BackupClosest(out _cachedSeparatingAxis); | 220 | _simplexSolver.BackupClosest(out _cachedSeparatingAxis); |
221 | checkSimplex = true; | 221 | checkSimplex = true; |
222 | break; | 222 | break; |
223 | } | 223 | } |
224 | 224 | ||
225 | if (_currentIteration++ > gjkMaxIter) | 225 | if (_currentIteration++ > gjkMaxIter) |
226 | { | 226 | { |
227 | #if DEBUG | 227 | #if DEBUG |
228 | Console.WriteLine("GjkPairDetector maxIter exceeded: {0}", _currentIteration); | 228 | Console.WriteLine("GjkPairDetector maxIter exceeded: {0}", _currentIteration); |
229 | Console.WriteLine("sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4}, shapeTypeB={5}", | 229 | Console.WriteLine("sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4}, shapeTypeB={5}", |
230 | _cachedSeparatingAxis.X, | 230 | _cachedSeparatingAxis.X, |
231 | _cachedSeparatingAxis.Y, | 231 | _cachedSeparatingAxis.Y, |
232 | _cachedSeparatingAxis.Z, | 232 | _cachedSeparatingAxis.Z, |
233 | squaredDistance, | 233 | squaredDistance, |
234 | _minkowskiA.ShapeType, | 234 | _minkowskiA.ShapeType, |
235 | _minkowskiB.ShapeType | 235 | _minkowskiB.ShapeType |
236 | ); | 236 | ); |
237 | #endif | 237 | #endif |
238 | break; | 238 | break; |
239 | } | 239 | } |
240 | 240 | ||
241 | bool check = (!_simplexSolver.FullSimplex); | 241 | bool check = (!_simplexSolver.FullSimplex); |
242 | 242 | ||
243 | if (!check) | 243 | if (!check) |
244 | { | 244 | { |
245 | _simplexSolver.BackupClosest(out _cachedSeparatingAxis); | 245 | _simplexSolver.BackupClosest(out _cachedSeparatingAxis); |
246 | break; | 246 | break; |
247 | } | 247 | } |
248 | } | 248 | } |
249 | 249 | ||
250 | if (checkSimplex) | 250 | if (checkSimplex) |
251 | { | 251 | { |
252 | _simplexSolver.ComputePoints(out pointOnA, out pointOnB); | 252 | _simplexSolver.ComputePoints(out pointOnA, out pointOnB); |
253 | normalInB = pointOnA - pointOnB; | 253 | normalInB = pointOnA - pointOnB; |
254 | float lenSqr = _cachedSeparatingAxis.LengthSquared(); | 254 | float lenSqr = _cachedSeparatingAxis.LengthSquared(); |
255 | 255 | ||
256 | if (lenSqr < 0.0001f) | 256 | if (lenSqr < 0.0001f) |
257 | { | 257 | { |
258 | _degenerateSimplex = 5; | 258 | _degenerateSimplex = 5; |
259 | } | 259 | } |
260 | 260 | ||
261 | if (lenSqr > MathHelper.Epsilon * MathHelper.Epsilon) | 261 | if (lenSqr > MathHelper.Epsilon * MathHelper.Epsilon) |
262 | { | 262 | { |
263 | float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr); | 263 | float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr); |
264 | normalInB *= rlen; | 264 | normalInB *= rlen; |
265 | float s = (float)Math.Sqrt((float)squaredDistance); | 265 | float s = (float)Math.Sqrt((float)squaredDistance); |
266 | 266 | ||
267 | BulletDebug.Assert(s > 0); | 267 | BulletDebug.Assert(s > 0); |
268 | pointOnA -= _cachedSeparatingAxis * (marginA / s); | 268 | pointOnA -= _cachedSeparatingAxis * (marginA / s); |
269 | pointOnB += _cachedSeparatingAxis * (marginB / s); | 269 | pointOnB += _cachedSeparatingAxis * (marginB / s); |
270 | distance = ((1 / rlen) - margin); | 270 | distance = ((1 / rlen) - margin); |
271 | 271 | ||
272 | isValid = true; | 272 | isValid = true; |
273 | 273 | ||
274 | _lastUsedMethod = 1; | 274 | _lastUsedMethod = 1; |
275 | } | 275 | } |
276 | else | 276 | else |
277 | { | 277 | { |
278 | _lastUsedMethod = 2; | 278 | _lastUsedMethod = 2; |
279 | } | 279 | } |
280 | } | 280 | } |
281 | 281 | ||
282 | bool catchDegeneratePenetrationCase = | 282 | bool catchDegeneratePenetrationCase = |
283 | (_catchDegeneracies != 0 && _penetrationDepthSolver != null && _degenerateSimplex != 0 && ((distance + margin) < 0.01f)); | 283 | (_catchDegeneracies != 0 && _penetrationDepthSolver != null && _degenerateSimplex != 0 && ((distance + margin) < 0.01f)); |
284 | 284 | ||
285 | if (checkPenetration && (!isValid || catchDegeneratePenetrationCase)) | 285 | if (checkPenetration && (!isValid || catchDegeneratePenetrationCase)) |
286 | { | 286 | { |
287 | #warning Check this | 287 | #warning Check this |
288 | if (_penetrationDepthSolver != null) | 288 | if (_penetrationDepthSolver != null) |
289 | { | 289 | { |
290 | Vector3 tmpPointOnA, tmpPointOnB; | 290 | Vector3 tmpPointOnA, tmpPointOnB; |
291 | 291 | ||
292 | _numDeepPenetrationChecks++; | 292 | _numDeepPenetrationChecks++; |
293 | 293 | ||
294 | bool isValid2 = _penetrationDepthSolver.CalculatePenetrationDepth( | 294 | bool isValid2 = _penetrationDepthSolver.CalculatePenetrationDepth( |
295 | _simplexSolver, _minkowskiA, _minkowskiB, localTransA, localTransB, | 295 | _simplexSolver, _minkowskiA, _minkowskiB, localTransA, localTransB, |
296 | _cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB, | 296 | _cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB, |
297 | debugDraw | 297 | debugDraw |
298 | ); | 298 | ); |
299 | 299 | ||
300 | if (isValid2) | 300 | if (isValid2) |
301 | { | 301 | { |
302 | Vector3 tmpNormalInB = tmpPointOnB - tmpPointOnA; | 302 | Vector3 tmpNormalInB = tmpPointOnB - tmpPointOnA; |
303 | float lengSqr = tmpNormalInB.LengthSquared(); | 303 | float lengSqr = tmpNormalInB.LengthSquared(); |
304 | 304 | ||
305 | if (lengSqr > (MathHelper.Epsilon * MathHelper.Epsilon)) | 305 | if (lengSqr > (MathHelper.Epsilon * MathHelper.Epsilon)) |
306 | { | 306 | { |
307 | tmpNormalInB /= (float)Math.Sqrt((float)lengSqr); | 307 | tmpNormalInB /= (float)Math.Sqrt((float)lengSqr); |
308 | float distance2 = -(tmpPointOnA - tmpPointOnB).Length(); | 308 | float distance2 = -(tmpPointOnA - tmpPointOnB).Length(); |
309 | 309 | ||
310 | if (!isValid || (distance2 < distance)) | 310 | if (!isValid || (distance2 < distance)) |
311 | { | 311 | { |
312 | distance = distance2; | 312 | distance = distance2; |
313 | pointOnA = tmpPointOnA; | 313 | pointOnA = tmpPointOnA; |
314 | pointOnB = tmpPointOnB; | 314 | pointOnB = tmpPointOnB; |
315 | normalInB = tmpNormalInB; | 315 | normalInB = tmpNormalInB; |
316 | isValid = true; | 316 | isValid = true; |
317 | _lastUsedMethod = 3; | 317 | _lastUsedMethod = 3; |
318 | } | 318 | } |
319 | else | 319 | else |
320 | { | 320 | { |
321 | 321 | ||
322 | } | 322 | } |
323 | } | 323 | } |
324 | else | 324 | else |
325 | { | 325 | { |
326 | _lastUsedMethod = 4; | 326 | _lastUsedMethod = 4; |
327 | } | 327 | } |
328 | } | 328 | } |
329 | else | 329 | else |
330 | { | 330 | { |
331 | _lastUsedMethod = 5; | 331 | _lastUsedMethod = 5; |
332 | } | 332 | } |
333 | } | 333 | } |
334 | } | 334 | } |
335 | 335 | ||
336 | if (isValid) | 336 | if (isValid) |
337 | { | 337 | { |
338 | output.AddContactPoint(normalInB, pointOnB + positionOffset, distance); | 338 | output.AddContactPoint(normalInB, pointOnB + positionOffset, distance); |
339 | } | 339 | } |
340 | } | 340 | } |
341 | } | 341 | } |
342 | } | 342 | } |
343 | } \ No newline at end of file | 343 | } \ No newline at end of file |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs index 59fd486..08206c8 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs | |||
@@ -1,42 +1,42 @@ | |||
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 | /// IConvexPenetrationDepthSolver provides an interface for penetration depth calculation. | 30 | /// IConvexPenetrationDepthSolver provides an interface for penetration depth calculation. |
31 | /// </summary> | 31 | /// </summary> |
32 | public interface IConvexPenetrationDepthSolver | 32 | public interface IConvexPenetrationDepthSolver |
33 | { | 33 | { |
34 | bool CalculatePenetrationDepth( | 34 | bool CalculatePenetrationDepth( |
35 | ISimplexSolver simplexSolver, | 35 | ISimplexSolver simplexSolver, |
36 | ConvexShape convexA, ConvexShape convexB, | 36 | ConvexShape convexA, ConvexShape convexB, |
37 | Matrix transformA, Matrix transformB, | 37 | Matrix transformA, Matrix transformB, |
38 | Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, | 38 | Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, |
39 | IDebugDraw debugDraw//, StackAlloc stackAlloc | 39 | IDebugDraw debugDraw//, StackAlloc stackAlloc |
40 | ); | 40 | ); |
41 | } | 41 | } |
42 | } | 42 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs index 71b1eaa..f1b1807 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs | |||
@@ -1,48 +1,48 @@ | |||
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 | /// ISimplexSolver can incrementally calculate distance between origin and up to 4 vertices | 29 | /// ISimplexSolver can incrementally calculate distance between origin and up to 4 vertices |
30 | /// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on | 30 | /// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on |
31 | /// voronoi regions or barycentric coordinates | 31 | /// voronoi regions or barycentric coordinates |
32 | public interface ISimplexSolver | 32 | public interface ISimplexSolver |
33 | { | 33 | { |
34 | void Reset(); | 34 | void Reset(); |
35 | void AddVertex(Vector3 w, Vector3 p, Vector3 q); | 35 | void AddVertex(Vector3 w, Vector3 p, Vector3 q); |
36 | bool Closest(out Vector3 v); | 36 | bool Closest(out Vector3 v); |
37 | 37 | ||
38 | int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf); | 38 | int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf); |
39 | bool InSimplex(Vector3 w); | 39 | bool InSimplex(Vector3 w); |
40 | void BackupClosest(out Vector3 v); | 40 | void BackupClosest(out Vector3 v); |
41 | void ComputePoints(out Vector3 pA, out Vector3 pB); | 41 | void ComputePoints(out Vector3 pA, out Vector3 pB); |
42 | 42 | ||
43 | int NumVertices { get;} | 43 | int NumVertices { get;} |
44 | bool EmptySimplex { get;} | 44 | bool EmptySimplex { get;} |
45 | float MaxVertex { get;} | 45 | float MaxVertex { get;} |
46 | bool FullSimplex { get;} | 46 | bool FullSimplex { get;} |
47 | } | 47 | } |
48 | } \ No newline at end of file | 48 | } \ No newline at end of file |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs index 751a9df..f5bcedc 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs | |||
@@ -1,78 +1,78 @@ | |||
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 | public class ManifoldPoint | 29 | public class ManifoldPoint |
30 | { | 30 | { |
31 | private Vector3 _localPointA; | 31 | private Vector3 _localPointA; |
32 | private Vector3 _localPointB; | 32 | private Vector3 _localPointB; |
33 | private Vector3 _positionWorldOnB; | 33 | private Vector3 _positionWorldOnB; |
34 | private Vector3 _positionWorldOnA; | 34 | private Vector3 _positionWorldOnA; |
35 | private Vector3 _normalWorldOnB; | 35 | private Vector3 _normalWorldOnB; |
36 | 36 | ||
37 | private float _distance; | 37 | private float _distance; |
38 | private float _combinedFriction; | 38 | private float _combinedFriction; |
39 | private float _combinedRestitution; | 39 | private float _combinedRestitution; |
40 | 40 | ||
41 | private object _userPersistentData; | 41 | private object _userPersistentData; |
42 | 42 | ||
43 | private int _lifeTime;//lifetime of the contactpoint in frames | 43 | private int _lifeTime;//lifetime of the contactpoint in frames |
44 | 44 | ||
45 | public ManifoldPoint() | 45 | public ManifoldPoint() |
46 | : this(new Vector3(), new Vector3(), new Vector3(), 0f) | 46 | : this(new Vector3(), new Vector3(), new Vector3(), 0f) |
47 | { | 47 | { |
48 | } | 48 | } |
49 | 49 | ||
50 | public ManifoldPoint(Vector3 pointA, Vector3 pointB, | 50 | public ManifoldPoint(Vector3 pointA, Vector3 pointB, |
51 | Vector3 normal, | 51 | Vector3 normal, |
52 | float distance) | 52 | float distance) |
53 | { | 53 | { |
54 | _localPointA = pointA; | 54 | _localPointA = pointA; |
55 | _localPointB = pointB; | 55 | _localPointB = pointB; |
56 | _normalWorldOnB = normal; | 56 | _normalWorldOnB = normal; |
57 | _distance = distance; | 57 | _distance = distance; |
58 | _positionWorldOnA = new Vector3(); | 58 | _positionWorldOnA = new Vector3(); |
59 | _positionWorldOnB = new Vector3(); | 59 | _positionWorldOnB = new Vector3(); |
60 | } | 60 | } |
61 | 61 | ||
62 | public float Distance { get { return _distance; } set { _distance = value; } } | 62 | public float Distance { get { return _distance; } set { _distance = value; } } |
63 | public int LifeTime { get { return _lifeTime; } set { _lifeTime = value; } } | 63 | public int LifeTime { get { return _lifeTime; } set { _lifeTime = value; } } |
64 | 64 | ||
65 | public Vector3 PositionWorldOnA { get { return _positionWorldOnA; } set { _positionWorldOnA = value; } } | 65 | public Vector3 PositionWorldOnA { get { return _positionWorldOnA; } set { _positionWorldOnA = value; } } |
66 | public Vector3 PositionWorldOnB { get { return _positionWorldOnB; } set { _positionWorldOnB = value; } } | 66 | public Vector3 PositionWorldOnB { get { return _positionWorldOnB; } set { _positionWorldOnB = value; } } |
67 | 67 | ||
68 | public Vector3 LocalPointA { get { return _localPointA; } set { _localPointA = value; } } | 68 | public Vector3 LocalPointA { get { return _localPointA; } set { _localPointA = value; } } |
69 | public Vector3 LocalPointB { get { return _localPointB; } set { _localPointB = value; } } | 69 | public Vector3 LocalPointB { get { return _localPointB; } set { _localPointB = value; } } |
70 | 70 | ||
71 | public Vector3 NormalWorldOnB { get { return _normalWorldOnB; } } | 71 | public Vector3 NormalWorldOnB { get { return _normalWorldOnB; } } |
72 | 72 | ||
73 | public float CombinedFriction { get { return _combinedFriction; } set { _combinedFriction = value; } } | 73 | public float CombinedFriction { get { return _combinedFriction; } set { _combinedFriction = value; } } |
74 | public float CombinedRestitution { get { return _combinedRestitution; } set { _combinedRestitution = value; } } | 74 | public float CombinedRestitution { get { return _combinedRestitution; } set { _combinedRestitution = value; } } |
75 | 75 | ||
76 | public object UserPersistentData { get { return _userPersistentData; } set { _userPersistentData = value; } } | 76 | public object UserPersistentData { get { return _userPersistentData; } set { _userPersistentData = value; } } |
77 | } | 77 | } |
78 | } | 78 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs index 81b05b8..7363e28 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs | |||
@@ -1,246 +1,246 @@ | |||
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 | /// MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation. | 30 | /// MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation. |
31 | /// Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points. | 31 | /// Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points. |
32 | /// </summary> | 32 | /// </summary> |
33 | public class MinkowskiPenetrationDepthSolver : IConvexPenetrationDepthSolver | 33 | public class MinkowskiPenetrationDepthSolver : IConvexPenetrationDepthSolver |
34 | { | 34 | { |
35 | private const int UnitSpherePointsCount = 42; | 35 | private const int UnitSpherePointsCount = 42; |
36 | 36 | ||
37 | private static Vector3[] penetrationDirections = | 37 | private static Vector3[] penetrationDirections = |
38 | { | 38 | { |
39 | new Vector3(0.000000f , -0.000000f,-1.000000f), | 39 | new Vector3(0.000000f , -0.000000f,-1.000000f), |
40 | new Vector3(0.723608f , -0.525725f,-0.447219f), | 40 | new Vector3(0.723608f , -0.525725f,-0.447219f), |
41 | new Vector3(-0.276388f , -0.850649f,-0.447219f), | 41 | new Vector3(-0.276388f , -0.850649f,-0.447219f), |
42 | new Vector3(-0.894426f , -0.000000f,-0.447216f), | 42 | new Vector3(-0.894426f , -0.000000f,-0.447216f), |
43 | new Vector3(-0.276388f , 0.850649f,-0.447220f), | 43 | new Vector3(-0.276388f , 0.850649f,-0.447220f), |
44 | new Vector3(0.723608f , 0.525725f,-0.447219f), | 44 | new Vector3(0.723608f , 0.525725f,-0.447219f), |
45 | new Vector3(0.276388f , -0.850649f,0.447220f), | 45 | new Vector3(0.276388f , -0.850649f,0.447220f), |
46 | new Vector3(-0.723608f , -0.525725f,0.447219f), | 46 | new Vector3(-0.723608f , -0.525725f,0.447219f), |
47 | new Vector3(-0.723608f , 0.525725f,0.447219f), | 47 | new Vector3(-0.723608f , 0.525725f,0.447219f), |
48 | new Vector3(0.276388f , 0.850649f,0.447219f), | 48 | new Vector3(0.276388f , 0.850649f,0.447219f), |
49 | new Vector3(0.894426f , 0.000000f,0.447216f), | 49 | new Vector3(0.894426f , 0.000000f,0.447216f), |
50 | new Vector3(-0.000000f , 0.000000f,1.000000f), | 50 | new Vector3(-0.000000f , 0.000000f,1.000000f), |
51 | new Vector3(0.425323f , -0.309011f,-0.850654f), | 51 | new Vector3(0.425323f , -0.309011f,-0.850654f), |
52 | new Vector3(-0.162456f , -0.499995f,-0.850654f), | 52 | new Vector3(-0.162456f , -0.499995f,-0.850654f), |
53 | new Vector3(0.262869f , -0.809012f,-0.525738f), | 53 | new Vector3(0.262869f , -0.809012f,-0.525738f), |
54 | new Vector3(0.425323f , 0.309011f,-0.850654f), | 54 | new Vector3(0.425323f , 0.309011f,-0.850654f), |
55 | new Vector3(0.850648f , -0.000000f,-0.525736f), | 55 | new Vector3(0.850648f , -0.000000f,-0.525736f), |
56 | new Vector3(-0.525730f , -0.000000f,-0.850652f), | 56 | new Vector3(-0.525730f , -0.000000f,-0.850652f), |
57 | new Vector3(-0.688190f , -0.499997f,-0.525736f), | 57 | new Vector3(-0.688190f , -0.499997f,-0.525736f), |
58 | new Vector3(-0.162456f , 0.499995f,-0.850654f), | 58 | new Vector3(-0.162456f , 0.499995f,-0.850654f), |
59 | new Vector3(-0.688190f , 0.499997f,-0.525736f), | 59 | new Vector3(-0.688190f , 0.499997f,-0.525736f), |
60 | new Vector3(0.262869f , 0.809012f,-0.525738f), | 60 | new Vector3(0.262869f , 0.809012f,-0.525738f), |
61 | new Vector3(0.951058f , 0.309013f,0.000000f), | 61 | new Vector3(0.951058f , 0.309013f,0.000000f), |
62 | new Vector3(0.951058f , -0.309013f,0.000000f), | 62 | new Vector3(0.951058f , -0.309013f,0.000000f), |
63 | new Vector3(0.587786f , -0.809017f,0.000000f), | 63 | new Vector3(0.587786f , -0.809017f,0.000000f), |
64 | new Vector3(0.000000f , -1.000000f,0.000000f), | 64 | new Vector3(0.000000f , -1.000000f,0.000000f), |
65 | new Vector3(-0.587786f , -0.809017f,0.000000f), | 65 | new Vector3(-0.587786f , -0.809017f,0.000000f), |
66 | new Vector3(-0.951058f , -0.309013f,-0.000000f), | 66 | new Vector3(-0.951058f , -0.309013f,-0.000000f), |
67 | new Vector3(-0.951058f , 0.309013f,-0.000000f), | 67 | new Vector3(-0.951058f , 0.309013f,-0.000000f), |
68 | new Vector3(-0.587786f , 0.809017f,-0.000000f), | 68 | new Vector3(-0.587786f , 0.809017f,-0.000000f), |
69 | new Vector3(-0.000000f , 1.000000f,-0.000000f), | 69 | new Vector3(-0.000000f , 1.000000f,-0.000000f), |
70 | new Vector3(0.587786f , 0.809017f,-0.000000f), | 70 | new Vector3(0.587786f , 0.809017f,-0.000000f), |
71 | new Vector3(0.688190f , -0.499997f,0.525736f), | 71 | new Vector3(0.688190f , -0.499997f,0.525736f), |
72 | new Vector3(-0.262869f , -0.809012f,0.525738f), | 72 | new Vector3(-0.262869f , -0.809012f,0.525738f), |
73 | new Vector3(-0.850648f , 0.000000f,0.525736f), | 73 | new Vector3(-0.850648f , 0.000000f,0.525736f), |
74 | new Vector3(-0.262869f , 0.809012f,0.525738f), | 74 | new Vector3(-0.262869f , 0.809012f,0.525738f), |
75 | new Vector3(0.688190f , 0.499997f,0.525736f), | 75 | new Vector3(0.688190f , 0.499997f,0.525736f), |
76 | new Vector3(0.525730f , 0.000000f,0.850652f), | 76 | new Vector3(0.525730f , 0.000000f,0.850652f), |
77 | new Vector3(0.162456f , -0.499995f,0.850654f), | 77 | new Vector3(0.162456f , -0.499995f,0.850654f), |
78 | new Vector3(-0.425323f , -0.309011f,0.850654f), | 78 | new Vector3(-0.425323f , -0.309011f,0.850654f), |
79 | new Vector3(-0.425323f , 0.309011f,0.850654f), | 79 | new Vector3(-0.425323f , 0.309011f,0.850654f), |
80 | new Vector3(0.162456f , 0.499995f,0.850654f) | 80 | new Vector3(0.162456f , 0.499995f,0.850654f) |
81 | }; | 81 | }; |
82 | 82 | ||
83 | private class IntermediateResult : DiscreteCollisionDetectorInterface.Result | 83 | private class IntermediateResult : DiscreteCollisionDetectorInterface.Result |
84 | { | 84 | { |
85 | private Vector3 _normalOnBInWorld; | 85 | private Vector3 _normalOnBInWorld; |
86 | private Vector3 _pointInWorld; | 86 | private Vector3 _pointInWorld; |
87 | private float _depth; | 87 | private float _depth; |
88 | private bool _hasResult; | 88 | private bool _hasResult; |
89 | 89 | ||
90 | public IntermediateResult() | 90 | public IntermediateResult() |
91 | { | 91 | { |
92 | _hasResult = false; | 92 | _hasResult = false; |
93 | } | 93 | } |
94 | 94 | ||
95 | public bool HasResult { get { return _hasResult; } } | 95 | public bool HasResult { get { return _hasResult; } } |
96 | public float Depth { get { return _depth; } } | 96 | public float Depth { get { return _depth; } } |
97 | public Vector3 PointInWorld { get { return _pointInWorld; } } | 97 | public Vector3 PointInWorld { get { return _pointInWorld; } } |
98 | 98 | ||
99 | public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1) | 99 | public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1) |
100 | { | 100 | { |
101 | } | 101 | } |
102 | 102 | ||
103 | public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) | 103 | public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) |
104 | { | 104 | { |
105 | _normalOnBInWorld = normalOnBInWorld; | 105 | _normalOnBInWorld = normalOnBInWorld; |
106 | _pointInWorld = pointInWorld; | 106 | _pointInWorld = pointInWorld; |
107 | _depth = depth; | 107 | _depth = depth; |
108 | _hasResult = true; | 108 | _hasResult = true; |
109 | } | 109 | } |
110 | } | 110 | } |
111 | 111 | ||
112 | #region IConvexPenetrationDepthSolver Members | 112 | #region IConvexPenetrationDepthSolver Members |
113 | public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, | 113 | public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, |
114 | ConvexShape convexA, ConvexShape convexB, | 114 | ConvexShape convexA, ConvexShape convexB, |
115 | Matrix transformA, Matrix transformB, | 115 | Matrix transformA, Matrix transformB, |
116 | Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw) | 116 | Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw) |
117 | { | 117 | { |
118 | pa = new Vector3(); | 118 | pa = new Vector3(); |
119 | pb = new Vector3(); | 119 | pb = new Vector3(); |
120 | //just take fixed number of orientation, and sample the penetration depth in that direction | 120 | //just take fixed number of orientation, and sample the penetration depth in that direction |
121 | float minProj = 1e30f; | 121 | float minProj = 1e30f; |
122 | Vector3 minNorm = new Vector3(); | 122 | Vector3 minNorm = new Vector3(); |
123 | Vector3 minA = new Vector3(), minB = new Vector3(); | 123 | Vector3 minA = new Vector3(), minB = new Vector3(); |
124 | Vector3 seperatingAxisInA, seperatingAxisInB; | 124 | Vector3 seperatingAxisInA, seperatingAxisInB; |
125 | Vector3 pInA, qInB, pWorld, qWorld, w; | 125 | Vector3 pInA, qInB, pWorld, qWorld, w; |
126 | 126 | ||
127 | Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; | 127 | Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; |
128 | Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; | 128 | Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; |
129 | Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; | 129 | Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; |
130 | Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; | 130 | Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; |
131 | 131 | ||
132 | int numSampleDirections = UnitSpherePointsCount; | 132 | int numSampleDirections = UnitSpherePointsCount; |
133 | 133 | ||
134 | for (int i = 0; i < numSampleDirections; i++) | 134 | for (int i = 0; i < numSampleDirections; i++) |
135 | { | 135 | { |
136 | Vector3 norm = penetrationDirections[i]; | 136 | Vector3 norm = penetrationDirections[i]; |
137 | seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA); | 137 | seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA); |
138 | seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB); | 138 | seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB); |
139 | } | 139 | } |
140 | 140 | ||
141 | { | 141 | { |
142 | int numPDA = convexA.PreferredPenetrationDirectionsCount; | 142 | int numPDA = convexA.PreferredPenetrationDirectionsCount; |
143 | if (numPDA != 0) | 143 | if (numPDA != 0) |
144 | { | 144 | { |
145 | for (int i = 0; i < numPDA; i++) | 145 | for (int i = 0; i < numPDA; i++) |
146 | { | 146 | { |
147 | Vector3 norm; | 147 | Vector3 norm; |
148 | convexA.GetPreferredPenetrationDirection(i, out norm); | 148 | convexA.GetPreferredPenetrationDirection(i, out norm); |
149 | norm = Vector3.TransformNormal(norm, transformA); | 149 | norm = Vector3.TransformNormal(norm, transformA); |
150 | penetrationDirections[numSampleDirections] = norm; | 150 | penetrationDirections[numSampleDirections] = norm; |
151 | seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); | 151 | seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); |
152 | seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); | 152 | seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); |
153 | numSampleDirections++; | 153 | numSampleDirections++; |
154 | } | 154 | } |
155 | } | 155 | } |
156 | } | 156 | } |
157 | 157 | ||
158 | { | 158 | { |
159 | int numPDB = convexB.PreferredPenetrationDirectionsCount; | 159 | int numPDB = convexB.PreferredPenetrationDirectionsCount; |
160 | if (numPDB != 0) | 160 | if (numPDB != 0) |
161 | { | 161 | { |
162 | for (int i = 0; i < numPDB; i++) | 162 | for (int i = 0; i < numPDB; i++) |
163 | { | 163 | { |
164 | Vector3 norm; | 164 | Vector3 norm; |
165 | convexB.GetPreferredPenetrationDirection(i, out norm); | 165 | convexB.GetPreferredPenetrationDirection(i, out norm); |
166 | norm = Vector3.TransformNormal(norm, transformB); | 166 | norm = Vector3.TransformNormal(norm, transformB); |
167 | penetrationDirections[numSampleDirections] = norm; | 167 | penetrationDirections[numSampleDirections] = norm; |
168 | seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); | 168 | seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); |
169 | seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); | 169 | seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); |
170 | numSampleDirections++; | 170 | numSampleDirections++; |
171 | } | 171 | } |
172 | } | 172 | } |
173 | } | 173 | } |
174 | 174 | ||
175 | convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections); | 175 | convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections); |
176 | convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections); | 176 | convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections); |
177 | 177 | ||
178 | for (int i = 0; i < numSampleDirections; i++) | 178 | for (int i = 0; i < numSampleDirections; i++) |
179 | { | 179 | { |
180 | Vector3 norm = penetrationDirections[i]; | 180 | Vector3 norm = penetrationDirections[i]; |
181 | seperatingAxisInA = seperatingAxisInABatch[i]; | 181 | seperatingAxisInA = seperatingAxisInABatch[i]; |
182 | seperatingAxisInB = seperatingAxisInBBatch[i]; | 182 | seperatingAxisInB = seperatingAxisInBBatch[i]; |
183 | 183 | ||
184 | pInA = supportVerticesABatch[i]; | 184 | pInA = supportVerticesABatch[i]; |
185 | qInB = supportVerticesBBatch[i]; | 185 | qInB = supportVerticesBBatch[i]; |
186 | 186 | ||
187 | pWorld = MathHelper.MatrixToVector(transformA, pInA); | 187 | pWorld = MathHelper.MatrixToVector(transformA, pInA); |
188 | qWorld = MathHelper.MatrixToVector(transformB, qInB); | 188 | qWorld = MathHelper.MatrixToVector(transformB, qInB); |
189 | w = qWorld - pWorld; | 189 | w = qWorld - pWorld; |
190 | float delta = Vector3.Dot(norm, w); | 190 | float delta = Vector3.Dot(norm, w); |
191 | //find smallest delta | 191 | //find smallest delta |
192 | if (delta < minProj) | 192 | if (delta < minProj) |
193 | { | 193 | { |
194 | minProj = delta; | 194 | minProj = delta; |
195 | minNorm = norm; | 195 | minNorm = norm; |
196 | minA = pWorld; | 196 | minA = pWorld; |
197 | minB = qWorld; | 197 | minB = qWorld; |
198 | } | 198 | } |
199 | } | 199 | } |
200 | 200 | ||
201 | //add the margins | 201 | //add the margins |
202 | minA += minNorm * convexA.Margin; | 202 | minA += minNorm * convexA.Margin; |
203 | minB -= minNorm * convexB.Margin; | 203 | minB -= minNorm * convexB.Margin; |
204 | //no penetration | 204 | //no penetration |
205 | if (minProj < 0) | 205 | if (minProj < 0) |
206 | return false; | 206 | return false; |
207 | 207 | ||
208 | minProj += (convexA.Margin + convexB.Margin); | 208 | minProj += (convexA.Margin + convexB.Margin); |
209 | 209 | ||
210 | GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null); | 210 | GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null); |
211 | 211 | ||
212 | float offsetDist = minProj; | 212 | float offsetDist = minProj; |
213 | Vector3 offset = minNorm * offsetDist; | 213 | Vector3 offset = minNorm * offsetDist; |
214 | 214 | ||
215 | GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | 215 | GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); |
216 | 216 | ||
217 | Vector3 newOrg = transformA.Translation + offset; | 217 | Vector3 newOrg = transformA.Translation + offset; |
218 | 218 | ||
219 | Matrix displacedTrans = transformA; | 219 | Matrix displacedTrans = transformA; |
220 | displacedTrans.Translation = newOrg; | 220 | displacedTrans.Translation = newOrg; |
221 | 221 | ||
222 | input.TransformA = displacedTrans; | 222 | input.TransformA = displacedTrans; |
223 | input.TransformB = transformB; | 223 | input.TransformB = transformB; |
224 | input.MaximumDistanceSquared = 1e30f;//minProj; | 224 | input.MaximumDistanceSquared = 1e30f;//minProj; |
225 | 225 | ||
226 | IntermediateResult res = new IntermediateResult(); | 226 | IntermediateResult res = new IntermediateResult(); |
227 | gjkdet.GetClosestPoints(input, res, debugDraw); | 227 | gjkdet.GetClosestPoints(input, res, debugDraw); |
228 | 228 | ||
229 | float correctedMinNorm = minProj - res.Depth; | 229 | float correctedMinNorm = minProj - res.Depth; |
230 | 230 | ||
231 | //the penetration depth is over-estimated, relax it | 231 | //the penetration depth is over-estimated, relax it |
232 | float penetration_relaxation = 1; | 232 | float penetration_relaxation = 1; |
233 | minNorm *= penetration_relaxation; | 233 | minNorm *= penetration_relaxation; |
234 | 234 | ||
235 | if (res.HasResult) | 235 | if (res.HasResult) |
236 | { | 236 | { |
237 | 237 | ||
238 | pa = res.PointInWorld - minNorm * correctedMinNorm; | 238 | pa = res.PointInWorld - minNorm * correctedMinNorm; |
239 | pb = res.PointInWorld; | 239 | pb = res.PointInWorld; |
240 | } | 240 | } |
241 | 241 | ||
242 | return res.HasResult; | 242 | return res.HasResult; |
243 | } | 243 | } |
244 | #endregion | 244 | #endregion |
245 | } | 245 | } |
246 | } | 246 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs index 47cfe9a..04a533d 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs | |||
@@ -1,272 +1,272 @@ | |||
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 | public delegate bool ContactDestroyedCallback(object userPersistentData); | 29 | public delegate bool ContactDestroyedCallback(object userPersistentData); |
30 | 30 | ||
31 | public class PersistentManifold | 31 | public class PersistentManifold |
32 | { | 32 | { |
33 | private static ContactDestroyedCallback _contactDestroyedCallback = null; | 33 | private static ContactDestroyedCallback _contactDestroyedCallback = null; |
34 | private static float _contactBreakingThreshold = 0.02f; | 34 | private static float _contactBreakingThreshold = 0.02f; |
35 | 35 | ||
36 | private ManifoldPoint[] _pointCache = new ManifoldPoint[4]; | 36 | private ManifoldPoint[] _pointCache = new ManifoldPoint[4]; |
37 | 37 | ||
38 | // this two body pointers can point to the physics rigidbody class. | 38 | // this two body pointers can point to the physics rigidbody class. |
39 | // object will allow any rigidbody class | 39 | // object will allow any rigidbody class |
40 | private object _bodyA; | 40 | private object _bodyA; |
41 | private object _bodyB; | 41 | private object _bodyB; |
42 | private int _cachedPoints; | 42 | private int _cachedPoints; |
43 | 43 | ||
44 | public PersistentManifold(object bodyA, object bodyB) | 44 | public PersistentManifold(object bodyA, object bodyB) |
45 | { | 45 | { |
46 | _bodyA = bodyA; | 46 | _bodyA = bodyA; |
47 | _bodyB = bodyB; | 47 | _bodyB = bodyB; |
48 | _cachedPoints = 0; | 48 | _cachedPoints = 0; |
49 | } | 49 | } |
50 | 50 | ||
51 | public object BodyA { get { return _bodyA; } } | 51 | public object BodyA { get { return _bodyA; } } |
52 | public object BodyB { get { return _bodyB; } } | 52 | public object BodyB { get { return _bodyB; } } |
53 | 53 | ||
54 | public int ContactsCount { get { return _cachedPoints; } } | 54 | public int ContactsCount { get { return _cachedPoints; } } |
55 | 55 | ||
56 | public static ContactDestroyedCallback ContactDestroyedCallback { get { return _contactDestroyedCallback; } set { _contactDestroyedCallback = value; } } | 56 | public static ContactDestroyedCallback ContactDestroyedCallback { get { return _contactDestroyedCallback; } set { _contactDestroyedCallback = value; } } |
57 | public static float ContactBreakingThreshold { get { return _contactBreakingThreshold; } } | 57 | public static float ContactBreakingThreshold { get { return _contactBreakingThreshold; } } |
58 | 58 | ||
59 | public void SetBodies(object bodyA, object bodyB) | 59 | public void SetBodies(object bodyA, object bodyB) |
60 | { | 60 | { |
61 | _bodyA = bodyA; | 61 | _bodyA = bodyA; |
62 | _bodyB = bodyB; | 62 | _bodyB = bodyB; |
63 | } | 63 | } |
64 | 64 | ||
65 | public ManifoldPoint GetContactPoint(int index) | 65 | public ManifoldPoint GetContactPoint(int index) |
66 | { | 66 | { |
67 | if (index >= _cachedPoints) | 67 | if (index >= _cachedPoints) |
68 | throw new ArgumentOutOfRangeException("index", "index must be smaller than cachedPoints"); | 68 | throw new ArgumentOutOfRangeException("index", "index must be smaller than cachedPoints"); |
69 | 69 | ||
70 | return _pointCache[index]; | 70 | return _pointCache[index]; |
71 | } | 71 | } |
72 | 72 | ||
73 | public int GetCacheEntry(ManifoldPoint newPoint) | 73 | public int GetCacheEntry(ManifoldPoint newPoint) |
74 | { | 74 | { |
75 | float shortestDist = ContactBreakingThreshold * ContactBreakingThreshold; | 75 | float shortestDist = ContactBreakingThreshold * ContactBreakingThreshold; |
76 | int size = ContactsCount; | 76 | int size = ContactsCount; |
77 | int nearestPoint = -1; | 77 | int nearestPoint = -1; |
78 | for (int i = 0; i < size; i++) | 78 | for (int i = 0; i < size; i++) |
79 | { | 79 | { |
80 | ManifoldPoint mp = _pointCache[i]; | 80 | ManifoldPoint mp = _pointCache[i]; |
81 | 81 | ||
82 | Vector3 diffA = mp.LocalPointA - newPoint.LocalPointA; | 82 | Vector3 diffA = mp.LocalPointA - newPoint.LocalPointA; |
83 | float distToManiPoint = Vector3.Dot(diffA, diffA); | 83 | float distToManiPoint = Vector3.Dot(diffA, diffA); |
84 | if (distToManiPoint < shortestDist) | 84 | if (distToManiPoint < shortestDist) |
85 | { | 85 | { |
86 | shortestDist = distToManiPoint; | 86 | shortestDist = distToManiPoint; |
87 | nearestPoint = i; | 87 | nearestPoint = i; |
88 | } | 88 | } |
89 | } | 89 | } |
90 | return nearestPoint; | 90 | return nearestPoint; |
91 | } | 91 | } |
92 | 92 | ||
93 | public void AddManifoldPoint(ManifoldPoint newPoint) | 93 | public void AddManifoldPoint(ManifoldPoint newPoint) |
94 | { | 94 | { |
95 | if (!ValidContactDistance(newPoint)) | 95 | if (!ValidContactDistance(newPoint)) |
96 | throw new BulletException(); | 96 | throw new BulletException(); |
97 | 97 | ||
98 | int insertIndex = ContactsCount; | 98 | int insertIndex = ContactsCount; |
99 | if (insertIndex == 4) | 99 | if (insertIndex == 4) |
100 | { | 100 | { |
101 | //sort cache so best points come first, based on area | 101 | //sort cache so best points come first, based on area |
102 | insertIndex = SortCachedPoints(newPoint); | 102 | insertIndex = SortCachedPoints(newPoint); |
103 | } | 103 | } |
104 | else | 104 | else |
105 | { | 105 | { |
106 | _cachedPoints++; | 106 | _cachedPoints++; |
107 | } | 107 | } |
108 | ReplaceContactPoint(newPoint, insertIndex); | 108 | ReplaceContactPoint(newPoint, insertIndex); |
109 | } | 109 | } |
110 | 110 | ||
111 | public void RemoveContactPoint(int index) | 111 | public void RemoveContactPoint(int index) |
112 | { | 112 | { |
113 | ClearUserCache(_pointCache[index]); | 113 | ClearUserCache(_pointCache[index]); |
114 | 114 | ||
115 | int lastUsedIndex = ContactsCount - 1; | 115 | int lastUsedIndex = ContactsCount - 1; |
116 | _pointCache[index] = _pointCache[lastUsedIndex]; | 116 | _pointCache[index] = _pointCache[lastUsedIndex]; |
117 | //get rid of duplicated userPersistentData pointer | 117 | //get rid of duplicated userPersistentData pointer |
118 | _pointCache[lastUsedIndex].UserPersistentData = null; | 118 | _pointCache[lastUsedIndex].UserPersistentData = null; |
119 | _cachedPoints--; | 119 | _cachedPoints--; |
120 | } | 120 | } |
121 | 121 | ||
122 | public void ReplaceContactPoint(ManifoldPoint newPoint, int insertIndex) | 122 | public void ReplaceContactPoint(ManifoldPoint newPoint, int insertIndex) |
123 | { | 123 | { |
124 | BulletDebug.Assert(ValidContactDistance(newPoint)); | 124 | BulletDebug.Assert(ValidContactDistance(newPoint)); |
125 | 125 | ||
126 | if (_pointCache[insertIndex] != null) | 126 | if (_pointCache[insertIndex] != null) |
127 | { | 127 | { |
128 | int lifeTime = _pointCache[insertIndex].LifeTime; | 128 | int lifeTime = _pointCache[insertIndex].LifeTime; |
129 | BulletDebug.Assert(lifeTime >= 0); | 129 | BulletDebug.Assert(lifeTime >= 0); |
130 | object cache = _pointCache[insertIndex].UserPersistentData; | 130 | object cache = _pointCache[insertIndex].UserPersistentData; |
131 | 131 | ||
132 | _pointCache[insertIndex] = newPoint; | 132 | _pointCache[insertIndex] = newPoint; |
133 | 133 | ||
134 | _pointCache[insertIndex].UserPersistentData = cache; | 134 | _pointCache[insertIndex].UserPersistentData = cache; |
135 | _pointCache[insertIndex].LifeTime = lifeTime; | 135 | _pointCache[insertIndex].LifeTime = lifeTime; |
136 | } | 136 | } |
137 | else | 137 | else |
138 | { | 138 | { |
139 | _pointCache[insertIndex] = newPoint; | 139 | _pointCache[insertIndex] = newPoint; |
140 | } | 140 | } |
141 | 141 | ||
142 | //ClearUserCache(_pointCache[insertIndex]); | 142 | //ClearUserCache(_pointCache[insertIndex]); |
143 | //_pointCache[insertIndex] = newPoint; | 143 | //_pointCache[insertIndex] = newPoint; |
144 | } | 144 | } |
145 | 145 | ||
146 | public bool ValidContactDistance(ManifoldPoint pt) | 146 | public bool ValidContactDistance(ManifoldPoint pt) |
147 | { | 147 | { |
148 | return pt.Distance <= ContactBreakingThreshold; | 148 | return pt.Distance <= ContactBreakingThreshold; |
149 | } | 149 | } |
150 | 150 | ||
151 | // calculated new worldspace coordinates and depth, and reject points that exceed the collision margin | 151 | // calculated new worldspace coordinates and depth, and reject points that exceed the collision margin |
152 | public void RefreshContactPoints(Matrix trA, Matrix trB) | 152 | public void RefreshContactPoints(Matrix trA, Matrix trB) |
153 | { | 153 | { |
154 | // first refresh worldspace positions and distance | 154 | // first refresh worldspace positions and distance |
155 | for (int i = ContactsCount - 1; i >= 0; i--) | 155 | for (int i = ContactsCount - 1; i >= 0; i--) |
156 | { | 156 | { |
157 | ManifoldPoint manifoldPoint = _pointCache[i]; | 157 | ManifoldPoint manifoldPoint = _pointCache[i]; |
158 | manifoldPoint.PositionWorldOnA = MathHelper.MatrixToVector(trA,manifoldPoint.LocalPointA); | 158 | manifoldPoint.PositionWorldOnA = MathHelper.MatrixToVector(trA,manifoldPoint.LocalPointA); |
159 | manifoldPoint.PositionWorldOnB = MathHelper.MatrixToVector(trB, manifoldPoint.LocalPointB); | 159 | manifoldPoint.PositionWorldOnB = MathHelper.MatrixToVector(trB, manifoldPoint.LocalPointB); |
160 | manifoldPoint.Distance = Vector3.Dot(manifoldPoint.PositionWorldOnA - manifoldPoint.PositionWorldOnB, manifoldPoint.NormalWorldOnB); | 160 | manifoldPoint.Distance = Vector3.Dot(manifoldPoint.PositionWorldOnA - manifoldPoint.PositionWorldOnB, manifoldPoint.NormalWorldOnB); |
161 | manifoldPoint.LifeTime++; | 161 | manifoldPoint.LifeTime++; |
162 | } | 162 | } |
163 | 163 | ||
164 | // then | 164 | // then |
165 | float distance2d; | 165 | float distance2d; |
166 | Vector3 projectedDifference, projectedPoint; | 166 | Vector3 projectedDifference, projectedPoint; |
167 | for (int i = ContactsCount - 1; i >= 0; i--) | 167 | for (int i = ContactsCount - 1; i >= 0; i--) |
168 | { | 168 | { |
169 | 169 | ||
170 | ManifoldPoint manifoldPoint = _pointCache[i]; | 170 | ManifoldPoint manifoldPoint = _pointCache[i]; |
171 | //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction) | 171 | //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction) |
172 | if (!ValidContactDistance(manifoldPoint)) | 172 | if (!ValidContactDistance(manifoldPoint)) |
173 | { | 173 | { |
174 | RemoveContactPoint(i); | 174 | RemoveContactPoint(i); |
175 | } | 175 | } |
176 | else | 176 | else |
177 | { | 177 | { |
178 | //contact also becomes invalid when relative movement orthogonal to normal exceeds margin | 178 | //contact also becomes invalid when relative movement orthogonal to normal exceeds margin |
179 | projectedPoint = manifoldPoint.PositionWorldOnA - manifoldPoint.NormalWorldOnB * manifoldPoint.Distance; | 179 | projectedPoint = manifoldPoint.PositionWorldOnA - manifoldPoint.NormalWorldOnB * manifoldPoint.Distance; |
180 | projectedDifference = manifoldPoint.PositionWorldOnB - projectedPoint; | 180 | projectedDifference = manifoldPoint.PositionWorldOnB - projectedPoint; |
181 | distance2d = Vector3.Dot(projectedDifference, projectedDifference); | 181 | distance2d = Vector3.Dot(projectedDifference, projectedDifference); |
182 | if (distance2d > ContactBreakingThreshold * ContactBreakingThreshold) | 182 | if (distance2d > ContactBreakingThreshold * ContactBreakingThreshold) |
183 | { | 183 | { |
184 | RemoveContactPoint(i); | 184 | RemoveContactPoint(i); |
185 | } | 185 | } |
186 | } | 186 | } |
187 | } | 187 | } |
188 | } | 188 | } |
189 | 189 | ||
190 | public void ClearManifold() | 190 | public void ClearManifold() |
191 | { | 191 | { |
192 | for (int i = 0; i < _cachedPoints; i++) | 192 | for (int i = 0; i < _cachedPoints; i++) |
193 | { | 193 | { |
194 | ClearUserCache(_pointCache[i]); | 194 | ClearUserCache(_pointCache[i]); |
195 | } | 195 | } |
196 | _cachedPoints = 0; | 196 | _cachedPoints = 0; |
197 | } | 197 | } |
198 | 198 | ||
199 | private void ClearUserCache(ManifoldPoint pt) | 199 | private void ClearUserCache(ManifoldPoint pt) |
200 | { | 200 | { |
201 | if (pt != null) | 201 | if (pt != null) |
202 | { | 202 | { |
203 | object oldPtr = pt.UserPersistentData; | 203 | object oldPtr = pt.UserPersistentData; |
204 | 204 | ||
205 | if (oldPtr != null) | 205 | if (oldPtr != null) |
206 | { | 206 | { |
207 | if (pt.UserPersistentData != null && _contactDestroyedCallback != null) | 207 | if (pt.UserPersistentData != null && _contactDestroyedCallback != null) |
208 | { | 208 | { |
209 | _contactDestroyedCallback(pt.UserPersistentData); | 209 | _contactDestroyedCallback(pt.UserPersistentData); |
210 | pt.UserPersistentData = null; | 210 | pt.UserPersistentData = null; |
211 | } | 211 | } |
212 | } | 212 | } |
213 | } | 213 | } |
214 | } | 214 | } |
215 | 215 | ||
216 | // sort cached points so most isolated points come first | 216 | // sort cached points so most isolated points come first |
217 | private int SortCachedPoints(ManifoldPoint pt) | 217 | private int SortCachedPoints(ManifoldPoint pt) |
218 | { | 218 | { |
219 | //calculate 4 possible cases areas, and take biggest area | 219 | //calculate 4 possible cases areas, and take biggest area |
220 | //also need to keep 'deepest' | 220 | //also need to keep 'deepest' |
221 | 221 | ||
222 | int maxPenetrationIndex = -1; | 222 | int maxPenetrationIndex = -1; |
223 | float maxPenetration = pt.Distance; | 223 | float maxPenetration = pt.Distance; |
224 | for (int i = 0; i < 4; i++) | 224 | for (int i = 0; i < 4; i++) |
225 | { | 225 | { |
226 | if (_pointCache[i].Distance < maxPenetration) | 226 | if (_pointCache[i].Distance < maxPenetration) |
227 | { | 227 | { |
228 | maxPenetrationIndex = i; | 228 | maxPenetrationIndex = i; |
229 | maxPenetration = _pointCache[i].Distance; | 229 | maxPenetration = _pointCache[i].Distance; |
230 | } | 230 | } |
231 | } | 231 | } |
232 | 232 | ||
233 | float res0 = 0, res1 = 0, res2 = 0, res3 = 0; | 233 | float res0 = 0, res1 = 0, res2 = 0, res3 = 0; |
234 | if (maxPenetrationIndex != 0) | 234 | if (maxPenetrationIndex != 0) |
235 | { | 235 | { |
236 | Vector3 a0 = pt.LocalPointA - _pointCache[1].LocalPointA; | 236 | Vector3 a0 = pt.LocalPointA - _pointCache[1].LocalPointA; |
237 | Vector3 b0 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; | 237 | Vector3 b0 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; |
238 | Vector3 cross = Vector3.Cross(a0, b0); | 238 | Vector3 cross = Vector3.Cross(a0, b0); |
239 | res0 = cross.LengthSquared(); | 239 | res0 = cross.LengthSquared(); |
240 | } | 240 | } |
241 | if (maxPenetrationIndex != 1) | 241 | if (maxPenetrationIndex != 1) |
242 | { | 242 | { |
243 | Vector3 a1 = pt.LocalPointA - _pointCache[0].LocalPointA; | 243 | Vector3 a1 = pt.LocalPointA - _pointCache[0].LocalPointA; |
244 | Vector3 b1 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; | 244 | Vector3 b1 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; |
245 | Vector3 cross = Vector3.Cross(a1, b1); | 245 | Vector3 cross = Vector3.Cross(a1, b1); |
246 | res1 = cross.LengthSquared(); | 246 | res1 = cross.LengthSquared(); |
247 | } | 247 | } |
248 | 248 | ||
249 | if (maxPenetrationIndex != 2) | 249 | if (maxPenetrationIndex != 2) |
250 | { | 250 | { |
251 | Vector3 a2 = pt.LocalPointA - _pointCache[0].LocalPointA; | 251 | Vector3 a2 = pt.LocalPointA - _pointCache[0].LocalPointA; |
252 | Vector3 b2 = _pointCache[3].LocalPointA - _pointCache[1].LocalPointA; | 252 | Vector3 b2 = _pointCache[3].LocalPointA - _pointCache[1].LocalPointA; |
253 | Vector3 cross = Vector3.Cross(a2, b2); | 253 | Vector3 cross = Vector3.Cross(a2, b2); |
254 | res2 = cross.LengthSquared(); | 254 | res2 = cross.LengthSquared(); |
255 | } | 255 | } |
256 | 256 | ||
257 | if (maxPenetrationIndex != 3) | 257 | if (maxPenetrationIndex != 3) |
258 | { | 258 | { |
259 | Vector3 a3 = pt.LocalPointA - _pointCache[0].LocalPointA; | 259 | Vector3 a3 = pt.LocalPointA - _pointCache[0].LocalPointA; |
260 | Vector3 b3 = _pointCache[2].LocalPointA - _pointCache[1].LocalPointA; | 260 | Vector3 b3 = _pointCache[2].LocalPointA - _pointCache[1].LocalPointA; |
261 | Vector3 cross = Vector3.Cross(a3, b3); | 261 | Vector3 cross = Vector3.Cross(a3, b3); |
262 | res3 = cross.LengthSquared(); | 262 | res3 = cross.LengthSquared(); |
263 | } | 263 | } |
264 | 264 | ||
265 | Vector4 maxvec = new Vector4(res0, res1, res2, res3); | 265 | Vector4 maxvec = new Vector4(res0, res1, res2, res3); |
266 | int biggestarea = MathHelper.ClosestAxis(maxvec); | 266 | int biggestarea = MathHelper.ClosestAxis(maxvec); |
267 | return biggestarea; | 267 | return biggestarea; |
268 | } | 268 | } |
269 | 269 | ||
270 | private int FindContactPoint(ManifoldPoint unUsed, int numUnused, ManifoldPoint pt) { return 0; } | 270 | private int FindContactPoint(ManifoldPoint unUsed, int numUnused, ManifoldPoint pt) { return 0; } |
271 | } | 271 | } |
272 | } | 272 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs index 4e0b0a0..c70741e 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs | |||
@@ -1,64 +1,64 @@ | |||
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 | public class PointCollector : DiscreteCollisionDetectorInterface.Result | 29 | public class PointCollector : DiscreteCollisionDetectorInterface.Result |
30 | { | 30 | { |
31 | private Vector3 _normalOnBInWorld; | 31 | private Vector3 _normalOnBInWorld; |
32 | private Vector3 _pointInWorld; | 32 | private Vector3 _pointInWorld; |
33 | private float _distance; //negative means penetration | 33 | private float _distance; //negative means penetration |
34 | private bool _hasResult; | 34 | private bool _hasResult; |
35 | 35 | ||
36 | public PointCollector() | 36 | public PointCollector() |
37 | { | 37 | { |
38 | _distance = 1e30f; | 38 | _distance = 1e30f; |
39 | _hasResult = false; | 39 | _hasResult = false; |
40 | } | 40 | } |
41 | 41 | ||
42 | public Vector3 NormalOnBInWorld { get { return _normalOnBInWorld; } } | 42 | public Vector3 NormalOnBInWorld { get { return _normalOnBInWorld; } } |
43 | public Vector3 PointInWorld { get { return _pointInWorld; } } | 43 | public Vector3 PointInWorld { get { return _pointInWorld; } } |
44 | public float Distance { get { return _distance; } } | 44 | public float Distance { get { return _distance; } } |
45 | public bool HasResult { get { return _hasResult; } } | 45 | public bool HasResult { get { return _hasResult; } } |
46 | 46 | ||
47 | public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB) | 47 | public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB) |
48 | { | 48 | { |
49 | //?? | 49 | //?? |
50 | } | 50 | } |
51 | 51 | ||
52 | public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) | 52 | public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) |
53 | { | 53 | { |
54 | if (depth < _distance) | 54 | if (depth < _distance) |
55 | { | 55 | { |
56 | _hasResult = true; | 56 | _hasResult = true; |
57 | _normalOnBInWorld = normalOnBInWorld; | 57 | _normalOnBInWorld = normalOnBInWorld; |
58 | _pointInWorld = pointInWorld; | 58 | _pointInWorld = pointInWorld; |
59 | //negative means penetration | 59 | //negative means penetration |
60 | _distance = depth; | 60 | _distance = depth; |
61 | } | 61 | } |
62 | } | 62 | } |
63 | } | 63 | } |
64 | } | 64 | } |
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 | ||
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 | /// 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 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs index c127460..1e0fb3b 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs | |||
@@ -1,115 +1,115 @@ | |||
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 | public abstract class TriangleRaycastCallback : ITriangleCallback | 29 | public abstract class TriangleRaycastCallback : ITriangleCallback |
30 | { | 30 | { |
31 | private Vector3 _from; | 31 | private Vector3 _from; |
32 | private Vector3 _to; | 32 | private Vector3 _to; |
33 | private float _hitFraction; | 33 | private float _hitFraction; |
34 | 34 | ||
35 | public TriangleRaycastCallback(Vector3 from, Vector3 to) | 35 | public TriangleRaycastCallback(Vector3 from, Vector3 to) |
36 | { | 36 | { |
37 | _from = from; | 37 | _from = from; |
38 | _to = to; | 38 | _to = to; |
39 | _hitFraction = 1; | 39 | _hitFraction = 1; |
40 | } | 40 | } |
41 | 41 | ||
42 | public Vector3 From { get { return _from; } set { _from = value; } } | 42 | public Vector3 From { get { return _from; } set { _from = value; } } |
43 | public Vector3 To { get { return _to; } set { _to = value; } } | 43 | public Vector3 To { get { return _to; } set { _to = value; } } |
44 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } | 44 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } |
45 | 45 | ||
46 | public abstract float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex); | 46 | public abstract float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex); |
47 | 47 | ||
48 | #region ITriangleCallback Members | 48 | #region ITriangleCallback Members |
49 | 49 | ||
50 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) | 50 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) |
51 | { | 51 | { |
52 | Vector3 vertA = triangle[0]; | 52 | Vector3 vertA = triangle[0]; |
53 | Vector3 vertB = triangle[1]; | 53 | Vector3 vertB = triangle[1]; |
54 | Vector3 vertC = triangle[2]; | 54 | Vector3 vertC = triangle[2]; |
55 | 55 | ||
56 | Vector3 vBA = vertB - vertA; | 56 | Vector3 vBA = vertB - vertA; |
57 | Vector3 vCA = vertC - vertA; | 57 | Vector3 vCA = vertC - vertA; |
58 | 58 | ||
59 | Vector3 triangleNormal = Vector3.Cross(vBA, vCA); | 59 | Vector3 triangleNormal = Vector3.Cross(vBA, vCA); |
60 | 60 | ||
61 | float dist = Vector3.Dot(vertA, triangleNormal); | 61 | float dist = Vector3.Dot(vertA, triangleNormal); |
62 | float distA = Vector3.Dot(triangleNormal, _from); | 62 | float distA = Vector3.Dot(triangleNormal, _from); |
63 | distA -= dist; | 63 | distA -= dist; |
64 | float distB = Vector3.Dot(triangleNormal, _to); | 64 | float distB = Vector3.Dot(triangleNormal, _to); |
65 | distB -= dist; | 65 | distB -= dist; |
66 | 66 | ||
67 | if (distA * distB >= 0.0f) | 67 | if (distA * distB >= 0.0f) |
68 | { | 68 | { |
69 | return; // same sign | 69 | return; // same sign |
70 | } | 70 | } |
71 | 71 | ||
72 | float projLength = distA - distB; | 72 | float projLength = distA - distB; |
73 | float distance = (distA) / (projLength); | 73 | float distance = (distA) / (projLength); |
74 | // Now we have the intersection point on the plane, we'll see if it's inside the triangle | 74 | // Now we have the intersection point on the plane, we'll see if it's inside the triangle |
75 | // Add an epsilon as a tolerance for the raycast, | 75 | // Add an epsilon as a tolerance for the raycast, |
76 | // in case the ray hits exacly on the edge of the triangle. | 76 | // in case the ray hits exacly on the edge of the triangle. |
77 | // It must be scaled for the triangle size. | 77 | // It must be scaled for the triangle size. |
78 | 78 | ||
79 | if (distance < _hitFraction) | 79 | if (distance < _hitFraction) |
80 | { | 80 | { |
81 | float edgeTolerance = triangleNormal.LengthSquared(); | 81 | float edgeTolerance = triangleNormal.LengthSquared(); |
82 | edgeTolerance *= -0.0001f; | 82 | edgeTolerance *= -0.0001f; |
83 | Vector3 point = new Vector3(); | 83 | Vector3 point = new Vector3(); |
84 | MathHelper.SetInterpolate3(_from, _to, distance, ref point); | 84 | MathHelper.SetInterpolate3(_from, _to, distance, ref point); |
85 | 85 | ||
86 | Vector3 vertexAPoint = vertA - point; | 86 | Vector3 vertexAPoint = vertA - point; |
87 | Vector3 vertexBPoint = vertB - point; | 87 | Vector3 vertexBPoint = vertB - point; |
88 | Vector3 contactPointA = Vector3.Cross(vertexAPoint, vertexBPoint); | 88 | Vector3 contactPointA = Vector3.Cross(vertexAPoint, vertexBPoint); |
89 | 89 | ||
90 | if (Vector3.Dot(contactPointA, triangleNormal) >= edgeTolerance) | 90 | if (Vector3.Dot(contactPointA, triangleNormal) >= edgeTolerance) |
91 | { | 91 | { |
92 | Vector3 vertexCPoint = vertC - point; | 92 | Vector3 vertexCPoint = vertC - point; |
93 | Vector3 contactPointB = Vector3.Cross(vertexBPoint, vertexCPoint); | 93 | Vector3 contactPointB = Vector3.Cross(vertexBPoint, vertexCPoint); |
94 | if (Vector3.Dot(contactPointB, triangleNormal) >= edgeTolerance) | 94 | if (Vector3.Dot(contactPointB, triangleNormal) >= edgeTolerance) |
95 | { | 95 | { |
96 | Vector3 contactPointC = Vector3.Cross(vertexCPoint, vertexAPoint); | 96 | Vector3 contactPointC = Vector3.Cross(vertexCPoint, vertexAPoint); |
97 | 97 | ||
98 | if (Vector3.Dot(contactPointC, triangleNormal) >= edgeTolerance) | 98 | if (Vector3.Dot(contactPointC, triangleNormal) >= edgeTolerance) |
99 | { | 99 | { |
100 | if (distA > 0) | 100 | if (distA > 0) |
101 | { | 101 | { |
102 | _hitFraction = ReportHit(triangleNormal, distance, partID, triangleIndex); | 102 | _hitFraction = ReportHit(triangleNormal, distance, partID, triangleIndex); |
103 | } | 103 | } |
104 | else | 104 | else |
105 | { | 105 | { |
106 | _hitFraction = ReportHit(-triangleNormal, distance, partID, triangleIndex); | 106 | _hitFraction = ReportHit(-triangleNormal, distance, partID, triangleIndex); |
107 | } | 107 | } |
108 | } | 108 | } |
109 | } | 109 | } |
110 | } | 110 | } |
111 | } | 111 | } |
112 | } | 112 | } |
113 | #endregion | 113 | #endregion |
114 | } | 114 | } |
115 | } | 115 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs index 16f3dab..1bc94d3 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs | |||
@@ -1,643 +1,643 @@ | |||
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 | public class UsageBitfield | 29 | public class UsageBitfield |
30 | { | 30 | { |
31 | private bool _usedVertexA, _usedVertexB, _usedVertexC, _usedVertexD; | 31 | private bool _usedVertexA, _usedVertexB, _usedVertexC, _usedVertexD; |
32 | 32 | ||
33 | public bool UsedVertexA { get { return _usedVertexA; } set { _usedVertexA = value; } } | 33 | public bool UsedVertexA { get { return _usedVertexA; } set { _usedVertexA = value; } } |
34 | public bool UsedVertexB { get { return _usedVertexB; } set { _usedVertexB = value; } } | 34 | public bool UsedVertexB { get { return _usedVertexB; } set { _usedVertexB = value; } } |
35 | public bool UsedVertexC { get { return _usedVertexC; } set { _usedVertexC = value; } } | 35 | public bool UsedVertexC { get { return _usedVertexC; } set { _usedVertexC = value; } } |
36 | public bool UsedVertexD { get { return _usedVertexD; } set { _usedVertexD = value; } } | 36 | public bool UsedVertexD { get { return _usedVertexD; } set { _usedVertexD = value; } } |
37 | 37 | ||
38 | public void Reset() | 38 | public void Reset() |
39 | { | 39 | { |
40 | _usedVertexA = _usedVertexB = _usedVertexC = _usedVertexD = false; | 40 | _usedVertexA = _usedVertexB = _usedVertexC = _usedVertexD = false; |
41 | } | 41 | } |
42 | } | 42 | } |
43 | 43 | ||
44 | public class SubSimplexClosestResult | 44 | public class SubSimplexClosestResult |
45 | { | 45 | { |
46 | private Vector3 _closestPointOnSimplex; | 46 | private Vector3 _closestPointOnSimplex; |
47 | 47 | ||
48 | //MASK for m_usedVertices | 48 | //MASK for m_usedVertices |
49 | //stores the simplex vertex-usage, using the MASK, | 49 | //stores the simplex vertex-usage, using the MASK, |
50 | // if m_usedVertices & MASK then the related vertex is used | 50 | // if m_usedVertices & MASK then the related vertex is used |
51 | private UsageBitfield _usedVertices = new UsageBitfield(); | 51 | private UsageBitfield _usedVertices = new UsageBitfield(); |
52 | private float[] _barycentricCoords = new float[4]; | 52 | private float[] _barycentricCoords = new float[4]; |
53 | private bool _degenerate; | 53 | private bool _degenerate; |
54 | 54 | ||
55 | public Vector3 ClosestPointOnSimplex { get { return _closestPointOnSimplex; } set { _closestPointOnSimplex = value; } } | 55 | public Vector3 ClosestPointOnSimplex { get { return _closestPointOnSimplex; } set { _closestPointOnSimplex = value; } } |
56 | public UsageBitfield UsedVertices { get { return _usedVertices; } set { _usedVertices = value; } } | 56 | public UsageBitfield UsedVertices { get { return _usedVertices; } set { _usedVertices = value; } } |
57 | public float[] BarycentricCoords { get { return _barycentricCoords; } set { _barycentricCoords = value; } } | 57 | public float[] BarycentricCoords { get { return _barycentricCoords; } set { _barycentricCoords = value; } } |
58 | public bool Degenerate { get { return _degenerate; } set { _degenerate = value; } } | 58 | public bool Degenerate { get { return _degenerate; } set { _degenerate = value; } } |
59 | 59 | ||
60 | public void Reset() | 60 | public void Reset() |
61 | { | 61 | { |
62 | _degenerate = false; | 62 | _degenerate = false; |
63 | SetBarycentricCoordinates(); | 63 | SetBarycentricCoordinates(); |
64 | _usedVertices.Reset(); | 64 | _usedVertices.Reset(); |
65 | } | 65 | } |
66 | 66 | ||
67 | public bool IsValid | 67 | public bool IsValid |
68 | { | 68 | { |
69 | get | 69 | get |
70 | { | 70 | { |
71 | return (_barycentricCoords[0] >= 0f) && | 71 | return (_barycentricCoords[0] >= 0f) && |
72 | (_barycentricCoords[1] >= 0f) && | 72 | (_barycentricCoords[1] >= 0f) && |
73 | (_barycentricCoords[2] >= 0f) && | 73 | (_barycentricCoords[2] >= 0f) && |
74 | (_barycentricCoords[3] >= 0f); | 74 | (_barycentricCoords[3] >= 0f); |
75 | } | 75 | } |
76 | } | 76 | } |
77 | 77 | ||
78 | public void SetBarycentricCoordinates() | 78 | public void SetBarycentricCoordinates() |
79 | { | 79 | { |
80 | SetBarycentricCoordinates(0f, 0f, 0f, 0f); | 80 | SetBarycentricCoordinates(0f, 0f, 0f, 0f); |
81 | } | 81 | } |
82 | 82 | ||
83 | public void SetBarycentricCoordinates(float a, float b, float c, float d) | 83 | public void SetBarycentricCoordinates(float a, float b, float c, float d) |
84 | { | 84 | { |
85 | _barycentricCoords[0] = a; | 85 | _barycentricCoords[0] = a; |
86 | _barycentricCoords[1] = b; | 86 | _barycentricCoords[1] = b; |
87 | _barycentricCoords[2] = c; | 87 | _barycentricCoords[2] = c; |
88 | _barycentricCoords[3] = d; | 88 | _barycentricCoords[3] = d; |
89 | } | 89 | } |
90 | } | 90 | } |
91 | 91 | ||
92 | /// VoronoiSimplexSolver is an implementation of the closest point distance | 92 | /// VoronoiSimplexSolver is an implementation of the closest point distance |
93 | /// algorithm from a 1-4 points simplex to the origin. | 93 | /// algorithm from a 1-4 points simplex to the origin. |
94 | /// Can be used with GJK, as an alternative to Johnson distance algorithm. | 94 | /// Can be used with GJK, as an alternative to Johnson distance algorithm. |
95 | public class VoronoiSimplexSolver : ISimplexSolver | 95 | public class VoronoiSimplexSolver : ISimplexSolver |
96 | { | 96 | { |
97 | private const int VertexA = 0, VertexB = 1, VertexC = 2, VertexD = 3; | 97 | private const int VertexA = 0, VertexB = 1, VertexC = 2, VertexD = 3; |
98 | 98 | ||
99 | private const int VoronoiSimplexMaxVerts = 5; | 99 | private const int VoronoiSimplexMaxVerts = 5; |
100 | private const bool CatchDegenerateTetrahedron = true; | 100 | private const bool CatchDegenerateTetrahedron = true; |
101 | 101 | ||
102 | private int _numVertices; | 102 | private int _numVertices; |
103 | 103 | ||
104 | private Vector3[] _simplexVectorW = new Vector3[VoronoiSimplexMaxVerts]; | 104 | private Vector3[] _simplexVectorW = new Vector3[VoronoiSimplexMaxVerts]; |
105 | private Vector3[] _simplexPointsP = new Vector3[VoronoiSimplexMaxVerts]; | 105 | private Vector3[] _simplexPointsP = new Vector3[VoronoiSimplexMaxVerts]; |
106 | private Vector3[] _simplexPointsQ = new Vector3[VoronoiSimplexMaxVerts]; | 106 | private Vector3[] _simplexPointsQ = new Vector3[VoronoiSimplexMaxVerts]; |
107 | 107 | ||
108 | private Vector3 _cachedPA; | 108 | private Vector3 _cachedPA; |
109 | private Vector3 _cachedPB; | 109 | private Vector3 _cachedPB; |
110 | private Vector3 _cachedV; | 110 | private Vector3 _cachedV; |
111 | private Vector3 _lastW; | 111 | private Vector3 _lastW; |
112 | private bool _cachedValidClosest; | 112 | private bool _cachedValidClosest; |
113 | 113 | ||
114 | private SubSimplexClosestResult _cachedBC = new SubSimplexClosestResult(); | 114 | private SubSimplexClosestResult _cachedBC = new SubSimplexClosestResult(); |
115 | 115 | ||
116 | private bool _needsUpdate; | 116 | private bool _needsUpdate; |
117 | 117 | ||
118 | #region ISimplexSolver Members | 118 | #region ISimplexSolver Members |
119 | 119 | ||
120 | public bool FullSimplex | 120 | public bool FullSimplex |
121 | { | 121 | { |
122 | get | 122 | get |
123 | { | 123 | { |
124 | return _numVertices == 4; | 124 | return _numVertices == 4; |
125 | } | 125 | } |
126 | } | 126 | } |
127 | 127 | ||
128 | public int NumVertices | 128 | public int NumVertices |
129 | { | 129 | { |
130 | get | 130 | get |
131 | { | 131 | { |
132 | return _numVertices; | 132 | return _numVertices; |
133 | } | 133 | } |
134 | } | 134 | } |
135 | 135 | ||
136 | public void Reset() | 136 | public void Reset() |
137 | { | 137 | { |
138 | _cachedValidClosest = false; | 138 | _cachedValidClosest = false; |
139 | _numVertices = 0; | 139 | _numVertices = 0; |
140 | _needsUpdate = true; | 140 | _needsUpdate = true; |
141 | _lastW = new Vector3(1e30f, 1e30f, 1e30f); | 141 | _lastW = new Vector3(1e30f, 1e30f, 1e30f); |
142 | _cachedBC.Reset(); | 142 | _cachedBC.Reset(); |
143 | } | 143 | } |
144 | 144 | ||
145 | public void AddVertex(Vector3 w, Vector3 p, Vector3 q) | 145 | public void AddVertex(Vector3 w, Vector3 p, Vector3 q) |
146 | { | 146 | { |
147 | _lastW = w; | 147 | _lastW = w; |
148 | _needsUpdate = true; | 148 | _needsUpdate = true; |
149 | 149 | ||
150 | _simplexVectorW[_numVertices] = w; | 150 | _simplexVectorW[_numVertices] = w; |
151 | _simplexPointsP[_numVertices] = p; | 151 | _simplexPointsP[_numVertices] = p; |
152 | _simplexPointsQ[_numVertices] = q; | 152 | _simplexPointsQ[_numVertices] = q; |
153 | 153 | ||
154 | _numVertices++; | 154 | _numVertices++; |
155 | } | 155 | } |
156 | 156 | ||
157 | //return/calculate the closest vertex | 157 | //return/calculate the closest vertex |
158 | public bool Closest(out Vector3 v) | 158 | public bool Closest(out Vector3 v) |
159 | { | 159 | { |
160 | bool succes = UpdateClosestVectorAndPoints(); | 160 | bool succes = UpdateClosestVectorAndPoints(); |
161 | v = _cachedV; | 161 | v = _cachedV; |
162 | return succes; | 162 | return succes; |
163 | } | 163 | } |
164 | 164 | ||
165 | public float MaxVertex | 165 | public float MaxVertex |
166 | { | 166 | { |
167 | get | 167 | get |
168 | { | 168 | { |
169 | int numverts = NumVertices; | 169 | int numverts = NumVertices; |
170 | float maxV = 0f, curLen2; | 170 | float maxV = 0f, curLen2; |
171 | for (int i = 0; i < numverts; i++) | 171 | for (int i = 0; i < numverts; i++) |
172 | { | 172 | { |
173 | curLen2 = _simplexVectorW[i].LengthSquared(); | 173 | curLen2 = _simplexVectorW[i].LengthSquared(); |
174 | if (maxV < curLen2) maxV = curLen2; | 174 | if (maxV < curLen2) maxV = curLen2; |
175 | } | 175 | } |
176 | return maxV; | 176 | return maxV; |
177 | } | 177 | } |
178 | } | 178 | } |
179 | 179 | ||
180 | //return the current simplex | 180 | //return the current simplex |
181 | public int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf) | 181 | public int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf) |
182 | { | 182 | { |
183 | int numverts = NumVertices; | 183 | int numverts = NumVertices; |
184 | pBuf = new Vector3[numverts]; | 184 | pBuf = new Vector3[numverts]; |
185 | qBuf = new Vector3[numverts]; | 185 | qBuf = new Vector3[numverts]; |
186 | yBuf = new Vector3[numverts]; | 186 | yBuf = new Vector3[numverts]; |
187 | for (int i = 0; i < numverts; i++) | 187 | for (int i = 0; i < numverts; i++) |
188 | { | 188 | { |
189 | yBuf[i] = _simplexVectorW[i]; | 189 | yBuf[i] = _simplexVectorW[i]; |
190 | pBuf[i] = _simplexPointsP[i]; | 190 | pBuf[i] = _simplexPointsP[i]; |
191 | qBuf[i] = _simplexPointsQ[i]; | 191 | qBuf[i] = _simplexPointsQ[i]; |
192 | } | 192 | } |
193 | return numverts; | 193 | return numverts; |
194 | } | 194 | } |
195 | 195 | ||
196 | public bool InSimplex(Vector3 w) | 196 | public bool InSimplex(Vector3 w) |
197 | { | 197 | { |
198 | //check in case lastW is already removed | 198 | //check in case lastW is already removed |
199 | if (w == _lastW) return true; | 199 | if (w == _lastW) return true; |
200 | 200 | ||
201 | //w is in the current (reduced) simplex | 201 | //w is in the current (reduced) simplex |
202 | int numverts = NumVertices; | 202 | int numverts = NumVertices; |
203 | for (int i = 0; i < numverts; i++) | 203 | for (int i = 0; i < numverts; i++) |
204 | if (_simplexVectorW[i] == w) return true; | 204 | if (_simplexVectorW[i] == w) return true; |
205 | 205 | ||
206 | return false; | 206 | return false; |
207 | } | 207 | } |
208 | 208 | ||
209 | public void BackupClosest(out Vector3 v) | 209 | public void BackupClosest(out Vector3 v) |
210 | { | 210 | { |
211 | v = _cachedV; | 211 | v = _cachedV; |
212 | } | 212 | } |
213 | 213 | ||
214 | public bool EmptySimplex | 214 | public bool EmptySimplex |
215 | { | 215 | { |
216 | get | 216 | get |
217 | { | 217 | { |
218 | return NumVertices == 0; | 218 | return NumVertices == 0; |
219 | } | 219 | } |
220 | } | 220 | } |
221 | 221 | ||
222 | public void ComputePoints(out Vector3 p1, out Vector3 p2) | 222 | public void ComputePoints(out Vector3 p1, out Vector3 p2) |
223 | { | 223 | { |
224 | UpdateClosestVectorAndPoints(); | 224 | UpdateClosestVectorAndPoints(); |
225 | p1 = _cachedPA; | 225 | p1 = _cachedPA; |
226 | p2 = _cachedPB; | 226 | p2 = _cachedPB; |
227 | } | 227 | } |
228 | 228 | ||
229 | #endregion | 229 | #endregion |
230 | 230 | ||
231 | public void RemoveVertex(int index) | 231 | public void RemoveVertex(int index) |
232 | { | 232 | { |
233 | BulletDebug.Assert(_numVertices > 0); | 233 | BulletDebug.Assert(_numVertices > 0); |
234 | _numVertices--; | 234 | _numVertices--; |
235 | _simplexVectorW[index] = _simplexVectorW[_numVertices]; | 235 | _simplexVectorW[index] = _simplexVectorW[_numVertices]; |
236 | _simplexPointsP[index] = _simplexPointsP[_numVertices]; | 236 | _simplexPointsP[index] = _simplexPointsP[_numVertices]; |
237 | _simplexPointsQ[index] = _simplexPointsQ[_numVertices]; | 237 | _simplexPointsQ[index] = _simplexPointsQ[_numVertices]; |
238 | } | 238 | } |
239 | 239 | ||
240 | public void ReduceVertices(UsageBitfield usedVerts) | 240 | public void ReduceVertices(UsageBitfield usedVerts) |
241 | { | 241 | { |
242 | if ((NumVertices >= 4) && (!usedVerts.UsedVertexD)) RemoveVertex(3); | 242 | if ((NumVertices >= 4) && (!usedVerts.UsedVertexD)) RemoveVertex(3); |
243 | if ((NumVertices >= 3) && (!usedVerts.UsedVertexC)) RemoveVertex(2); | 243 | if ((NumVertices >= 3) && (!usedVerts.UsedVertexC)) RemoveVertex(2); |
244 | if ((NumVertices >= 2) && (!usedVerts.UsedVertexB)) RemoveVertex(1); | 244 | if ((NumVertices >= 2) && (!usedVerts.UsedVertexB)) RemoveVertex(1); |
245 | if ((NumVertices >= 1) && (!usedVerts.UsedVertexA)) RemoveVertex(0); | 245 | if ((NumVertices >= 1) && (!usedVerts.UsedVertexA)) RemoveVertex(0); |
246 | } | 246 | } |
247 | 247 | ||
248 | public bool UpdateClosestVectorAndPoints() | 248 | public bool UpdateClosestVectorAndPoints() |
249 | { | 249 | { |
250 | if (_needsUpdate) | 250 | if (_needsUpdate) |
251 | { | 251 | { |
252 | _cachedBC.Reset(); | 252 | _cachedBC.Reset(); |
253 | _needsUpdate = false; | 253 | _needsUpdate = false; |
254 | 254 | ||
255 | Vector3 p, a, b, c, d; | 255 | Vector3 p, a, b, c, d; |
256 | switch (NumVertices) | 256 | switch (NumVertices) |
257 | { | 257 | { |
258 | case 0: | 258 | case 0: |
259 | _cachedValidClosest = false; | 259 | _cachedValidClosest = false; |
260 | break; | 260 | break; |
261 | case 1: | 261 | case 1: |
262 | _cachedPA = _simplexPointsP[0]; | 262 | _cachedPA = _simplexPointsP[0]; |
263 | _cachedPB = _simplexPointsQ[0]; | 263 | _cachedPB = _simplexPointsQ[0]; |
264 | _cachedV = _cachedPA - _cachedPB; | 264 | _cachedV = _cachedPA - _cachedPB; |
265 | _cachedBC.Reset(); | 265 | _cachedBC.Reset(); |
266 | _cachedBC.SetBarycentricCoordinates(1f, 0f, 0f, 0f); | 266 | _cachedBC.SetBarycentricCoordinates(1f, 0f, 0f, 0f); |
267 | _cachedValidClosest = _cachedBC.IsValid; | 267 | _cachedValidClosest = _cachedBC.IsValid; |
268 | break; | 268 | break; |
269 | case 2: | 269 | case 2: |
270 | //closest point origin from line segment | 270 | //closest point origin from line segment |
271 | Vector3 from = _simplexVectorW[0]; | 271 | Vector3 from = _simplexVectorW[0]; |
272 | Vector3 to = _simplexVectorW[1]; | 272 | Vector3 to = _simplexVectorW[1]; |
273 | Vector3 nearest; | 273 | Vector3 nearest; |
274 | 274 | ||
275 | Vector3 diff = -from; | 275 | Vector3 diff = -from; |
276 | Vector3 v = to - from; | 276 | Vector3 v = to - from; |
277 | float t = Vector3.Dot(v, diff); | 277 | float t = Vector3.Dot(v, diff); |
278 | 278 | ||
279 | if (t > 0) | 279 | if (t > 0) |
280 | { | 280 | { |
281 | float dotVV = v.LengthSquared(); | 281 | float dotVV = v.LengthSquared(); |
282 | if (t < dotVV) | 282 | if (t < dotVV) |
283 | { | 283 | { |
284 | t /= dotVV; | 284 | t /= dotVV; |
285 | diff -= t * v; | 285 | diff -= t * v; |
286 | _cachedBC.UsedVertices.UsedVertexA = true; | 286 | _cachedBC.UsedVertices.UsedVertexA = true; |
287 | _cachedBC.UsedVertices.UsedVertexB = true; | 287 | _cachedBC.UsedVertices.UsedVertexB = true; |
288 | } | 288 | } |
289 | else | 289 | else |
290 | { | 290 | { |
291 | t = 1; | 291 | t = 1; |
292 | diff -= v; | 292 | diff -= v; |
293 | //reduce to 1 point | 293 | //reduce to 1 point |
294 | _cachedBC.UsedVertices.UsedVertexB = true; | 294 | _cachedBC.UsedVertices.UsedVertexB = true; |
295 | } | 295 | } |
296 | } | 296 | } |
297 | else | 297 | else |
298 | { | 298 | { |
299 | t = 0; | 299 | t = 0; |
300 | //reduce to 1 point | 300 | //reduce to 1 point |
301 | _cachedBC.UsedVertices.UsedVertexA = true; | 301 | _cachedBC.UsedVertices.UsedVertexA = true; |
302 | } | 302 | } |
303 | 303 | ||
304 | _cachedBC.SetBarycentricCoordinates(1 - t, t, 0, 0); | 304 | _cachedBC.SetBarycentricCoordinates(1 - t, t, 0, 0); |
305 | nearest = from + t * v; | 305 | nearest = from + t * v; |
306 | 306 | ||
307 | _cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]); | 307 | _cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]); |
308 | _cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]); | 308 | _cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]); |
309 | _cachedV = _cachedPA - _cachedPB; | 309 | _cachedV = _cachedPA - _cachedPB; |
310 | 310 | ||
311 | ReduceVertices(_cachedBC.UsedVertices); | 311 | ReduceVertices(_cachedBC.UsedVertices); |
312 | 312 | ||
313 | _cachedValidClosest = _cachedBC.IsValid; | 313 | _cachedValidClosest = _cachedBC.IsValid; |
314 | break; | 314 | break; |
315 | case 3: | 315 | case 3: |
316 | //closest point origin from triangle | 316 | //closest point origin from triangle |
317 | p = new Vector3(); | 317 | p = new Vector3(); |
318 | a = _simplexVectorW[0]; | 318 | a = _simplexVectorW[0]; |
319 | b = _simplexVectorW[1]; | 319 | b = _simplexVectorW[1]; |
320 | c = _simplexVectorW[2]; | 320 | c = _simplexVectorW[2]; |
321 | 321 | ||
322 | ClosestPtPointTriangle(p, a, b, c, ref _cachedBC); | 322 | ClosestPtPointTriangle(p, a, b, c, ref _cachedBC); |
323 | _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] + | 323 | _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] + |
324 | _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] + | 324 | _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] + |
325 | _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] + | 325 | _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] + |
326 | _simplexPointsP[3] * _cachedBC.BarycentricCoords[3]; | 326 | _simplexPointsP[3] * _cachedBC.BarycentricCoords[3]; |
327 | 327 | ||
328 | _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] + | 328 | _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] + |
329 | _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] + | 329 | _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] + |
330 | _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] + | 330 | _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] + |
331 | _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3]; | 331 | _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3]; |
332 | 332 | ||
333 | _cachedV = _cachedPA - _cachedPB; | 333 | _cachedV = _cachedPA - _cachedPB; |
334 | 334 | ||
335 | ReduceVertices(_cachedBC.UsedVertices); | 335 | ReduceVertices(_cachedBC.UsedVertices); |
336 | _cachedValidClosest = _cachedBC.IsValid; | 336 | _cachedValidClosest = _cachedBC.IsValid; |
337 | break; | 337 | break; |
338 | case 4: | 338 | case 4: |
339 | p = new Vector3(); | 339 | p = new Vector3(); |
340 | a = _simplexVectorW[0]; | 340 | a = _simplexVectorW[0]; |
341 | b = _simplexVectorW[1]; | 341 | b = _simplexVectorW[1]; |
342 | c = _simplexVectorW[2]; | 342 | c = _simplexVectorW[2]; |
343 | d = _simplexVectorW[3]; | 343 | d = _simplexVectorW[3]; |
344 | 344 | ||
345 | bool hasSeperation = ClosestPtPointTetrahedron(p, a, b, c, d, ref _cachedBC); | 345 | bool hasSeperation = ClosestPtPointTetrahedron(p, a, b, c, d, ref _cachedBC); |
346 | 346 | ||
347 | if (hasSeperation) | 347 | if (hasSeperation) |
348 | { | 348 | { |
349 | _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] + | 349 | _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] + |
350 | _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] + | 350 | _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] + |
351 | _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] + | 351 | _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] + |
352 | _simplexPointsP[3] * _cachedBC.BarycentricCoords[3]; | 352 | _simplexPointsP[3] * _cachedBC.BarycentricCoords[3]; |
353 | 353 | ||
354 | _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] + | 354 | _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] + |
355 | _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] + | 355 | _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] + |
356 | _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] + | 356 | _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] + |
357 | _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3]; | 357 | _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3]; |
358 | 358 | ||
359 | _cachedV = _cachedPA - _cachedPB; | 359 | _cachedV = _cachedPA - _cachedPB; |
360 | ReduceVertices(_cachedBC.UsedVertices); | 360 | ReduceVertices(_cachedBC.UsedVertices); |
361 | } | 361 | } |
362 | else | 362 | else |
363 | { | 363 | { |
364 | if (_cachedBC.Degenerate) | 364 | if (_cachedBC.Degenerate) |
365 | { | 365 | { |
366 | _cachedValidClosest = false; | 366 | _cachedValidClosest = false; |
367 | } | 367 | } |
368 | else | 368 | else |
369 | { | 369 | { |
370 | _cachedValidClosest = true; | 370 | _cachedValidClosest = true; |
371 | //degenerate case == false, penetration = true + zero | 371 | //degenerate case == false, penetration = true + zero |
372 | _cachedV.X = _cachedV.Y = _cachedV.Z = 0f; | 372 | _cachedV.X = _cachedV.Y = _cachedV.Z = 0f; |
373 | } | 373 | } |
374 | break; // !!!!!!!!!!!! proverit na vsakiy sluchai | 374 | break; // !!!!!!!!!!!! proverit na vsakiy sluchai |
375 | } | 375 | } |
376 | 376 | ||
377 | _cachedValidClosest = _cachedBC.IsValid; | 377 | _cachedValidClosest = _cachedBC.IsValid; |
378 | 378 | ||
379 | //closest point origin from tetrahedron | 379 | //closest point origin from tetrahedron |
380 | break; | 380 | break; |
381 | default: | 381 | default: |
382 | _cachedValidClosest = false; | 382 | _cachedValidClosest = false; |
383 | break; | 383 | break; |
384 | } | 384 | } |
385 | } | 385 | } |
386 | 386 | ||
387 | return _cachedValidClosest; | 387 | return _cachedValidClosest; |
388 | } | 388 | } |
389 | 389 | ||
390 | public bool ClosestPtPointTriangle(Vector3 p, Vector3 a, Vector3 b, Vector3 c, | 390 | public bool ClosestPtPointTriangle(Vector3 p, Vector3 a, Vector3 b, Vector3 c, |
391 | ref SubSimplexClosestResult result) | 391 | ref SubSimplexClosestResult result) |
392 | { | 392 | { |
393 | result.UsedVertices.Reset(); | 393 | result.UsedVertices.Reset(); |
394 | 394 | ||
395 | float v, w; | 395 | float v, w; |
396 | 396 | ||
397 | // Check if P in vertex region outside A | 397 | // Check if P in vertex region outside A |
398 | Vector3 ab = b - a; | 398 | Vector3 ab = b - a; |
399 | Vector3 ac = c - a; | 399 | Vector3 ac = c - a; |
400 | Vector3 ap = p - a; | 400 | Vector3 ap = p - a; |
401 | float d1 = Vector3.Dot(ab, ap); | 401 | float d1 = Vector3.Dot(ab, ap); |
402 | float d2 = Vector3.Dot(ac, ap); | 402 | float d2 = Vector3.Dot(ac, ap); |
403 | if (d1 <= 0f && d2 <= 0f) | 403 | if (d1 <= 0f && d2 <= 0f) |
404 | { | 404 | { |
405 | result.ClosestPointOnSimplex = a; | 405 | result.ClosestPointOnSimplex = a; |
406 | result.UsedVertices.UsedVertexA = true; | 406 | result.UsedVertices.UsedVertexA = true; |
407 | result.SetBarycentricCoordinates(1, 0, 0, 0); | 407 | result.SetBarycentricCoordinates(1, 0, 0, 0); |
408 | return true; // a; // barycentric coordinates (1,0,0) | 408 | return true; // a; // barycentric coordinates (1,0,0) |
409 | } | 409 | } |
410 | 410 | ||
411 | // Check if P in vertex region outside B | 411 | // Check if P in vertex region outside B |
412 | Vector3 bp = p - b; | 412 | Vector3 bp = p - b; |
413 | float d3 = Vector3.Dot(ab, bp); | 413 | float d3 = Vector3.Dot(ab, bp); |
414 | float d4 = Vector3.Dot(ac, bp); | 414 | float d4 = Vector3.Dot(ac, bp); |
415 | if (d3 >= 0f && d4 <= d3) | 415 | if (d3 >= 0f && d4 <= d3) |
416 | { | 416 | { |
417 | result.ClosestPointOnSimplex = b; | 417 | result.ClosestPointOnSimplex = b; |
418 | result.UsedVertices.UsedVertexB = true; | 418 | result.UsedVertices.UsedVertexB = true; |
419 | result.SetBarycentricCoordinates(0, 1, 0, 0); | 419 | result.SetBarycentricCoordinates(0, 1, 0, 0); |
420 | 420 | ||
421 | return true; // b; // barycentric coordinates (0,1,0) | 421 | return true; // b; // barycentric coordinates (0,1,0) |
422 | } | 422 | } |
423 | // Check if P in edge region of AB, if so return projection of P onto AB | 423 | // Check if P in edge region of AB, if so return projection of P onto AB |
424 | float vc = d1 * d4 - d3 * d2; | 424 | float vc = d1 * d4 - d3 * d2; |
425 | if (vc <= 0f && d1 >= 0f && d3 <= 0f) | 425 | if (vc <= 0f && d1 >= 0f && d3 <= 0f) |
426 | { | 426 | { |
427 | v = d1 / (d1 - d3); | 427 | v = d1 / (d1 - d3); |
428 | result.ClosestPointOnSimplex = a + v * ab; | 428 | result.ClosestPointOnSimplex = a + v * ab; |
429 | result.UsedVertices.UsedVertexA = true; | 429 | result.UsedVertices.UsedVertexA = true; |
430 | result.UsedVertices.UsedVertexB = true; | 430 | result.UsedVertices.UsedVertexB = true; |
431 | result.SetBarycentricCoordinates(1 - v, v, 0, 0); | 431 | result.SetBarycentricCoordinates(1 - v, v, 0, 0); |
432 | return true; | 432 | return true; |
433 | //return a + v * ab; // barycentric coordinates (1-v,v,0) | 433 | //return a + v * ab; // barycentric coordinates (1-v,v,0) |
434 | } | 434 | } |
435 | 435 | ||
436 | // Check if P in vertex region outside C | 436 | // Check if P in vertex region outside C |
437 | Vector3 cp = p - c; | 437 | Vector3 cp = p - c; |
438 | float d5 = Vector3.Dot(ab, cp); | 438 | float d5 = Vector3.Dot(ab, cp); |
439 | float d6 = Vector3.Dot(ac, cp); | 439 | float d6 = Vector3.Dot(ac, cp); |
440 | if (d6 >= 0f && d5 <= d6) | 440 | if (d6 >= 0f && d5 <= d6) |
441 | { | 441 | { |
442 | result.ClosestPointOnSimplex = c; | 442 | result.ClosestPointOnSimplex = c; |
443 | result.UsedVertices.UsedVertexC = true; | 443 | result.UsedVertices.UsedVertexC = true; |
444 | result.SetBarycentricCoordinates(0, 0, 1, 0); | 444 | result.SetBarycentricCoordinates(0, 0, 1, 0); |
445 | return true;//c; // barycentric coordinates (0,0,1) | 445 | return true;//c; // barycentric coordinates (0,0,1) |
446 | } | 446 | } |
447 | 447 | ||
448 | // Check if P in edge region of AC, if so return projection of P onto AC | 448 | // Check if P in edge region of AC, if so return projection of P onto AC |
449 | float vb = d5 * d2 - d1 * d6; | 449 | float vb = d5 * d2 - d1 * d6; |
450 | if (vb <= 0f && d2 >= 0f && d6 <= 0f) | 450 | if (vb <= 0f && d2 >= 0f && d6 <= 0f) |
451 | { | 451 | { |
452 | w = d2 / (d2 - d6); | 452 | w = d2 / (d2 - d6); |
453 | result.ClosestPointOnSimplex = a + w * ac; | 453 | result.ClosestPointOnSimplex = a + w * ac; |
454 | result.UsedVertices.UsedVertexA = true; | 454 | result.UsedVertices.UsedVertexA = true; |
455 | result.UsedVertices.UsedVertexC = true; | 455 | result.UsedVertices.UsedVertexC = true; |
456 | result.SetBarycentricCoordinates(1 - w, 0, w, 0); | 456 | result.SetBarycentricCoordinates(1 - w, 0, w, 0); |
457 | return true; | 457 | return true; |
458 | //return a + w * ac; // barycentric coordinates (1-w,0,w) | 458 | //return a + w * ac; // barycentric coordinates (1-w,0,w) |
459 | } | 459 | } |
460 | 460 | ||
461 | // Check if P in edge region of BC, if so return projection of P onto BC | 461 | // Check if P in edge region of BC, if so return projection of P onto BC |
462 | float va = d3 * d6 - d5 * d4; | 462 | float va = d3 * d6 - d5 * d4; |
463 | if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f) | 463 | if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f) |
464 | { | 464 | { |
465 | w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); | 465 | w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); |
466 | 466 | ||
467 | result.ClosestPointOnSimplex = b + w * (c - b); | 467 | result.ClosestPointOnSimplex = b + w * (c - b); |
468 | result.UsedVertices.UsedVertexB = true; | 468 | result.UsedVertices.UsedVertexB = true; |
469 | result.UsedVertices.UsedVertexC = true; | 469 | result.UsedVertices.UsedVertexC = true; |
470 | result.SetBarycentricCoordinates(0, 1 - w, w, 0); | 470 | result.SetBarycentricCoordinates(0, 1 - w, w, 0); |
471 | return true; | 471 | return true; |
472 | // return b + w * (c - b); // barycentric coordinates (0,1-w,w) | 472 | // return b + w * (c - b); // barycentric coordinates (0,1-w,w) |
473 | } | 473 | } |
474 | 474 | ||
475 | // P inside face region. Compute Q through its barycentric coordinates (u,v,w) | 475 | // P inside face region. Compute Q through its barycentric coordinates (u,v,w) |
476 | float denom = 1.0f / (va + vb + vc); | 476 | float denom = 1.0f / (va + vb + vc); |
477 | v = vb * denom; | 477 | v = vb * denom; |
478 | w = vc * denom; | 478 | w = vc * denom; |
479 | 479 | ||
480 | result.ClosestPointOnSimplex = a + ab * v + ac * w; | 480 | result.ClosestPointOnSimplex = a + ab * v + ac * w; |
481 | result.UsedVertices.UsedVertexA = true; | 481 | result.UsedVertices.UsedVertexA = true; |
482 | result.UsedVertices.UsedVertexB = true; | 482 | result.UsedVertices.UsedVertexB = true; |
483 | result.UsedVertices.UsedVertexC = true; | 483 | result.UsedVertices.UsedVertexC = true; |
484 | result.SetBarycentricCoordinates(1 - v - w, v, w, 0); | 484 | result.SetBarycentricCoordinates(1 - v - w, v, w, 0); |
485 | 485 | ||
486 | return true; | 486 | return true; |
487 | } | 487 | } |
488 | 488 | ||
489 | /// Test if point p and d lie on opposite sides of plane through abc | 489 | /// Test if point p and d lie on opposite sides of plane through abc |
490 | public int PointOutsideOfPlane(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d) | 490 | public int PointOutsideOfPlane(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d) |
491 | { | 491 | { |
492 | Vector3 normal = Vector3.Cross(b - a, c - a); | 492 | Vector3 normal = Vector3.Cross(b - a, c - a); |
493 | 493 | ||
494 | float signp = Vector3.Dot(p - a, normal); // [AP AB AC] | 494 | float signp = Vector3.Dot(p - a, normal); // [AP AB AC] |
495 | float signd = Vector3.Dot(d - a, normal); // [AD AB AC] | 495 | float signd = Vector3.Dot(d - a, normal); // [AD AB AC] |
496 | 496 | ||
497 | if (CatchDegenerateTetrahedron) | 497 | if (CatchDegenerateTetrahedron) |
498 | if (signd * signd < (1e-4f * 1e-4f)) return -1; | 498 | if (signd * signd < (1e-4f * 1e-4f)) return -1; |
499 | 499 | ||
500 | // Points on opposite sides if expression signs are opposite | 500 | // Points on opposite sides if expression signs are opposite |
501 | return signp * signd < 0f ? 1 : 0; | 501 | return signp * signd < 0f ? 1 : 0; |
502 | } | 502 | } |
503 | 503 | ||
504 | public bool ClosestPtPointTetrahedron(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d, | 504 | public bool ClosestPtPointTetrahedron(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d, |
505 | ref SubSimplexClosestResult finalResult) | 505 | ref SubSimplexClosestResult finalResult) |
506 | { | 506 | { |
507 | SubSimplexClosestResult tempResult = new SubSimplexClosestResult(); | 507 | SubSimplexClosestResult tempResult = new SubSimplexClosestResult(); |
508 | 508 | ||
509 | // Start out assuming point inside all halfspaces, so closest to itself | 509 | // Start out assuming point inside all halfspaces, so closest to itself |
510 | finalResult.ClosestPointOnSimplex = p; | 510 | finalResult.ClosestPointOnSimplex = p; |
511 | finalResult.UsedVertices.Reset(); | 511 | finalResult.UsedVertices.Reset(); |
512 | finalResult.UsedVertices.UsedVertexA = true; | 512 | finalResult.UsedVertices.UsedVertexA = true; |
513 | finalResult.UsedVertices.UsedVertexB = true; | 513 | finalResult.UsedVertices.UsedVertexB = true; |
514 | finalResult.UsedVertices.UsedVertexC = true; | 514 | finalResult.UsedVertices.UsedVertexC = true; |
515 | finalResult.UsedVertices.UsedVertexD = true; | 515 | finalResult.UsedVertices.UsedVertexD = true; |
516 | 516 | ||
517 | int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d); | 517 | int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d); |
518 | int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b); | 518 | int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b); |
519 | int pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c); | 519 | int pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c); |
520 | int pointOutsideBDC = PointOutsideOfPlane(p, b, d, c, a); | 520 | int pointOutsideBDC = PointOutsideOfPlane(p, b, d, c, a); |
521 | 521 | ||
522 | if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) | 522 | if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) |
523 | { | 523 | { |
524 | finalResult.Degenerate = true; | 524 | finalResult.Degenerate = true; |
525 | return false; | 525 | return false; |
526 | } | 526 | } |
527 | 527 | ||
528 | if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0) | 528 | if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0) |
529 | return false; | 529 | return false; |
530 | 530 | ||
531 | float bestSqDist = float.MaxValue; | 531 | float bestSqDist = float.MaxValue; |
532 | // If point outside face abc then compute closest point on abc | 532 | // If point outside face abc then compute closest point on abc |
533 | if (pointOutsideABC != 0) | 533 | if (pointOutsideABC != 0) |
534 | { | 534 | { |
535 | ClosestPtPointTriangle(p, a, b, c, ref tempResult); | 535 | ClosestPtPointTriangle(p, a, b, c, ref tempResult); |
536 | Vector3 q = tempResult.ClosestPointOnSimplex; | 536 | Vector3 q = tempResult.ClosestPointOnSimplex; |
537 | 537 | ||
538 | float sqDist = ((Vector3)(q - p)).LengthSquared(); | 538 | float sqDist = ((Vector3)(q - p)).LengthSquared(); |
539 | // Update best closest point if (squared) distance is less than current best | 539 | // Update best closest point if (squared) distance is less than current best |
540 | if (sqDist < bestSqDist) | 540 | if (sqDist < bestSqDist) |
541 | { | 541 | { |
542 | bestSqDist = sqDist; | 542 | bestSqDist = sqDist; |
543 | finalResult.ClosestPointOnSimplex = q; | 543 | finalResult.ClosestPointOnSimplex = q; |
544 | //convert result bitmask! | 544 | //convert result bitmask! |
545 | finalResult.UsedVertices.Reset(); | 545 | finalResult.UsedVertices.Reset(); |
546 | finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; | 546 | finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; |
547 | finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexB; | 547 | finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexB; |
548 | finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC; | 548 | finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC; |
549 | finalResult.SetBarycentricCoordinates( | 549 | finalResult.SetBarycentricCoordinates( |
550 | tempResult.BarycentricCoords[VertexA], | 550 | tempResult.BarycentricCoords[VertexA], |
551 | tempResult.BarycentricCoords[VertexB], | 551 | tempResult.BarycentricCoords[VertexB], |
552 | tempResult.BarycentricCoords[VertexC], | 552 | tempResult.BarycentricCoords[VertexC], |
553 | 0); | 553 | 0); |
554 | } | 554 | } |
555 | } | 555 | } |
556 | 556 | ||
557 | // Repeat test for face acd | 557 | // Repeat test for face acd |
558 | if (pointOutsideACD != 0) | 558 | if (pointOutsideACD != 0) |
559 | { | 559 | { |
560 | ClosestPtPointTriangle(p, a, c, d, ref tempResult); | 560 | ClosestPtPointTriangle(p, a, c, d, ref tempResult); |
561 | Vector3 q = tempResult.ClosestPointOnSimplex; | 561 | Vector3 q = tempResult.ClosestPointOnSimplex; |
562 | //convert result bitmask! | 562 | //convert result bitmask! |
563 | 563 | ||
564 | float sqDist = ((Vector3)(q - p)).LengthSquared(); | 564 | float sqDist = ((Vector3)(q - p)).LengthSquared(); |
565 | if (sqDist < bestSqDist) | 565 | if (sqDist < bestSqDist) |
566 | { | 566 | { |
567 | bestSqDist = sqDist; | 567 | bestSqDist = sqDist; |
568 | finalResult.ClosestPointOnSimplex = q; | 568 | finalResult.ClosestPointOnSimplex = q; |
569 | finalResult.UsedVertices.Reset(); | 569 | finalResult.UsedVertices.Reset(); |
570 | finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; | 570 | finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; |
571 | finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexB; | 571 | finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexB; |
572 | finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexC; | 572 | finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexC; |
573 | finalResult.SetBarycentricCoordinates( | 573 | finalResult.SetBarycentricCoordinates( |
574 | tempResult.BarycentricCoords[VertexA], | 574 | tempResult.BarycentricCoords[VertexA], |
575 | 0, | 575 | 0, |
576 | tempResult.BarycentricCoords[VertexB], | 576 | tempResult.BarycentricCoords[VertexB], |
577 | tempResult.BarycentricCoords[VertexC]); | 577 | tempResult.BarycentricCoords[VertexC]); |
578 | } | 578 | } |
579 | } | 579 | } |
580 | // Repeat test for face adb | 580 | // Repeat test for face adb |
581 | 581 | ||
582 | if (pointOutsideADB != 0) | 582 | if (pointOutsideADB != 0) |
583 | { | 583 | { |
584 | ClosestPtPointTriangle(p, a, d, b, ref tempResult); | 584 | ClosestPtPointTriangle(p, a, d, b, ref tempResult); |
585 | Vector3 q = tempResult.ClosestPointOnSimplex; | 585 | Vector3 q = tempResult.ClosestPointOnSimplex; |
586 | //convert result bitmask! | 586 | //convert result bitmask! |
587 | 587 | ||
588 | float sqDist = ((Vector3)(q - p)).LengthSquared(); | 588 | float sqDist = ((Vector3)(q - p)).LengthSquared(); |
589 | if (sqDist < bestSqDist) | 589 | if (sqDist < bestSqDist) |
590 | { | 590 | { |
591 | bestSqDist = sqDist; | 591 | bestSqDist = sqDist; |
592 | finalResult.ClosestPointOnSimplex = q; | 592 | finalResult.ClosestPointOnSimplex = q; |
593 | finalResult.UsedVertices.Reset(); | 593 | finalResult.UsedVertices.Reset(); |
594 | finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; | 594 | finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; |
595 | finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB; | 595 | finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB; |
596 | finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexC; | 596 | finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexC; |
597 | finalResult.SetBarycentricCoordinates( | 597 | finalResult.SetBarycentricCoordinates( |
598 | tempResult.BarycentricCoords[VertexA], | 598 | tempResult.BarycentricCoords[VertexA], |
599 | tempResult.BarycentricCoords[VertexC], | 599 | tempResult.BarycentricCoords[VertexC], |
600 | 0, | 600 | 0, |
601 | tempResult.BarycentricCoords[VertexB]); | 601 | tempResult.BarycentricCoords[VertexB]); |
602 | 602 | ||
603 | } | 603 | } |
604 | } | 604 | } |
605 | // Repeat test for face bdc | 605 | // Repeat test for face bdc |
606 | 606 | ||
607 | if (pointOutsideBDC != 0) | 607 | if (pointOutsideBDC != 0) |
608 | { | 608 | { |
609 | ClosestPtPointTriangle(p, b, d, c, ref tempResult); | 609 | ClosestPtPointTriangle(p, b, d, c, ref tempResult); |
610 | Vector3 q = tempResult.ClosestPointOnSimplex; | 610 | Vector3 q = tempResult.ClosestPointOnSimplex; |
611 | //convert result bitmask! | 611 | //convert result bitmask! |
612 | float sqDist = ((Vector3)(q - p)).LengthSquared(); | 612 | float sqDist = ((Vector3)(q - p)).LengthSquared(); |
613 | if (sqDist < bestSqDist) | 613 | if (sqDist < bestSqDist) |
614 | { | 614 | { |
615 | bestSqDist = sqDist; | 615 | bestSqDist = sqDist; |
616 | finalResult.ClosestPointOnSimplex = q; | 616 | finalResult.ClosestPointOnSimplex = q; |
617 | finalResult.UsedVertices.Reset(); | 617 | finalResult.UsedVertices.Reset(); |
618 | finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexA; | 618 | finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexA; |
619 | finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB; | 619 | finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB; |
620 | finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC; | 620 | finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC; |
621 | 621 | ||
622 | finalResult.SetBarycentricCoordinates( | 622 | finalResult.SetBarycentricCoordinates( |
623 | 0, | 623 | 0, |
624 | tempResult.BarycentricCoords[VertexA], | 624 | tempResult.BarycentricCoords[VertexA], |
625 | tempResult.BarycentricCoords[VertexC], | 625 | tempResult.BarycentricCoords[VertexC], |
626 | tempResult.BarycentricCoords[VertexB]); | 626 | tempResult.BarycentricCoords[VertexB]); |
627 | } | 627 | } |
628 | } | 628 | } |
629 | 629 | ||
630 | //help! we ended up full ! | 630 | //help! we ended up full ! |
631 | 631 | ||
632 | if (finalResult.UsedVertices.UsedVertexA && | 632 | if (finalResult.UsedVertices.UsedVertexA && |
633 | finalResult.UsedVertices.UsedVertexB && | 633 | finalResult.UsedVertices.UsedVertexB && |
634 | finalResult.UsedVertices.UsedVertexC && | 634 | finalResult.UsedVertices.UsedVertexC && |
635 | finalResult.UsedVertices.UsedVertexD) | 635 | finalResult.UsedVertices.UsedVertexD) |
636 | { | 636 | { |
637 | return true; | 637 | return true; |
638 | } | 638 | } |
639 | 639 | ||
640 | return true; | 640 | return true; |
641 | } | 641 | } |
642 | } | 642 | } |
643 | } | 643 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs index e4a9ae2..e731643 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs | |||
@@ -1,488 +1,488 @@ | |||
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.Dynamics | 27 | namespace XnaDevRu.BulletX.Dynamics |
28 | { | 28 | { |
29 | public delegate float ContactSolverFunc (RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo info); | 29 | public delegate float ContactSolverFunc (RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo info); |
30 | 30 | ||
31 | public enum ContactSolverType | 31 | public enum ContactSolverType |
32 | { | 32 | { |
33 | Default = 0, | 33 | Default = 0, |
34 | TypeA, | 34 | TypeA, |
35 | TypeB, | 35 | TypeB, |
36 | User, | 36 | User, |
37 | MaxContactSolverType, | 37 | MaxContactSolverType, |
38 | } | 38 | } |
39 | 39 | ||
40 | public class ConstraintPersistentData | 40 | public class ConstraintPersistentData |
41 | { | 41 | { |
42 | // total applied impulse during most recent frame | 42 | // total applied impulse during most recent frame |
43 | private float _appliedImpulse; | 43 | private float _appliedImpulse; |
44 | private float _previousAppliedImpulse; | 44 | private float _previousAppliedImpulse; |
45 | private float _accumulatedTangentImpulse0; | 45 | private float _accumulatedTangentImpulse0; |
46 | private float _accumulatedTangentImpulse1; | 46 | private float _accumulatedTangentImpulse1; |
47 | 47 | ||
48 | private float _jacDiagABInv; | 48 | private float _jacDiagABInv; |
49 | private float _jacDiagABInvTangentA; | 49 | private float _jacDiagABInvTangentA; |
50 | private float _jacDiagABInvTangentB; | 50 | private float _jacDiagABInvTangentB; |
51 | private int _persistentLifeTime; | 51 | private int _persistentLifeTime; |
52 | private float _restitution; | 52 | private float _restitution; |
53 | private float _friction; | 53 | private float _friction; |
54 | private float _penetration; | 54 | private float _penetration; |
55 | private Vector3 _frictionWorldTangentialA; | 55 | private Vector3 _frictionWorldTangentialA; |
56 | private Vector3 _frictionWorldTangentialB; | 56 | private Vector3 _frictionWorldTangentialB; |
57 | 57 | ||
58 | private Vector3 _frictionAngularComponent0A; | 58 | private Vector3 _frictionAngularComponent0A; |
59 | private Vector3 _frictionAngularComponent0B; | 59 | private Vector3 _frictionAngularComponent0B; |
60 | private Vector3 _frictionAngularComponent1A; | 60 | private Vector3 _frictionAngularComponent1A; |
61 | private Vector3 _frictionAngularComponent1B; | 61 | private Vector3 _frictionAngularComponent1B; |
62 | 62 | ||
63 | //some data doesn't need to be persistent over frames: todo: clean/reuse this | 63 | //some data doesn't need to be persistent over frames: todo: clean/reuse this |
64 | private Vector3 _angularComponentA; | 64 | private Vector3 _angularComponentA; |
65 | private Vector3 _angularComponentB; | 65 | private Vector3 _angularComponentB; |
66 | 66 | ||
67 | private ContactSolverFunc _contactSolverFunc; | 67 | private ContactSolverFunc _contactSolverFunc; |
68 | private ContactSolverFunc _frictionSolverFunc; | 68 | private ContactSolverFunc _frictionSolverFunc; |
69 | 69 | ||
70 | public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } } | 70 | public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } } |
71 | public float PreviousAppliedImpulse { get { return _previousAppliedImpulse; } set { _previousAppliedImpulse = value; } } | 71 | public float PreviousAppliedImpulse { get { return _previousAppliedImpulse; } set { _previousAppliedImpulse = value; } } |
72 | public float AccumulatedTangentImpulseA { get { return _accumulatedTangentImpulse0; } set { _accumulatedTangentImpulse0 = value; } } | 72 | public float AccumulatedTangentImpulseA { get { return _accumulatedTangentImpulse0; } set { _accumulatedTangentImpulse0 = value; } } |
73 | public float AccumulatedTangentImpulseB { get { return _accumulatedTangentImpulse1; } set { _accumulatedTangentImpulse1 = value; } } | 73 | public float AccumulatedTangentImpulseB { get { return _accumulatedTangentImpulse1; } set { _accumulatedTangentImpulse1 = value; } } |
74 | 74 | ||
75 | public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } } | 75 | public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } } |
76 | public float JacDiagABInvTangentA { get { return _jacDiagABInvTangentA; } set { _jacDiagABInvTangentA = value; } } | 76 | public float JacDiagABInvTangentA { get { return _jacDiagABInvTangentA; } set { _jacDiagABInvTangentA = value; } } |
77 | public float JacDiagABInvTangentB { get { return _jacDiagABInvTangentB; } set { _jacDiagABInvTangentB = value; } } | 77 | public float JacDiagABInvTangentB { get { return _jacDiagABInvTangentB; } set { _jacDiagABInvTangentB = value; } } |
78 | public int PersistentLifeTime { get { return _persistentLifeTime; } set { _persistentLifeTime = value; } } | 78 | public int PersistentLifeTime { get { return _persistentLifeTime; } set { _persistentLifeTime = value; } } |
79 | public float Restitution { get { return _restitution; } set { _restitution = value; } } | 79 | public float Restitution { get { return _restitution; } set { _restitution = value; } } |
80 | public float Friction { get { return _friction; } set { _friction = value; } } | 80 | public float Friction { get { return _friction; } set { _friction = value; } } |
81 | public float Penetration { get { return _penetration; } set { _penetration = value; } } | 81 | public float Penetration { get { return _penetration; } set { _penetration = value; } } |
82 | public Vector3 FrictionWorldTangentialA { get { return _frictionWorldTangentialA; } set { _frictionWorldTangentialA = value; } } | 82 | public Vector3 FrictionWorldTangentialA { get { return _frictionWorldTangentialA; } set { _frictionWorldTangentialA = value; } } |
83 | public Vector3 FrictionWorldTangentialB { get { return _frictionWorldTangentialB; } set { _frictionWorldTangentialB = value; } } | 83 | public Vector3 FrictionWorldTangentialB { get { return _frictionWorldTangentialB; } set { _frictionWorldTangentialB = value; } } |
84 | 84 | ||
85 | public Vector3 FrictionAngularComponent0A { get { return _frictionAngularComponent0A; } set { _frictionAngularComponent0A = value; } } | 85 | public Vector3 FrictionAngularComponent0A { get { return _frictionAngularComponent0A; } set { _frictionAngularComponent0A = value; } } |
86 | public Vector3 FrictionAngularComponent0B { get { return _frictionAngularComponent0B; } set { _frictionAngularComponent0B = value; } } | 86 | public Vector3 FrictionAngularComponent0B { get { return _frictionAngularComponent0B; } set { _frictionAngularComponent0B = value; } } |
87 | public Vector3 FrictionAngularComponent1A { get { return _frictionAngularComponent1A; } set { _frictionAngularComponent1A = value; } } | 87 | public Vector3 FrictionAngularComponent1A { get { return _frictionAngularComponent1A; } set { _frictionAngularComponent1A = value; } } |
88 | public Vector3 FrictionAngularComponent1B { get { return _frictionAngularComponent1B; } set { _frictionAngularComponent1B = value; } } | 88 | public Vector3 FrictionAngularComponent1B { get { return _frictionAngularComponent1B; } set { _frictionAngularComponent1B = value; } } |
89 | 89 | ||
90 | public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } } | 90 | public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } } |
91 | public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } } | 91 | public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } } |
92 | 92 | ||
93 | public ContactSolverFunc ContactSolverFunc { get { return _contactSolverFunc; } set { _contactSolverFunc = value; } } | 93 | public ContactSolverFunc ContactSolverFunc { get { return _contactSolverFunc; } set { _contactSolverFunc = value; } } |
94 | public ContactSolverFunc FrictionSolverFunc { get { return _frictionSolverFunc; } set { _frictionSolverFunc = value; } } | 94 | public ContactSolverFunc FrictionSolverFunc { get { return _frictionSolverFunc; } set { _frictionSolverFunc = value; } } |
95 | } | 95 | } |
96 | 96 | ||
97 | public static class ContactConstraint | 97 | public static class ContactConstraint |
98 | { | 98 | { |
99 | private const int UseInternalApplyImpulse = 1; | 99 | private const int UseInternalApplyImpulse = 1; |
100 | 100 | ||
101 | /// <summary> | 101 | /// <summary> |
102 | /// bilateral constraint between two dynamic objects | 102 | /// bilateral constraint between two dynamic objects |
103 | /// positive distance = separation, negative distance = penetration | 103 | /// positive distance = separation, negative distance = penetration |
104 | /// </summary> | 104 | /// </summary> |
105 | /// <param name="body1"></param> | 105 | /// <param name="body1"></param> |
106 | /// <param name="pos1"></param> | 106 | /// <param name="pos1"></param> |
107 | /// <param name="body2"></param> | 107 | /// <param name="body2"></param> |
108 | /// <param name="pos2"></param> | 108 | /// <param name="pos2"></param> |
109 | /// <param name="distance"></param> | 109 | /// <param name="distance"></param> |
110 | /// <param name="normal"></param> | 110 | /// <param name="normal"></param> |
111 | /// <param name="impulse"></param> | 111 | /// <param name="impulse"></param> |
112 | /// <param name="timeStep"></param> | 112 | /// <param name="timeStep"></param> |
113 | public static void ResolveSingleBilateral(RigidBody bodyA, Vector3 posA, | 113 | public static void ResolveSingleBilateral(RigidBody bodyA, Vector3 posA, |
114 | RigidBody bodyB, Vector3 posB, | 114 | RigidBody bodyB, Vector3 posB, |
115 | float distance, Vector3 normal, out float impulse, float timeStep) | 115 | float distance, Vector3 normal, out float impulse, float timeStep) |
116 | { | 116 | { |
117 | float normalLenSqr = normal.LengthSquared(); | 117 | float normalLenSqr = normal.LengthSquared(); |
118 | 118 | ||
119 | if (Math.Abs(normalLenSqr) >= 1.1f) | 119 | if (Math.Abs(normalLenSqr) >= 1.1f) |
120 | throw new BulletException(); | 120 | throw new BulletException(); |
121 | 121 | ||
122 | /*if (normalLenSqr > 1.1f) | 122 | /*if (normalLenSqr > 1.1f) |
123 | { | 123 | { |
124 | impulse = 0f; | 124 | impulse = 0f; |
125 | return; | 125 | return; |
126 | }*/ | 126 | }*/ |
127 | Vector3 rel_pos1 = posA - bodyA.CenterOfMassPosition; | 127 | Vector3 rel_pos1 = posA - bodyA.CenterOfMassPosition; |
128 | Vector3 rel_pos2 = posB - bodyB.CenterOfMassPosition; | 128 | Vector3 rel_pos2 = posB - bodyB.CenterOfMassPosition; |
129 | //this jacobian entry could be re-used for all iterations | 129 | //this jacobian entry could be re-used for all iterations |
130 | 130 | ||
131 | Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); | 131 | Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); |
132 | Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); | 132 | Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); |
133 | Vector3 vel = vel1 - vel2; | 133 | Vector3 vel = vel1 - vel2; |
134 | 134 | ||
135 | 135 | ||
136 | JacobianEntry jac = new JacobianEntry(Matrix.Transpose(bodyA.CenterOfMassTransform), | 136 | JacobianEntry jac = new JacobianEntry(Matrix.Transpose(bodyA.CenterOfMassTransform), |
137 | Matrix.Transpose(bodyB.CenterOfMassTransform), | 137 | Matrix.Transpose(bodyB.CenterOfMassTransform), |
138 | rel_pos1, rel_pos2, normal, bodyA.InvInertiaDiagLocal, bodyA.InverseMass, | 138 | rel_pos1, rel_pos2, normal, bodyA.InvInertiaDiagLocal, bodyA.InverseMass, |
139 | bodyB.InvInertiaDiagLocal, bodyB.InverseMass); | 139 | bodyB.InvInertiaDiagLocal, bodyB.InverseMass); |
140 | 140 | ||
141 | float jacDiagAB = jac.Diagonal; | 141 | float jacDiagAB = jac.Diagonal; |
142 | float jacDiagABInv = 1f / jacDiagAB; | 142 | float jacDiagABInv = 1f / jacDiagAB; |
143 | 143 | ||
144 | float rel_vel = jac.GetRelativeVelocity( | 144 | float rel_vel = jac.GetRelativeVelocity( |
145 | bodyA.LinearVelocity, | 145 | bodyA.LinearVelocity, |
146 | Vector3.TransformNormal(bodyA.AngularVelocity, Matrix.Transpose(bodyA.CenterOfMassTransform)), | 146 | Vector3.TransformNormal(bodyA.AngularVelocity, Matrix.Transpose(bodyA.CenterOfMassTransform)), |
147 | bodyB.LinearVelocity, | 147 | bodyB.LinearVelocity, |
148 | Vector3.TransformNormal(bodyB.AngularVelocity, Matrix.Transpose(bodyB.CenterOfMassTransform))); | 148 | Vector3.TransformNormal(bodyB.AngularVelocity, Matrix.Transpose(bodyB.CenterOfMassTransform))); |
149 | float a; | 149 | float a; |
150 | a = jacDiagABInv; | 150 | a = jacDiagABInv; |
151 | 151 | ||
152 | 152 | ||
153 | rel_vel = Vector3.Dot(normal, vel); | 153 | rel_vel = Vector3.Dot(normal, vel); |
154 | 154 | ||
155 | float contactDamping = 0.2f; | 155 | float contactDamping = 0.2f; |
156 | 156 | ||
157 | float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; | 157 | float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; |
158 | impulse = velocityImpulse; | 158 | impulse = velocityImpulse; |
159 | } | 159 | } |
160 | 160 | ||
161 | 161 | ||
162 | /// <summary> | 162 | /// <summary> |
163 | /// contact constraint resolution: | 163 | /// contact constraint resolution: |
164 | /// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint | 164 | /// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint |
165 | /// positive distance = separation, negative distance = penetration | 165 | /// positive distance = separation, negative distance = penetration |
166 | /// </summary> | 166 | /// </summary> |
167 | /// <param name="body1"></param> | 167 | /// <param name="body1"></param> |
168 | /// <param name="body2"></param> | 168 | /// <param name="body2"></param> |
169 | /// <param name="contactPoint"></param> | 169 | /// <param name="contactPoint"></param> |
170 | /// <param name="info"></param> | 170 | /// <param name="info"></param> |
171 | /// <returns></returns> | 171 | /// <returns></returns> |
172 | public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB, | 172 | public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB, |
173 | ManifoldPoint contactPoint, ContactSolverInfo solverInfo) | 173 | ManifoldPoint contactPoint, ContactSolverInfo solverInfo) |
174 | { | 174 | { |
175 | Vector3 pos1 = contactPoint.PositionWorldOnA; | 175 | Vector3 pos1 = contactPoint.PositionWorldOnA; |
176 | Vector3 pos2 = contactPoint.PositionWorldOnB; | 176 | Vector3 pos2 = contactPoint.PositionWorldOnB; |
177 | 177 | ||
178 | 178 | ||
179 | // printf("distance=%f\n",distance); | 179 | // printf("distance=%f\n",distance); |
180 | 180 | ||
181 | Vector3 normal = contactPoint.NormalWorldOnB; | 181 | Vector3 normal = contactPoint.NormalWorldOnB; |
182 | 182 | ||
183 | Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition; | 183 | Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition; |
184 | Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition; | 184 | Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition; |
185 | 185 | ||
186 | Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); | 186 | Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); |
187 | Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); | 187 | Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); |
188 | Vector3 vel = vel1 - vel2; | 188 | Vector3 vel = vel1 - vel2; |
189 | float rel_vel; | 189 | float rel_vel; |
190 | rel_vel = Vector3.Dot(normal, vel); | 190 | rel_vel = Vector3.Dot(normal, vel); |
191 | 191 | ||
192 | 192 | ||
193 | float Kfps = 1f / solverInfo.TimeStep; | 193 | float Kfps = 1f / solverInfo.TimeStep; |
194 | 194 | ||
195 | //float damping = solverInfo.m_damping; | 195 | //float damping = solverInfo.m_damping; |
196 | float Kerp = solverInfo.Erp; | 196 | float Kerp = solverInfo.Erp; |
197 | 197 | ||
198 | float Kcor = Kerp * Kfps; | 198 | float Kcor = Kerp * Kfps; |
199 | 199 | ||
200 | //printf("dist=%f\n",distance); | 200 | //printf("dist=%f\n",distance); |
201 | 201 | ||
202 | ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; | 202 | ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; |
203 | if (cpd == null) | 203 | if (cpd == null) |
204 | throw new BulletException(); | 204 | throw new BulletException(); |
205 | 205 | ||
206 | float distance = cpd.Penetration;//contactPoint.getDistance(); | 206 | float distance = cpd.Penetration;//contactPoint.getDistance(); |
207 | 207 | ||
208 | 208 | ||
209 | //distance = 0.f; | 209 | //distance = 0.f; |
210 | float positionalError = Kcor * -distance; | 210 | float positionalError = Kcor * -distance; |
211 | //jacDiagABInv; | 211 | //jacDiagABInv; |
212 | float velocityError = cpd.Restitution - rel_vel;// * damping; | 212 | float velocityError = cpd.Restitution - rel_vel;// * damping; |
213 | 213 | ||
214 | 214 | ||
215 | float penetrationImpulse = positionalError * cpd.JacDiagABInv; | 215 | float penetrationImpulse = positionalError * cpd.JacDiagABInv; |
216 | float velocityImpulse = velocityError * cpd.JacDiagABInv; | 216 | float velocityImpulse = velocityError * cpd.JacDiagABInv; |
217 | float normalImpulse = penetrationImpulse + velocityImpulse; | 217 | float normalImpulse = penetrationImpulse + velocityImpulse; |
218 | 218 | ||
219 | // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse | 219 | // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse |
220 | float oldNormalImpulse = cpd.AppliedImpulse; | 220 | float oldNormalImpulse = cpd.AppliedImpulse; |
221 | float sum = oldNormalImpulse + normalImpulse; | 221 | float sum = oldNormalImpulse + normalImpulse; |
222 | cpd.AppliedImpulse = 0f > sum ? 0f : sum; | 222 | cpd.AppliedImpulse = 0f > sum ? 0f : sum; |
223 | 223 | ||
224 | normalImpulse = cpd.AppliedImpulse - oldNormalImpulse; | 224 | normalImpulse = cpd.AppliedImpulse - oldNormalImpulse; |
225 | 225 | ||
226 | if (bodyA.InverseMass != 0) | 226 | if (bodyA.InverseMass != 0) |
227 | { | 227 | { |
228 | bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse); | 228 | bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse); |
229 | } | 229 | } |
230 | if (bodyB.InverseMass != 0) | 230 | if (bodyB.InverseMass != 0) |
231 | { | 231 | { |
232 | bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse); | 232 | bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse); |
233 | } | 233 | } |
234 | 234 | ||
235 | /*body1.applyImpulse(normal * (normalImpulse), rel_pos1); | 235 | /*body1.applyImpulse(normal * (normalImpulse), rel_pos1); |
236 | body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/ | 236 | body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/ |
237 | 237 | ||
238 | return normalImpulse; | 238 | return normalImpulse; |
239 | } | 239 | } |
240 | 240 | ||
241 | public static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB, | 241 | public static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB, |
242 | ManifoldPoint contactPoint, ContactSolverInfo solverInfo) | 242 | ManifoldPoint contactPoint, ContactSolverInfo solverInfo) |
243 | { | 243 | { |
244 | 244 | ||
245 | Vector3 pos1 = contactPoint.PositionWorldOnA; | 245 | Vector3 pos1 = contactPoint.PositionWorldOnA; |
246 | Vector3 pos2 = contactPoint.PositionWorldOnB; | 246 | Vector3 pos2 = contactPoint.PositionWorldOnB; |
247 | 247 | ||
248 | Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition; | 248 | Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition; |
249 | Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition; | 249 | Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition; |
250 | 250 | ||
251 | ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; | 251 | ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; |
252 | if (cpd == null) | 252 | if (cpd == null) |
253 | throw new BulletException(); | 253 | throw new BulletException(); |
254 | 254 | ||
255 | float combinedFriction = cpd.Friction; | 255 | float combinedFriction = cpd.Friction; |
256 | 256 | ||
257 | float limit = cpd.AppliedImpulse * combinedFriction; | 257 | float limit = cpd.AppliedImpulse * combinedFriction; |
258 | 258 | ||
259 | //friction | 259 | //friction |
260 | if (cpd.AppliedImpulse > 0) | 260 | if (cpd.AppliedImpulse > 0) |
261 | { | 261 | { |
262 | //apply friction in the 2 tangential directions | 262 | //apply friction in the 2 tangential directions |
263 | 263 | ||
264 | // 1st tangent | 264 | // 1st tangent |
265 | Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); | 265 | Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); |
266 | Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); | 266 | Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); |
267 | Vector3 vel = vel1 - vel2; | 267 | Vector3 vel = vel1 - vel2; |
268 | 268 | ||
269 | float j1, j2; | 269 | float j1, j2; |
270 | 270 | ||
271 | { | 271 | { |
272 | 272 | ||
273 | float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel); | 273 | float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel); |
274 | 274 | ||
275 | // calculate j that moves us to zero relative velocity | 275 | // calculate j that moves us to zero relative velocity |
276 | j1 = -vrel * cpd.JacDiagABInvTangentA; | 276 | j1 = -vrel * cpd.JacDiagABInvTangentA; |
277 | float oldTangentImpulse = cpd.AccumulatedTangentImpulseA; | 277 | float oldTangentImpulse = cpd.AccumulatedTangentImpulseA; |
278 | cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1; | 278 | cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1; |
279 | float atia = cpd.AccumulatedTangentImpulseA; | 279 | float atia = cpd.AccumulatedTangentImpulseA; |
280 | MathHelper.SetMin(ref atia, limit); | 280 | MathHelper.SetMin(ref atia, limit); |
281 | MathHelper.SetMax(ref atia, -limit); | 281 | MathHelper.SetMax(ref atia, -limit); |
282 | cpd.AccumulatedTangentImpulseA = atia; | 282 | cpd.AccumulatedTangentImpulseA = atia; |
283 | j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse; | 283 | j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse; |
284 | 284 | ||
285 | } | 285 | } |
286 | { | 286 | { |
287 | // 2nd tangent | 287 | // 2nd tangent |
288 | 288 | ||
289 | float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel); | 289 | float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel); |
290 | 290 | ||
291 | // calculate j that moves us to zero relative velocity | 291 | // calculate j that moves us to zero relative velocity |
292 | j2 = -vrel * cpd.JacDiagABInvTangentB; | 292 | j2 = -vrel * cpd.JacDiagABInvTangentB; |
293 | float oldTangentImpulse = cpd.AccumulatedTangentImpulseB; | 293 | float oldTangentImpulse = cpd.AccumulatedTangentImpulseB; |
294 | cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2; | 294 | cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2; |
295 | float atib = cpd.AccumulatedTangentImpulseB; | 295 | float atib = cpd.AccumulatedTangentImpulseB; |
296 | MathHelper.SetMin(ref atib, limit); | 296 | MathHelper.SetMin(ref atib, limit); |
297 | MathHelper.SetMax(ref atib, -limit); | 297 | MathHelper.SetMax(ref atib, -limit); |
298 | cpd.AccumulatedTangentImpulseB = atib; | 298 | cpd.AccumulatedTangentImpulseB = atib; |
299 | j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse; | 299 | j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse; |
300 | } | 300 | } |
301 | 301 | ||
302 | if (bodyA.InverseMass != 0) | 302 | if (bodyA.InverseMass != 0) |
303 | { | 303 | { |
304 | bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1); | 304 | bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1); |
305 | bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2); | 305 | bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2); |
306 | } | 306 | } |
307 | if (bodyB.InverseMass != 0) | 307 | if (bodyB.InverseMass != 0) |
308 | { | 308 | { |
309 | bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1); | 309 | bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1); |
310 | bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2); | 310 | bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2); |
311 | } | 311 | } |
312 | 312 | ||
313 | } | 313 | } |
314 | return cpd.AppliedImpulse; | 314 | return cpd.AppliedImpulse; |
315 | } | 315 | } |
316 | 316 | ||
317 | public static float ResolveSingleFrictionOriginal( | 317 | public static float ResolveSingleFrictionOriginal( |
318 | RigidBody bodyA, | 318 | RigidBody bodyA, |
319 | RigidBody bodyB, | 319 | RigidBody bodyB, |
320 | ManifoldPoint contactPoint, | 320 | ManifoldPoint contactPoint, |
321 | ContactSolverInfo solverInfo) | 321 | ContactSolverInfo solverInfo) |
322 | { | 322 | { |
323 | Vector3 posA = contactPoint.PositionWorldOnA; | 323 | Vector3 posA = contactPoint.PositionWorldOnA; |
324 | Vector3 posB = contactPoint.PositionWorldOnB; | 324 | Vector3 posB = contactPoint.PositionWorldOnB; |
325 | 325 | ||
326 | Vector3 relPosA = posA - bodyA.CenterOfMassPosition; | 326 | Vector3 relPosA = posA - bodyA.CenterOfMassPosition; |
327 | Vector3 relPosB = posB - bodyB.CenterOfMassPosition; | 327 | Vector3 relPosB = posB - bodyB.CenterOfMassPosition; |
328 | 328 | ||
329 | ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; | 329 | ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; |
330 | if (cpd == null) | 330 | if (cpd == null) |
331 | throw new BulletException(); | 331 | throw new BulletException(); |
332 | 332 | ||
333 | float combinedFriction = cpd.Friction; | 333 | float combinedFriction = cpd.Friction; |
334 | 334 | ||
335 | float limit = cpd.AppliedImpulse * combinedFriction; | 335 | float limit = cpd.AppliedImpulse * combinedFriction; |
336 | //if (contactPoint.m_appliedImpulse>0.f) | 336 | //if (contactPoint.m_appliedImpulse>0.f) |
337 | //friction | 337 | //friction |
338 | { | 338 | { |
339 | //apply friction in the 2 tangential directions | 339 | //apply friction in the 2 tangential directions |
340 | 340 | ||
341 | { | 341 | { |
342 | // 1st tangent | 342 | // 1st tangent |
343 | Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); | 343 | Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); |
344 | Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); | 344 | Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); |
345 | Vector3 vel = velA - velB; | 345 | Vector3 vel = velA - velB; |
346 | 346 | ||
347 | float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel); | 347 | float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel); |
348 | 348 | ||
349 | // calculate j that moves us to zero relative velocity | 349 | // calculate j that moves us to zero relative velocity |
350 | float j = -vrel * cpd.JacDiagABInvTangentA; | 350 | float j = -vrel * cpd.JacDiagABInvTangentA; |
351 | float total = cpd.AccumulatedTangentImpulseA + j; | 351 | float total = cpd.AccumulatedTangentImpulseA + j; |
352 | if (limit < total) | 352 | if (limit < total) |
353 | total = limit; | 353 | total = limit; |
354 | if (total < -limit) | 354 | if (total < -limit) |
355 | total = -limit; | 355 | total = -limit; |
356 | j = total - cpd.AccumulatedTangentImpulseA; | 356 | j = total - cpd.AccumulatedTangentImpulseA; |
357 | cpd.AccumulatedTangentImpulseA = total; | 357 | cpd.AccumulatedTangentImpulseA = total; |
358 | bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA); | 358 | bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA); |
359 | bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB); | 359 | bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB); |
360 | } | 360 | } |
361 | 361 | ||
362 | 362 | ||
363 | { | 363 | { |
364 | // 2nd tangent | 364 | // 2nd tangent |
365 | Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); | 365 | Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); |
366 | Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); | 366 | Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); |
367 | Vector3 vel = velA - velB; | 367 | Vector3 vel = velA - velB; |
368 | 368 | ||
369 | float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel); | 369 | float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel); |
370 | 370 | ||
371 | // calculate j that moves us to zero relative velocity | 371 | // calculate j that moves us to zero relative velocity |
372 | float j = -vrel * cpd.JacDiagABInvTangentB; | 372 | float j = -vrel * cpd.JacDiagABInvTangentB; |
373 | float total = cpd.AccumulatedTangentImpulseB + j; | 373 | float total = cpd.AccumulatedTangentImpulseB + j; |
374 | if (limit < total) | 374 | if (limit < total) |
375 | total = limit; | 375 | total = limit; |
376 | if (total < -limit) | 376 | if (total < -limit) |
377 | total = -limit; | 377 | total = -limit; |
378 | j = total - cpd.AccumulatedTangentImpulseB; | 378 | j = total - cpd.AccumulatedTangentImpulseB; |
379 | cpd.AccumulatedTangentImpulseB = total; | 379 | cpd.AccumulatedTangentImpulseB = total; |
380 | bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA); | 380 | bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA); |
381 | bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB); | 381 | bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB); |
382 | } | 382 | } |
383 | } | 383 | } |
384 | 384 | ||
385 | return cpd.AppliedImpulse; | 385 | return cpd.AppliedImpulse; |
386 | } | 386 | } |
387 | 387 | ||
388 | //velocity + friction | 388 | //velocity + friction |
389 | //response between two dynamic objects with friction | 389 | //response between two dynamic objects with friction |
390 | public static float ResolveSingleCollisionCombined( | 390 | public static float ResolveSingleCollisionCombined( |
391 | RigidBody bodyA, | 391 | RigidBody bodyA, |
392 | RigidBody bodyB, | 392 | RigidBody bodyB, |
393 | ManifoldPoint contactPoint, | 393 | ManifoldPoint contactPoint, |
394 | ContactSolverInfo solverInfo) | 394 | ContactSolverInfo solverInfo) |
395 | { | 395 | { |
396 | 396 | ||
397 | Vector3 posA = contactPoint.PositionWorldOnA; | 397 | Vector3 posA = contactPoint.PositionWorldOnA; |
398 | Vector3 posB = contactPoint.PositionWorldOnB; | 398 | Vector3 posB = contactPoint.PositionWorldOnB; |
399 | Vector3 normal = contactPoint.NormalWorldOnB; | 399 | Vector3 normal = contactPoint.NormalWorldOnB; |
400 | 400 | ||
401 | Vector3 relPosA = posA - bodyA.CenterOfMassPosition; | 401 | Vector3 relPosA = posA - bodyA.CenterOfMassPosition; |
402 | Vector3 relPosB = posB - bodyB.CenterOfMassPosition; | 402 | Vector3 relPosB = posB - bodyB.CenterOfMassPosition; |
403 | 403 | ||
404 | Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); | 404 | Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); |
405 | Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); | 405 | Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); |
406 | Vector3 vel = velA - velB; | 406 | Vector3 vel = velA - velB; |
407 | float relVel; | 407 | float relVel; |
408 | relVel = Vector3.Dot(normal, vel); | 408 | relVel = Vector3.Dot(normal, vel); |
409 | 409 | ||
410 | float Kfps = 1f / solverInfo.TimeStep; | 410 | float Kfps = 1f / solverInfo.TimeStep; |
411 | 411 | ||
412 | //float damping = solverInfo.m_damping; | 412 | //float damping = solverInfo.m_damping; |
413 | float Kerp = solverInfo.Erp; | 413 | float Kerp = solverInfo.Erp; |
414 | float Kcor = Kerp * Kfps; | 414 | float Kcor = Kerp * Kfps; |
415 | 415 | ||
416 | ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; | 416 | ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; |
417 | if (cpd == null) | 417 | if (cpd == null) |
418 | throw new BulletException(); | 418 | throw new BulletException(); |
419 | 419 | ||
420 | float distance = cpd.Penetration; | 420 | float distance = cpd.Penetration; |
421 | float positionalError = Kcor * -distance; | 421 | float positionalError = Kcor * -distance; |
422 | float velocityError = cpd.Restitution - relVel;// * damping; | 422 | float velocityError = cpd.Restitution - relVel;// * damping; |
423 | 423 | ||
424 | float penetrationImpulse = positionalError * cpd.JacDiagABInv; | 424 | float penetrationImpulse = positionalError * cpd.JacDiagABInv; |
425 | 425 | ||
426 | float velocityImpulse = velocityError * cpd.JacDiagABInv; | 426 | float velocityImpulse = velocityError * cpd.JacDiagABInv; |
427 | 427 | ||
428 | float normalImpulse = penetrationImpulse + velocityImpulse; | 428 | float normalImpulse = penetrationImpulse + velocityImpulse; |
429 | 429 | ||
430 | // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse | 430 | // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse |
431 | float oldNormalImpulse = cpd.AppliedImpulse; | 431 | float oldNormalImpulse = cpd.AppliedImpulse; |
432 | float sum = oldNormalImpulse + normalImpulse; | 432 | float sum = oldNormalImpulse + normalImpulse; |
433 | cpd.AppliedImpulse = 0 > sum ? 0 : sum; | 433 | cpd.AppliedImpulse = 0 > sum ? 0 : sum; |
434 | 434 | ||
435 | normalImpulse = cpd.AppliedImpulse - oldNormalImpulse; | 435 | normalImpulse = cpd.AppliedImpulse - oldNormalImpulse; |
436 | 436 | ||
437 | if (bodyA.InverseMass != 0) | 437 | if (bodyA.InverseMass != 0) |
438 | { | 438 | { |
439 | bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse); | 439 | bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse); |
440 | } | 440 | } |
441 | if (bodyB.InverseMass != 0) | 441 | if (bodyB.InverseMass != 0) |
442 | { | 442 | { |
443 | bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse); | 443 | bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse); |
444 | } | 444 | } |
445 | 445 | ||
446 | { | 446 | { |
447 | //friction | 447 | //friction |
448 | Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA); | 448 | Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA); |
449 | Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB); | 449 | Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB); |
450 | Vector3 vel3 = vel12 - vel22; | 450 | Vector3 vel3 = vel12 - vel22; |
451 | 451 | ||
452 | relVel = Vector3.Dot(normal, vel3); | 452 | relVel = Vector3.Dot(normal, vel3); |
453 | 453 | ||
454 | 454 | ||
455 | Vector3 latVel = vel3 - normal * relVel; | 455 | Vector3 latVel = vel3 - normal * relVel; |
456 | float lat_rel_vel = latVel.Length(); | 456 | float lat_rel_vel = latVel.Length(); |
457 | 457 | ||
458 | float combinedFriction = cpd.Friction; | 458 | float combinedFriction = cpd.Friction; |
459 | 459 | ||
460 | if (cpd.AppliedImpulse > 0) | 460 | if (cpd.AppliedImpulse > 0) |
461 | if (lat_rel_vel > float.Epsilon) | 461 | if (lat_rel_vel > float.Epsilon) |
462 | { | 462 | { |
463 | latVel /= lat_rel_vel; | 463 | latVel /= lat_rel_vel; |
464 | Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld); | 464 | Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld); |
465 | Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld); | 465 | Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld); |
466 | float friction_impulse = lat_rel_vel / | 466 | float friction_impulse = lat_rel_vel / |
467 | (bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB))); | 467 | (bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB))); |
468 | float normal_impulse = cpd.AppliedImpulse * combinedFriction; | 468 | float normal_impulse = cpd.AppliedImpulse * combinedFriction; |
469 | 469 | ||
470 | MathHelper.SetMin(ref friction_impulse, normal_impulse); | 470 | MathHelper.SetMin(ref friction_impulse, normal_impulse); |
471 | MathHelper.SetMin(ref friction_impulse, -normal_impulse); | 471 | MathHelper.SetMin(ref friction_impulse, -normal_impulse); |
472 | bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA); | 472 | bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA); |
473 | bodyB.ApplyImpulse(latVel * friction_impulse, relPosB); | 473 | bodyB.ApplyImpulse(latVel * friction_impulse, relPosB); |
474 | } | 474 | } |
475 | } | 475 | } |
476 | return normalImpulse; | 476 | return normalImpulse; |
477 | } | 477 | } |
478 | 478 | ||
479 | public static float ResolveSingleFrictionEmpty( | 479 | public static float ResolveSingleFrictionEmpty( |
480 | RigidBody bodyA, | 480 | RigidBody bodyA, |
481 | RigidBody bodyB, | 481 | RigidBody bodyB, |
482 | ManifoldPoint contactPoint, | 482 | ManifoldPoint contactPoint, |
483 | ContactSolverInfo solverInfo) | 483 | ContactSolverInfo solverInfo) |
484 | { | 484 | { |
485 | return 0; | 485 | return 0; |
486 | } | 486 | } |
487 | } | 487 | } |
488 | } | 488 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs index c1763df..d3424f6 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs | |||
@@ -1,62 +1,62 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX.Dynamics | 26 | namespace XnaDevRu.BulletX.Dynamics |
27 | { | 27 | { |
28 | public class ContactSolverInfo | 28 | public class ContactSolverInfo |
29 | { | 29 | { |
30 | private float _tau; | 30 | private float _tau; |
31 | private float _damping; | 31 | private float _damping; |
32 | private float _friction; | 32 | private float _friction; |
33 | private float _timeStep; | 33 | private float _timeStep; |
34 | private float _restitution; | 34 | private float _restitution; |
35 | private int _numIterations; | 35 | private int _numIterations; |
36 | private float _maxErrorReduction; | 36 | private float _maxErrorReduction; |
37 | private float _sor; | 37 | private float _sor; |
38 | private float _erp; | 38 | private float _erp; |
39 | 39 | ||
40 | public ContactSolverInfo() | 40 | public ContactSolverInfo() |
41 | { | 41 | { |
42 | _tau = 0.6f; | 42 | _tau = 0.6f; |
43 | _damping = 1.0f; | 43 | _damping = 1.0f; |
44 | _friction = 0.3f; | 44 | _friction = 0.3f; |
45 | _restitution = 0f; | 45 | _restitution = 0f; |
46 | _maxErrorReduction = 20f; | 46 | _maxErrorReduction = 20f; |
47 | _numIterations = 10; | 47 | _numIterations = 10; |
48 | _erp = 0.4f; | 48 | _erp = 0.4f; |
49 | _sor = 1.3f; | 49 | _sor = 1.3f; |
50 | } | 50 | } |
51 | 51 | ||
52 | public float Tau { get { return _tau; } set { _tau = value; } } | 52 | public float Tau { get { return _tau; } set { _tau = value; } } |
53 | public float Damping { get { return _damping; } set { _damping = value; } } | 53 | public float Damping { get { return _damping; } set { _damping = value; } } |
54 | public float Friction { get { return _friction; } set { _friction = value; } } | 54 | public float Friction { get { return _friction; } set { _friction = value; } } |
55 | public float TimeStep { get { return _timeStep; } set { _timeStep = value; } } | 55 | public float TimeStep { get { return _timeStep; } set { _timeStep = value; } } |
56 | public float Restitution { get { return _restitution; } set { _restitution = value; } } | 56 | public float Restitution { get { return _restitution; } set { _restitution = value; } } |
57 | public int IterationsCount { get { return _numIterations; } set { _numIterations = value; } } | 57 | public int IterationsCount { get { return _numIterations; } set { _numIterations = value; } } |
58 | public float MaxErrorReduction { get { return _maxErrorReduction; } set { _maxErrorReduction = value; } } | 58 | public float MaxErrorReduction { get { return _maxErrorReduction; } set { _maxErrorReduction = value; } } |
59 | public float Sor { get { return _sor; } set { _sor = value; } } | 59 | public float Sor { get { return _sor; } set { _sor = value; } } |
60 | public float Erp { get { return _erp; } set { _erp = value; } } | 60 | public float Erp { get { return _erp; } set { _erp = value; } } |
61 | } | 61 | } |
62 | } | 62 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs index 479c863..f1e0b1e 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs | |||
@@ -1,440 +1,440 @@ | |||
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.Dynamics | 27 | namespace XnaDevRu.BulletX.Dynamics |
28 | { | 28 | { |
29 | /// <summary> | 29 | /// <summary> |
30 | /// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space | 30 | /// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space |
31 | /// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked' | 31 | /// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked' |
32 | /// Work in progress (is still a Hinge actually) | 32 | /// Work in progress (is still a Hinge actually) |
33 | /// </summary> | 33 | /// </summary> |
34 | public class Generic6DofConstraint : TypedConstraint | 34 | public class Generic6DofConstraint : TypedConstraint |
35 | { | 35 | { |
36 | private static readonly float[] _sign = { 1.0f, -1.0f, 1.0f }; | 36 | private static readonly float[] _sign = { 1.0f, -1.0f, 1.0f }; |
37 | private static readonly int[] _axisA = { 1, 0, 0 }; | 37 | private static readonly int[] _axisA = { 1, 0, 0 }; |
38 | private static readonly int[] _axisB = { 2, 2, 1 }; | 38 | private static readonly int[] _axisB = { 2, 2, 1 }; |
39 | 39 | ||
40 | private JacobianEntry[] _jacLinear = new JacobianEntry[3]; // 3 orthogonal linear constraints | 40 | private JacobianEntry[] _jacLinear = new JacobianEntry[3]; // 3 orthogonal linear constraints |
41 | private JacobianEntry[] _jacAng = new JacobianEntry[3]; // 3 orthogonal angular constraints | 41 | private JacobianEntry[] _jacAng = new JacobianEntry[3]; // 3 orthogonal angular constraints |
42 | 42 | ||
43 | private Matrix _frameInA; // the constraint space w.r.t body A | 43 | private Matrix _frameInA; // the constraint space w.r.t body A |
44 | private Matrix _frameInB; // the constraint space w.r.t body B | 44 | private Matrix _frameInB; // the constraint space w.r.t body B |
45 | 45 | ||
46 | private float[] _lowerLimit = new float[6]; // the constraint lower limits | 46 | private float[] _lowerLimit = new float[6]; // the constraint lower limits |
47 | private float[] _upperLimit = new float[6]; // the constraint upper limits | 47 | private float[] _upperLimit = new float[6]; // the constraint upper limits |
48 | 48 | ||
49 | private float[] _accumulatedImpulse = new float[6]; | 49 | private float[] _accumulatedImpulse = new float[6]; |
50 | 50 | ||
51 | public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB) | 51 | public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB) |
52 | : base(rbA, rbB) | 52 | : base(rbA, rbB) |
53 | { | 53 | { |
54 | _frameInA = frameInA; | 54 | _frameInA = frameInA; |
55 | _frameInB = frameInB; | 55 | _frameInB = frameInB; |
56 | //free means upper < lower, | 56 | //free means upper < lower, |
57 | //locked means upper == lower | 57 | //locked means upper == lower |
58 | //limited means upper > lower | 58 | //limited means upper > lower |
59 | //so start all locked | 59 | //so start all locked |
60 | for (int i = 0; i < 6; ++i) | 60 | for (int i = 0; i < 6; ++i) |
61 | { | 61 | { |
62 | _lowerLimit[i] = 0.0f; | 62 | _lowerLimit[i] = 0.0f; |
63 | _upperLimit[i] = 0.0f; | 63 | _upperLimit[i] = 0.0f; |
64 | _accumulatedImpulse[i] = 0.0f; | 64 | _accumulatedImpulse[i] = 0.0f; |
65 | } | 65 | } |
66 | } | 66 | } |
67 | 67 | ||
68 | public Generic6DofConstraint() { } | 68 | public Generic6DofConstraint() { } |
69 | 69 | ||
70 | public void UpdateRHS(float timeStep) { } | 70 | public void UpdateRHS(float timeStep) { } |
71 | 71 | ||
72 | public float ComputeAngle(int axis) | 72 | public float ComputeAngle(int axis) |
73 | { | 73 | { |
74 | float angle = 0; | 74 | float angle = 0; |
75 | 75 | ||
76 | switch (axis) | 76 | switch (axis) |
77 | { | 77 | { |
78 | case 0: | 78 | case 0: |
79 | { | 79 | { |
80 | Vector3 v1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 1), RigidBodyA.CenterOfMassTransform); | 80 | Vector3 v1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 1), RigidBodyA.CenterOfMassTransform); |
81 | Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); | 81 | Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); |
82 | Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); | 82 | Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); |
83 | 83 | ||
84 | float s = Vector3.Dot(v1, w2); | 84 | float s = Vector3.Dot(v1, w2); |
85 | float c = Vector3.Dot(v1, v2); | 85 | float c = Vector3.Dot(v1, v2); |
86 | 86 | ||
87 | angle = (float)Math.Atan2(s, c); | 87 | angle = (float)Math.Atan2(s, c); |
88 | break; | 88 | break; |
89 | } | 89 | } |
90 | case 1: | 90 | case 1: |
91 | { | 91 | { |
92 | Vector3 w1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 2), RigidBodyA.CenterOfMassTransform); | 92 | Vector3 w1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 2), RigidBodyA.CenterOfMassTransform); |
93 | Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); | 93 | Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); |
94 | Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); | 94 | Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); |
95 | 95 | ||
96 | float s = Vector3.Dot(w1, u2); | 96 | float s = Vector3.Dot(w1, u2); |
97 | float c = Vector3.Dot(w1, w2); | 97 | float c = Vector3.Dot(w1, w2); |
98 | 98 | ||
99 | angle = (float)Math.Atan2(s, c); | 99 | angle = (float)Math.Atan2(s, c); |
100 | break; | 100 | break; |
101 | } | 101 | } |
102 | case 2: | 102 | case 2: |
103 | { | 103 | { |
104 | Vector3 u1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 0), RigidBodyA.CenterOfMassTransform); | 104 | Vector3 u1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 0), RigidBodyA.CenterOfMassTransform); |
105 | Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); | 105 | Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); |
106 | Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); | 106 | Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); |
107 | 107 | ||
108 | float s = Vector3.Dot(u1, v2); | 108 | float s = Vector3.Dot(u1, v2); |
109 | float c = Vector3.Dot(u1, u2); | 109 | float c = Vector3.Dot(u1, u2); |
110 | 110 | ||
111 | angle = (float)Math.Atan2(s, c); | 111 | angle = (float)Math.Atan2(s, c); |
112 | break; | 112 | break; |
113 | } | 113 | } |
114 | default: BulletDebug.Assert(false); break; | 114 | default: BulletDebug.Assert(false); break; |
115 | } | 115 | } |
116 | 116 | ||
117 | return angle; | 117 | return angle; |
118 | } | 118 | } |
119 | 119 | ||
120 | public void SetLinearLowerLimit(Vector3 linearLower) | 120 | public void SetLinearLowerLimit(Vector3 linearLower) |
121 | { | 121 | { |
122 | _lowerLimit[0] = linearLower.X; | 122 | _lowerLimit[0] = linearLower.X; |
123 | _lowerLimit[1] = linearLower.Y; | 123 | _lowerLimit[1] = linearLower.Y; |
124 | _lowerLimit[2] = linearLower.Z; | 124 | _lowerLimit[2] = linearLower.Z; |
125 | } | 125 | } |
126 | 126 | ||
127 | public void SetLinearUpperLimit(Vector3 linearUpper) | 127 | public void SetLinearUpperLimit(Vector3 linearUpper) |
128 | { | 128 | { |
129 | _upperLimit[0] = linearUpper.X; | 129 | _upperLimit[0] = linearUpper.X; |
130 | _upperLimit[1] = linearUpper.Y; | 130 | _upperLimit[1] = linearUpper.Y; |
131 | _upperLimit[2] = linearUpper.Z; | 131 | _upperLimit[2] = linearUpper.Z; |
132 | } | 132 | } |
133 | 133 | ||
134 | public void SetAngularLowerLimit(Vector3 angularLower) | 134 | public void SetAngularLowerLimit(Vector3 angularLower) |
135 | { | 135 | { |
136 | _lowerLimit[3] = angularLower.X; | 136 | _lowerLimit[3] = angularLower.X; |
137 | _lowerLimit[4] = angularLower.Y; | 137 | _lowerLimit[4] = angularLower.Y; |
138 | _lowerLimit[5] = angularLower.Z; | 138 | _lowerLimit[5] = angularLower.Z; |
139 | } | 139 | } |
140 | 140 | ||
141 | public void SetAngularUpperLimit(Vector3 angularUpper) | 141 | public void SetAngularUpperLimit(Vector3 angularUpper) |
142 | { | 142 | { |
143 | _upperLimit[3] = angularUpper.X; | 143 | _upperLimit[3] = angularUpper.X; |
144 | _upperLimit[4] = angularUpper.Y; | 144 | _upperLimit[4] = angularUpper.Y; |
145 | _upperLimit[5] = angularUpper.Z; | 145 | _upperLimit[5] = angularUpper.Z; |
146 | } | 146 | } |
147 | 147 | ||
148 | //first 3 are linear, next 3 are angular | 148 | //first 3 are linear, next 3 are angular |
149 | public void SetLimit(int axis, float lo, float hi) | 149 | public void SetLimit(int axis, float lo, float hi) |
150 | { | 150 | { |
151 | _lowerLimit[axis] = lo; | 151 | _lowerLimit[axis] = lo; |
152 | _upperLimit[axis] = hi; | 152 | _upperLimit[axis] = hi; |
153 | } | 153 | } |
154 | 154 | ||
155 | //free means upper < lower, | 155 | //free means upper < lower, |
156 | //locked means upper == lower | 156 | //locked means upper == lower |
157 | //limited means upper > lower | 157 | //limited means upper > lower |
158 | //limitIndex: first 3 are linear, next 3 are angular | 158 | //limitIndex: first 3 are linear, next 3 are angular |
159 | public bool IsLimited(int limitIndex) | 159 | public bool IsLimited(int limitIndex) |
160 | { | 160 | { |
161 | return (_upperLimit[limitIndex] >= _lowerLimit[limitIndex]); | 161 | return (_upperLimit[limitIndex] >= _lowerLimit[limitIndex]); |
162 | } | 162 | } |
163 | 163 | ||
164 | public override void BuildJacobian() | 164 | public override void BuildJacobian() |
165 | { | 165 | { |
166 | Vector3 localNormalInA = new Vector3(0, 0, 0); | 166 | Vector3 localNormalInA = new Vector3(0, 0, 0); |
167 | 167 | ||
168 | Vector3 pivotInA = _frameInA.Translation; | 168 | Vector3 pivotInA = _frameInA.Translation; |
169 | Vector3 pivotInB = _frameInB.Translation; | 169 | Vector3 pivotInB = _frameInB.Translation; |
170 | 170 | ||
171 | Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); | 171 | Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); |
172 | Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); | 172 | Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); |
173 | 173 | ||
174 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; | 174 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; |
175 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; | 175 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; |
176 | 176 | ||
177 | //linear part | 177 | //linear part |
178 | for (int i = 0; i < 3; i++) | 178 | for (int i = 0; i < 3; i++) |
179 | { | 179 | { |
180 | if (IsLimited(i)) | 180 | if (IsLimited(i)) |
181 | { | 181 | { |
182 | if (i == 0) | 182 | if (i == 0) |
183 | localNormalInA = new Vector3(1, 0, 0); | 183 | localNormalInA = new Vector3(1, 0, 0); |
184 | else if (i == 1) | 184 | else if (i == 1) |
185 | localNormalInA = new Vector3(0, 1, 0); | 185 | localNormalInA = new Vector3(0, 1, 0); |
186 | else | 186 | else |
187 | localNormalInA = new Vector3(0, 0, 1); | 187 | localNormalInA = new Vector3(0, 0, 1); |
188 | 188 | ||
189 | Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); | 189 | Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); |
190 | 190 | ||
191 | // Create linear atom | 191 | // Create linear atom |
192 | _jacLinear[i] = new JacobianEntry( | 192 | _jacLinear[i] = new JacobianEntry( |
193 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | 193 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), |
194 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | 194 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), |
195 | MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, | 195 | MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, |
196 | MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, | 196 | MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, |
197 | normalWorld, | 197 | normalWorld, |
198 | RigidBodyA.InvInertiaDiagLocal, | 198 | RigidBodyA.InvInertiaDiagLocal, |
199 | RigidBodyA.InverseMass, | 199 | RigidBodyA.InverseMass, |
200 | RigidBodyB.InvInertiaDiagLocal, | 200 | RigidBodyB.InvInertiaDiagLocal, |
201 | RigidBodyB.InverseMass); | 201 | RigidBodyB.InverseMass); |
202 | 202 | ||
203 | //optionally disable warmstarting | 203 | //optionally disable warmstarting |
204 | _accumulatedImpulse[i] = 0f; | 204 | _accumulatedImpulse[i] = 0f; |
205 | 205 | ||
206 | // Apply accumulated impulse | 206 | // Apply accumulated impulse |
207 | Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld; | 207 | Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld; |
208 | 208 | ||
209 | RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); | 209 | RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); |
210 | RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); | 210 | RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); |
211 | } | 211 | } |
212 | } | 212 | } |
213 | 213 | ||
214 | // angular part | 214 | // angular part |
215 | for (int i = 0; i < 3; i++) | 215 | for (int i = 0; i < 3; i++) |
216 | { | 216 | { |
217 | if (IsLimited(i + 3)) | 217 | if (IsLimited(i + 3)) |
218 | { | 218 | { |
219 | Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); | 219 | Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); |
220 | Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform); | 220 | Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform); |
221 | 221 | ||
222 | Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB); | 222 | Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB); |
223 | 223 | ||
224 | // Create angular atom | 224 | // Create angular atom |
225 | _jacAng[i] = new JacobianEntry(axis, | 225 | _jacAng[i] = new JacobianEntry(axis, |
226 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | 226 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), |
227 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | 227 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), |
228 | RigidBodyA.InvInertiaDiagLocal, | 228 | RigidBodyA.InvInertiaDiagLocal, |
229 | RigidBodyB.InvInertiaDiagLocal); | 229 | RigidBodyB.InvInertiaDiagLocal); |
230 | 230 | ||
231 | _accumulatedImpulse[i + 3] = 0f; | 231 | _accumulatedImpulse[i + 3] = 0f; |
232 | 232 | ||
233 | // Apply accumulated impulse | 233 | // Apply accumulated impulse |
234 | Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis; | 234 | Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis; |
235 | 235 | ||
236 | RigidBodyA.ApplyTorqueImpulse(impulse_vector); | 236 | RigidBodyA.ApplyTorqueImpulse(impulse_vector); |
237 | RigidBodyB.ApplyTorqueImpulse(-impulse_vector); | 237 | RigidBodyB.ApplyTorqueImpulse(-impulse_vector); |
238 | } | 238 | } |
239 | } | 239 | } |
240 | } | 240 | } |
241 | 241 | ||
242 | public override void SolveConstraint(float timeStep) | 242 | public override void SolveConstraint(float timeStep) |
243 | { | 243 | { |
244 | float tau = 0.1f; | 244 | float tau = 0.1f; |
245 | float damping = 1.0f; | 245 | float damping = 1.0f; |
246 | 246 | ||
247 | Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); | 247 | Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); |
248 | Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); | 248 | Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); |
249 | 249 | ||
250 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; | 250 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; |
251 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; | 251 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; |
252 | 252 | ||
253 | Vector3 localNormalInA = new Vector3(); | 253 | Vector3 localNormalInA = new Vector3(); |
254 | 254 | ||
255 | // linear | 255 | // linear |
256 | for (int i = 0; i < 3; i++) | 256 | for (int i = 0; i < 3; i++) |
257 | { | 257 | { |
258 | if (IsLimited(i)) | 258 | if (IsLimited(i)) |
259 | { | 259 | { |
260 | Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); | 260 | Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); |
261 | Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); | 261 | Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); |
262 | 262 | ||
263 | if (i == 0) | 263 | if (i == 0) |
264 | localNormalInA = new Vector3(1, 0, 0); | 264 | localNormalInA = new Vector3(1, 0, 0); |
265 | else if (i == 1) | 265 | else if (i == 1) |
266 | localNormalInA = new Vector3(0, 1, 0); | 266 | localNormalInA = new Vector3(0, 1, 0); |
267 | else | 267 | else |
268 | localNormalInA = new Vector3(0, 0, 1); | 268 | localNormalInA = new Vector3(0, 0, 1); |
269 | 269 | ||
270 | Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); | 270 | Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); |
271 | 271 | ||
272 | float jacDiagABInv = 1f / _jacLinear[i].Diagonal; | 272 | float jacDiagABInv = 1f / _jacLinear[i].Diagonal; |
273 | 273 | ||
274 | //velocity error (first order error) | 274 | //velocity error (first order error) |
275 | float rel_vel = _jacLinear[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, | 275 | float rel_vel = _jacLinear[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, |
276 | RigidBodyB.LinearVelocity, angvelB); | 276 | RigidBodyB.LinearVelocity, angvelB); |
277 | 277 | ||
278 | //positional error (zeroth order error) | 278 | //positional error (zeroth order error) |
279 | float depth = -Vector3.Dot(pivotAInW - pivotBInW, normalWorld); | 279 | float depth = -Vector3.Dot(pivotAInW - pivotBInW, normalWorld); |
280 | float lo = -1e30f; | 280 | float lo = -1e30f; |
281 | float hi = 1e30f; | 281 | float hi = 1e30f; |
282 | 282 | ||
283 | //handle the limits | 283 | //handle the limits |
284 | if (_lowerLimit[i] < _upperLimit[i]) | 284 | if (_lowerLimit[i] < _upperLimit[i]) |
285 | { | 285 | { |
286 | if (depth > _upperLimit[i]) | 286 | if (depth > _upperLimit[i]) |
287 | { | 287 | { |
288 | depth -= _upperLimit[i]; | 288 | depth -= _upperLimit[i]; |
289 | lo = 0f; | 289 | lo = 0f; |
290 | } | 290 | } |
291 | else | 291 | else |
292 | { | 292 | { |
293 | if (depth < _lowerLimit[i]) | 293 | if (depth < _lowerLimit[i]) |
294 | { | 294 | { |
295 | depth -= _lowerLimit[i]; | 295 | depth -= _lowerLimit[i]; |
296 | hi = 0f; | 296 | hi = 0f; |
297 | } | 297 | } |
298 | else | 298 | else |
299 | { | 299 | { |
300 | continue; | 300 | continue; |
301 | } | 301 | } |
302 | } | 302 | } |
303 | } | 303 | } |
304 | 304 | ||
305 | float normalImpulse = (tau * depth / timeStep - damping * rel_vel) * jacDiagABInv; | 305 | float normalImpulse = (tau * depth / timeStep - damping * rel_vel) * jacDiagABInv; |
306 | float oldNormalImpulse = _accumulatedImpulse[i]; | 306 | float oldNormalImpulse = _accumulatedImpulse[i]; |
307 | float sum = oldNormalImpulse + normalImpulse; | 307 | float sum = oldNormalImpulse + normalImpulse; |
308 | _accumulatedImpulse[i] = sum > hi ? 0f : sum < lo ? 0f : sum; | 308 | _accumulatedImpulse[i] = sum > hi ? 0f : sum < lo ? 0f : sum; |
309 | normalImpulse = _accumulatedImpulse[i] - oldNormalImpulse; | 309 | normalImpulse = _accumulatedImpulse[i] - oldNormalImpulse; |
310 | 310 | ||
311 | Vector3 impulse_vector = normalWorld * normalImpulse; | 311 | Vector3 impulse_vector = normalWorld * normalImpulse; |
312 | RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); | 312 | RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); |
313 | RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); | 313 | RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); |
314 | } | 314 | } |
315 | } | 315 | } |
316 | 316 | ||
317 | Vector3 axis; | 317 | Vector3 axis; |
318 | float angle; | 318 | float angle; |
319 | Matrix frameAWorld = RigidBodyA.CenterOfMassTransform * _frameInA; | 319 | Matrix frameAWorld = RigidBodyA.CenterOfMassTransform * _frameInA; |
320 | Matrix frameBWorld = RigidBodyB.CenterOfMassTransform * _frameInB; | 320 | Matrix frameBWorld = RigidBodyB.CenterOfMassTransform * _frameInB; |
321 | 321 | ||
322 | TransformUtil.CalculateDiffAxisAngle(frameAWorld, frameBWorld, out axis, out angle); | 322 | TransformUtil.CalculateDiffAxisAngle(frameAWorld, frameBWorld, out axis, out angle); |
323 | Quaternion diff = new Quaternion(axis, angle); | 323 | Quaternion diff = new Quaternion(axis, angle); |
324 | Matrix diffMat = Matrix.CreateFromQuaternion(diff); | 324 | Matrix diffMat = Matrix.CreateFromQuaternion(diff); |
325 | Vector3 xyz; | 325 | Vector3 xyz; |
326 | // this is not perfect, we can first check which axis are limited, and choose a more appropriate order | 326 | // this is not perfect, we can first check which axis are limited, and choose a more appropriate order |
327 | MatrixToEulerXYZ(diffMat, out xyz); | 327 | MatrixToEulerXYZ(diffMat, out xyz); |
328 | 328 | ||
329 | // angular | 329 | // angular |
330 | for (int i = 0; i < 3; i++) | 330 | for (int i = 0; i < 3; i++) |
331 | { | 331 | { |
332 | if (IsLimited(i + 3)) | 332 | if (IsLimited(i + 3)) |
333 | { | 333 | { |
334 | Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); | 334 | Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); |
335 | Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); | 335 | Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); |
336 | 336 | ||
337 | float jacDiagABInv = 1f / _jacAng[i].Diagonal; | 337 | float jacDiagABInv = 1f / _jacAng[i].Diagonal; |
338 | 338 | ||
339 | //velocity error (first order error) | 339 | //velocity error (first order error) |
340 | float rel_vel = _jacAng[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, | 340 | float rel_vel = _jacAng[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, |
341 | RigidBodyB.LinearVelocity, angvelB); | 341 | RigidBodyB.LinearVelocity, angvelB); |
342 | 342 | ||
343 | //positional error (zeroth order error) | 343 | //positional error (zeroth order error) |
344 | Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); | 344 | Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); |
345 | Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform); | 345 | Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform); |
346 | 346 | ||
347 | float rel_pos = _sign[i] * Vector3.Dot(axisA, axisB); | 347 | float rel_pos = _sign[i] * Vector3.Dot(axisA, axisB); |
348 | 348 | ||
349 | float lo = -1e30f; | 349 | float lo = -1e30f; |
350 | float hi = 1e30f; | 350 | float hi = 1e30f; |
351 | 351 | ||
352 | //handle the twist limit | 352 | //handle the twist limit |
353 | if (_lowerLimit[i + 3] < _upperLimit[i + 3]) | 353 | if (_lowerLimit[i + 3] < _upperLimit[i + 3]) |
354 | { | 354 | { |
355 | //clamp the values | 355 | //clamp the values |
356 | float loLimit = _upperLimit[i + 3] > -3.1415 ? _lowerLimit[i + 3] : -1e30f; | 356 | float loLimit = _upperLimit[i + 3] > -3.1415 ? _lowerLimit[i + 3] : -1e30f; |
357 | float hiLimit = _upperLimit[i + 3] < 3.1415 ? _upperLimit[i + 3] : 1e30f; | 357 | float hiLimit = _upperLimit[i + 3] < 3.1415 ? _upperLimit[i + 3] : 1e30f; |
358 | 358 | ||
359 | float projAngle; | 359 | float projAngle; |
360 | 360 | ||
361 | if (i == 0) | 361 | if (i == 0) |
362 | projAngle = -2f * xyz.Z; | 362 | projAngle = -2f * xyz.Z; |
363 | else if (i == 1) | 363 | else if (i == 1) |
364 | projAngle = -2f * xyz.Y; | 364 | projAngle = -2f * xyz.Y; |
365 | else | 365 | else |
366 | projAngle = -2f * xyz.Z; | 366 | projAngle = -2f * xyz.Z; |
367 | 367 | ||
368 | if (projAngle < loLimit) | 368 | if (projAngle < loLimit) |
369 | { | 369 | { |
370 | hi = 0f; | 370 | hi = 0f; |
371 | rel_pos = loLimit - projAngle; | 371 | rel_pos = loLimit - projAngle; |
372 | } | 372 | } |
373 | else | 373 | else |
374 | { | 374 | { |
375 | if (projAngle > hiLimit) | 375 | if (projAngle > hiLimit) |
376 | { | 376 | { |
377 | lo = 0f; | 377 | lo = 0f; |
378 | rel_pos = (hiLimit - projAngle); | 378 | rel_pos = (hiLimit - projAngle); |
379 | } | 379 | } |
380 | else | 380 | else |
381 | { | 381 | { |
382 | continue; | 382 | continue; |
383 | } | 383 | } |
384 | } | 384 | } |
385 | } | 385 | } |
386 | 386 | ||
387 | //impulse | 387 | //impulse |
388 | 388 | ||
389 | float normalImpulse = -(tau * rel_pos / timeStep + damping * rel_vel) * jacDiagABInv; | 389 | float normalImpulse = -(tau * rel_pos / timeStep + damping * rel_vel) * jacDiagABInv; |
390 | float oldNormalImpulse = _accumulatedImpulse[i + 3]; | 390 | float oldNormalImpulse = _accumulatedImpulse[i + 3]; |
391 | float sum = oldNormalImpulse + normalImpulse; | 391 | float sum = oldNormalImpulse + normalImpulse; |
392 | _accumulatedImpulse[i + 3] = sum > hi ? 0f : sum < lo ? 0f : sum; | 392 | _accumulatedImpulse[i + 3] = sum > hi ? 0f : sum < lo ? 0f : sum; |
393 | normalImpulse = _accumulatedImpulse[i + 3] - oldNormalImpulse; | 393 | normalImpulse = _accumulatedImpulse[i + 3] - oldNormalImpulse; |
394 | 394 | ||
395 | Vector3 axis2 = _sign[i] * Vector3.Cross(axisA, axisB); | 395 | Vector3 axis2 = _sign[i] * Vector3.Cross(axisA, axisB); |
396 | Vector3 impulse_vector = axis2 * normalImpulse; | 396 | Vector3 impulse_vector = axis2 * normalImpulse; |
397 | 397 | ||
398 | RigidBodyA.ApplyTorqueImpulse(impulse_vector); | 398 | RigidBodyA.ApplyTorqueImpulse(impulse_vector); |
399 | RigidBodyB.ApplyTorqueImpulse(-impulse_vector); | 399 | RigidBodyB.ApplyTorqueImpulse(-impulse_vector); |
400 | } | 400 | } |
401 | } | 401 | } |
402 | } | 402 | } |
403 | 403 | ||
404 | //MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html | 404 | //MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html |
405 | private bool MatrixToEulerXYZ(Matrix mat, out Vector3 xyz) | 405 | private bool MatrixToEulerXYZ(Matrix mat, out Vector3 xyz) |
406 | { | 406 | { |
407 | // rot = cy*cz -cy*sz sy | 407 | // rot = cy*cz -cy*sz sy |
408 | // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx | 408 | // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx |
409 | // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy | 409 | // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy |
410 | xyz = new Vector3(); | 410 | xyz = new Vector3(); |
411 | 411 | ||
412 | if (MathHelper.GetElement(mat, 2) < 1.0f) | 412 | if (MathHelper.GetElement(mat, 2) < 1.0f) |
413 | { | 413 | { |
414 | if (MathHelper.GetElement(mat, 2) > -1.0f) | 414 | if (MathHelper.GetElement(mat, 2) > -1.0f) |
415 | { | 415 | { |
416 | xyz.X = (float)Math.Atan2(-MathHelper.GetElement(mat, 5), MathHelper.GetElement(mat, 8)); | 416 | xyz.X = (float)Math.Atan2(-MathHelper.GetElement(mat, 5), MathHelper.GetElement(mat, 8)); |
417 | xyz.Y = (float)Math.Asin(MathHelper.GetElement(mat, 2)); | 417 | xyz.Y = (float)Math.Asin(MathHelper.GetElement(mat, 2)); |
418 | xyz.Z = (float)Math.Atan2(-MathHelper.GetElement(mat, 1), MathHelper.GetElement(mat, 0)); | 418 | xyz.Z = (float)Math.Atan2(-MathHelper.GetElement(mat, 1), MathHelper.GetElement(mat, 0)); |
419 | return true; | 419 | return true; |
420 | } | 420 | } |
421 | else | 421 | else |
422 | { | 422 | { |
423 | // WARNING. Not unique. XA - ZA = -atan2(r10,r11) | 423 | // WARNING. Not unique. XA - ZA = -atan2(r10,r11) |
424 | xyz.X = -(float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); | 424 | xyz.X = -(float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); |
425 | xyz.Y = -(float)Math.PI / 2; | 425 | xyz.Y = -(float)Math.PI / 2; |
426 | xyz.Z = 0.0f; | 426 | xyz.Z = 0.0f; |
427 | return false; | 427 | return false; |
428 | } | 428 | } |
429 | } | 429 | } |
430 | else | 430 | else |
431 | { | 431 | { |
432 | // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) | 432 | // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) |
433 | xyz.X = (float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); | 433 | xyz.X = (float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); |
434 | xyz.Y = (float)Math.PI / 2; | 434 | xyz.Y = (float)Math.PI / 2; |
435 | xyz.Z = 0.0f; | 435 | xyz.Z = 0.0f; |
436 | return false; | 436 | return false; |
437 | } | 437 | } |
438 | } | 438 | } |
439 | } | 439 | } |
440 | } | 440 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs index 3e35550..02bcf8a 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs | |||
@@ -1,246 +1,246 @@ | |||
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.Dynamics | 27 | namespace XnaDevRu.BulletX.Dynamics |
28 | { | 28 | { |
29 | /// <summary> | 29 | /// <summary> |
30 | /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space | 30 | /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space |
31 | /// axis defines the orientation of the hinge axis | 31 | /// axis defines the orientation of the hinge axis |
32 | /// </summary> | 32 | /// </summary> |
33 | public class HingeConstraint : TypedConstraint | 33 | public class HingeConstraint : TypedConstraint |
34 | { | 34 | { |
35 | private JacobianEntry[] _jac = new JacobianEntry[3]; //3 orthogonal linear constraints | 35 | private JacobianEntry[] _jac = new JacobianEntry[3]; //3 orthogonal linear constraints |
36 | private JacobianEntry[] _jacAng = new JacobianEntry[3]; //2 orthogonal angular constraints + 1 for limit/motor | 36 | private JacobianEntry[] _jacAng = new JacobianEntry[3]; //2 orthogonal angular constraints + 1 for limit/motor |
37 | 37 | ||
38 | private Vector3 _pivotInA; | 38 | private Vector3 _pivotInA; |
39 | private Vector3 _pivotInB; | 39 | private Vector3 _pivotInB; |
40 | private Vector3 _axisInA; | 40 | private Vector3 _axisInA; |
41 | private Vector3 _axisInB; | 41 | private Vector3 _axisInB; |
42 | 42 | ||
43 | private bool _angularOnly; | 43 | private bool _angularOnly; |
44 | 44 | ||
45 | private float _motorTargetVelocity; | 45 | private float _motorTargetVelocity; |
46 | private float _maxMotorImpulse; | 46 | private float _maxMotorImpulse; |
47 | private bool _enableAngularMotor; | 47 | private bool _enableAngularMotor; |
48 | 48 | ||
49 | public HingeConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB, Vector3 axisInA, Vector3 axisInB) | 49 | public HingeConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB, Vector3 axisInA, Vector3 axisInB) |
50 | : base(rbA, rbB) | 50 | : base(rbA, rbB) |
51 | { | 51 | { |
52 | _pivotInA = pivotInA; | 52 | _pivotInA = pivotInA; |
53 | _pivotInB = pivotInB; | 53 | _pivotInB = pivotInB; |
54 | _axisInA = axisInA; | 54 | _axisInA = axisInA; |
55 | _axisInB = -axisInB; | 55 | _axisInB = -axisInB; |
56 | _angularOnly = false; | 56 | _angularOnly = false; |
57 | } | 57 | } |
58 | 58 | ||
59 | public HingeConstraint(RigidBody rbA, Vector3 pivotInA, Vector3 axisInA) | 59 | public HingeConstraint(RigidBody rbA, Vector3 pivotInA, Vector3 axisInA) |
60 | : base(rbA) | 60 | : base(rbA) |
61 | { | 61 | { |
62 | _pivotInA = pivotInA; | 62 | _pivotInA = pivotInA; |
63 | _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, pivotInA); | 63 | _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, pivotInA); |
64 | _axisInA = axisInA; | 64 | _axisInA = axisInA; |
65 | //fixed axis in worldspace | 65 | //fixed axis in worldspace |
66 | _axisInB = MathHelper.TransformNormal(-axisInA, rbA.CenterOfMassTransform); | 66 | _axisInB = MathHelper.TransformNormal(-axisInA, rbA.CenterOfMassTransform); |
67 | _angularOnly = false; | 67 | _angularOnly = false; |
68 | } | 68 | } |
69 | 69 | ||
70 | public HingeConstraint() { } | 70 | public HingeConstraint() { } |
71 | 71 | ||
72 | public bool AngularOnly { set { _angularOnly = value; } } | 72 | public bool AngularOnly { set { _angularOnly = value; } } |
73 | 73 | ||
74 | public override void BuildJacobian() | 74 | public override void BuildJacobian() |
75 | { | 75 | { |
76 | AppliedImpulse = 0f; | 76 | AppliedImpulse = 0f; |
77 | 77 | ||
78 | Vector3 normal = new Vector3(); | 78 | Vector3 normal = new Vector3(); |
79 | 79 | ||
80 | if (!_angularOnly) | 80 | if (!_angularOnly) |
81 | { | 81 | { |
82 | for (int i = 0; i < 3; i++) | 82 | for (int i = 0; i < 3; i++) |
83 | { | 83 | { |
84 | MathHelper.SetElement(ref normal, i, 1); | 84 | MathHelper.SetElement(ref normal, i, 1); |
85 | _jac[i] = new JacobianEntry( | 85 | _jac[i] = new JacobianEntry( |
86 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | 86 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), |
87 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | 87 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), |
88 | MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, | 88 | MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, |
89 | MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, | 89 | MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, |
90 | normal, | 90 | normal, |
91 | RigidBodyA.InvInertiaDiagLocal, | 91 | RigidBodyA.InvInertiaDiagLocal, |
92 | RigidBodyA.InverseMass, | 92 | RigidBodyA.InverseMass, |
93 | RigidBodyB.InvInertiaDiagLocal, | 93 | RigidBodyB.InvInertiaDiagLocal, |
94 | RigidBodyB.InverseMass); | 94 | RigidBodyB.InverseMass); |
95 | MathHelper.SetElement(ref normal, i, 0); | 95 | MathHelper.SetElement(ref normal, i, 0); |
96 | } | 96 | } |
97 | } | 97 | } |
98 | 98 | ||
99 | //calculate two perpendicular jointAxis, orthogonal to hingeAxis | 99 | //calculate two perpendicular jointAxis, orthogonal to hingeAxis |
100 | //these two jointAxis require equal angular velocities for both bodies | 100 | //these two jointAxis require equal angular velocities for both bodies |
101 | //this is unused for now, it's a todo | 101 | //this is unused for now, it's a todo |
102 | Vector3 jointAxisALocal = new Vector3(); | 102 | Vector3 jointAxisALocal = new Vector3(); |
103 | Vector3 jointAxisBLocal = new Vector3(); | 103 | Vector3 jointAxisBLocal = new Vector3(); |
104 | MathHelper.PlaneSpace1(_axisInA, ref jointAxisALocal, ref jointAxisBLocal); | 104 | MathHelper.PlaneSpace1(_axisInA, ref jointAxisALocal, ref jointAxisBLocal); |
105 | 105 | ||
106 | Vector3 jointAxisA = MathHelper.TransformNormal(jointAxisALocal, RigidBodyA.CenterOfMassTransform); | 106 | Vector3 jointAxisA = MathHelper.TransformNormal(jointAxisALocal, RigidBodyA.CenterOfMassTransform); |
107 | Vector3 jointAxisB = MathHelper.TransformNormal(jointAxisBLocal, RigidBodyA.CenterOfMassTransform); | 107 | Vector3 jointAxisB = MathHelper.TransformNormal(jointAxisBLocal, RigidBodyA.CenterOfMassTransform); |
108 | Vector3 hingeAxisWorld = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform); | 108 | Vector3 hingeAxisWorld = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform); |
109 | 109 | ||
110 | _jacAng[0] = new JacobianEntry(jointAxisA, | 110 | _jacAng[0] = new JacobianEntry(jointAxisA, |
111 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | 111 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), |
112 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | 112 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), |
113 | RigidBodyA.InvInertiaDiagLocal, | 113 | RigidBodyA.InvInertiaDiagLocal, |
114 | RigidBodyB.InvInertiaDiagLocal); | 114 | RigidBodyB.InvInertiaDiagLocal); |
115 | 115 | ||
116 | _jacAng[1] = new JacobianEntry(jointAxisB, | 116 | _jacAng[1] = new JacobianEntry(jointAxisB, |
117 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | 117 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), |
118 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | 118 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), |
119 | RigidBodyA.InvInertiaDiagLocal, | 119 | RigidBodyA.InvInertiaDiagLocal, |
120 | RigidBodyB.InvInertiaDiagLocal); | 120 | RigidBodyB.InvInertiaDiagLocal); |
121 | 121 | ||
122 | _jacAng[2] = new JacobianEntry(hingeAxisWorld, | 122 | _jacAng[2] = new JacobianEntry(hingeAxisWorld, |
123 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | 123 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), |
124 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | 124 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), |
125 | RigidBodyA.InvInertiaDiagLocal, | 125 | RigidBodyA.InvInertiaDiagLocal, |
126 | RigidBodyB.InvInertiaDiagLocal); | 126 | RigidBodyB.InvInertiaDiagLocal); |
127 | } | 127 | } |
128 | 128 | ||
129 | public override void SolveConstraint(float timeStep) | 129 | public override void SolveConstraint(float timeStep) |
130 | { | 130 | { |
131 | Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform); | 131 | Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform); |
132 | Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform); | 132 | Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform); |
133 | 133 | ||
134 | Vector3 normal = new Vector3(0, 0, 0); | 134 | Vector3 normal = new Vector3(0, 0, 0); |
135 | float tau = 0.3f; | 135 | float tau = 0.3f; |
136 | float damping = 1f; | 136 | float damping = 1f; |
137 | 137 | ||
138 | //linear part | 138 | //linear part |
139 | if (!_angularOnly) | 139 | if (!_angularOnly) |
140 | { | 140 | { |
141 | for (int i = 0; i < 3; i++) | 141 | for (int i = 0; i < 3; i++) |
142 | { | 142 | { |
143 | if (i == 0) | 143 | if (i == 0) |
144 | normal = new Vector3(1, 0, 0); | 144 | normal = new Vector3(1, 0, 0); |
145 | else if (i == 1) | 145 | else if (i == 1) |
146 | normal = new Vector3(0, 1, 0); | 146 | normal = new Vector3(0, 1, 0); |
147 | else | 147 | else |
148 | normal = new Vector3(0, 0, 1); | 148 | normal = new Vector3(0, 0, 1); |
149 | 149 | ||
150 | float jacDiagABInv = 1f / _jac[i].Diagonal; | 150 | float jacDiagABInv = 1f / _jac[i].Diagonal; |
151 | 151 | ||
152 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; | 152 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; |
153 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; | 153 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; |
154 | 154 | ||
155 | Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1); | 155 | Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1); |
156 | Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2); | 156 | Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2); |
157 | Vector3 vel = vel1 - vel2; | 157 | Vector3 vel = vel1 - vel2; |
158 | float rel_vel; | 158 | float rel_vel; |
159 | rel_vel = Vector3.Dot(normal, vel); | 159 | rel_vel = Vector3.Dot(normal, vel); |
160 | //positional error (zeroth order error) | 160 | //positional error (zeroth order error) |
161 | float depth = -Vector3.Dot(pivotAInW - pivotBInW, normal); //this is the error projected on the normal | 161 | float depth = -Vector3.Dot(pivotAInW - pivotBInW, normal); //this is the error projected on the normal |
162 | float impulse = depth * tau / timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping; | 162 | float impulse = depth * tau / timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping; |
163 | AppliedImpulse += impulse; | 163 | AppliedImpulse += impulse; |
164 | Vector3 impulse_vector = normal * impulse; | 164 | Vector3 impulse_vector = normal * impulse; |
165 | RigidBodyA.ApplyImpulse(impulse_vector, pivotAInW - RigidBodyA.CenterOfMassPosition); | 165 | RigidBodyA.ApplyImpulse(impulse_vector, pivotAInW - RigidBodyA.CenterOfMassPosition); |
166 | RigidBodyB.ApplyImpulse(-impulse_vector, pivotBInW - RigidBodyB.CenterOfMassPosition); | 166 | RigidBodyB.ApplyImpulse(-impulse_vector, pivotBInW - RigidBodyB.CenterOfMassPosition); |
167 | } | 167 | } |
168 | } | 168 | } |
169 | //solve angular part | 169 | //solve angular part |
170 | // get axes in world space | 170 | // get axes in world space |
171 | Vector3 axisA = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform); | 171 | Vector3 axisA = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform); |
172 | Vector3 axisB = MathHelper.TransformNormal(_axisInB, RigidBodyB.CenterOfMassTransform); | 172 | Vector3 axisB = MathHelper.TransformNormal(_axisInB, RigidBodyB.CenterOfMassTransform); |
173 | 173 | ||
174 | Vector3 angVelA = RigidBodyA.AngularVelocity; | 174 | Vector3 angVelA = RigidBodyA.AngularVelocity; |
175 | Vector3 angVelB = RigidBodyB.AngularVelocity; | 175 | Vector3 angVelB = RigidBodyB.AngularVelocity; |
176 | Vector3 angVelAroundHingeAxisA = axisA * Vector3.Dot(axisA, angVelA); | 176 | Vector3 angVelAroundHingeAxisA = axisA * Vector3.Dot(axisA, angVelA); |
177 | Vector3 angVelAroundHingeAxisB = axisB * Vector3.Dot(axisB, angVelB); | 177 | Vector3 angVelAroundHingeAxisB = axisB * Vector3.Dot(axisB, angVelB); |
178 | 178 | ||
179 | Vector3 angAOrthog = angVelA - angVelAroundHingeAxisA; | 179 | Vector3 angAOrthog = angVelA - angVelAroundHingeAxisA; |
180 | Vector3 angBOrthog = angVelB - angVelAroundHingeAxisB; | 180 | Vector3 angBOrthog = angVelB - angVelAroundHingeAxisB; |
181 | Vector3 velrelOrthog = angAOrthog - angBOrthog; | 181 | Vector3 velrelOrthog = angAOrthog - angBOrthog; |
182 | 182 | ||
183 | //solve angular velocity correction | 183 | //solve angular velocity correction |
184 | float relaxation = 1f; | 184 | float relaxation = 1f; |
185 | float len = velrelOrthog.Length(); | 185 | float len = velrelOrthog.Length(); |
186 | if (len > 0.00001f) | 186 | if (len > 0.00001f) |
187 | { | 187 | { |
188 | Vector3 normal2 = Vector3.Normalize(velrelOrthog); | 188 | Vector3 normal2 = Vector3.Normalize(velrelOrthog); |
189 | float denom = RigidBodyA.ComputeAngularImpulseDenominator(normal2) + | 189 | float denom = RigidBodyA.ComputeAngularImpulseDenominator(normal2) + |
190 | RigidBodyB.ComputeAngularImpulseDenominator(normal2); | 190 | RigidBodyB.ComputeAngularImpulseDenominator(normal2); |
191 | // scale for mass and relaxation | 191 | // scale for mass and relaxation |
192 | velrelOrthog *= (1f / denom) * 0.9f; | 192 | velrelOrthog *= (1f / denom) * 0.9f; |
193 | } | 193 | } |
194 | 194 | ||
195 | //solve angular positional correction | 195 | //solve angular positional correction |
196 | Vector3 angularError = -Vector3.Cross(axisA, axisB) * (1f / timeStep); | 196 | Vector3 angularError = -Vector3.Cross(axisA, axisB) * (1f / timeStep); |
197 | float len2 = angularError.Length(); | 197 | float len2 = angularError.Length(); |
198 | if (len2 > 0.00001f) | 198 | if (len2 > 0.00001f) |
199 | { | 199 | { |
200 | Vector3 normal2 = Vector3.Normalize(angularError); | 200 | Vector3 normal2 = Vector3.Normalize(angularError); |
201 | float denom2 = RigidBodyA.ComputeAngularImpulseDenominator(normal2) + | 201 | float denom2 = RigidBodyA.ComputeAngularImpulseDenominator(normal2) + |
202 | RigidBodyB.ComputeAngularImpulseDenominator(normal2); | 202 | RigidBodyB.ComputeAngularImpulseDenominator(normal2); |
203 | angularError *= (1f / denom2) * relaxation; | 203 | angularError *= (1f / denom2) * relaxation; |
204 | } | 204 | } |
205 | 205 | ||
206 | RigidBodyA.ApplyTorqueImpulse(-velrelOrthog + angularError); | 206 | RigidBodyA.ApplyTorqueImpulse(-velrelOrthog + angularError); |
207 | RigidBodyB.ApplyTorqueImpulse(velrelOrthog - angularError); | 207 | RigidBodyB.ApplyTorqueImpulse(velrelOrthog - angularError); |
208 | 208 | ||
209 | //apply motor | 209 | //apply motor |
210 | if (_enableAngularMotor) | 210 | if (_enableAngularMotor) |
211 | { | 211 | { |
212 | //todo: add limits too | 212 | //todo: add limits too |
213 | Vector3 angularLimit = Vector3.Zero; | 213 | Vector3 angularLimit = Vector3.Zero; |
214 | 214 | ||
215 | Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; | 215 | Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; |
216 | float projRelVel = Vector3.Dot(velrel, axisA); | 216 | float projRelVel = Vector3.Dot(velrel, axisA); |
217 | 217 | ||
218 | float desiredMotorVel = _motorTargetVelocity; | 218 | float desiredMotorVel = _motorTargetVelocity; |
219 | float motorRelvel = desiredMotorVel - projRelVel; | 219 | float motorRelvel = desiredMotorVel - projRelVel; |
220 | 220 | ||
221 | float denom3 = RigidBodyA.ComputeAngularImpulseDenominator(axisA) + | 221 | float denom3 = RigidBodyA.ComputeAngularImpulseDenominator(axisA) + |
222 | RigidBodyB.ComputeAngularImpulseDenominator(axisA); | 222 | RigidBodyB.ComputeAngularImpulseDenominator(axisA); |
223 | 223 | ||
224 | float unclippedMotorImpulse = (1f / denom3) * motorRelvel; | 224 | float unclippedMotorImpulse = (1f / denom3) * motorRelvel; |
225 | //todo: should clip against accumulated impulse | 225 | //todo: should clip against accumulated impulse |
226 | float clippedMotorImpulse = unclippedMotorImpulse > _maxMotorImpulse ? _maxMotorImpulse : unclippedMotorImpulse; | 226 | float clippedMotorImpulse = unclippedMotorImpulse > _maxMotorImpulse ? _maxMotorImpulse : unclippedMotorImpulse; |
227 | clippedMotorImpulse = clippedMotorImpulse < -_maxMotorImpulse ? -_maxMotorImpulse : clippedMotorImpulse; | 227 | clippedMotorImpulse = clippedMotorImpulse < -_maxMotorImpulse ? -_maxMotorImpulse : clippedMotorImpulse; |
228 | Vector3 motorImp = clippedMotorImpulse * axisA; | 228 | Vector3 motorImp = clippedMotorImpulse * axisA; |
229 | 229 | ||
230 | RigidBodyA.ApplyTorqueImpulse(motorImp + angularLimit); | 230 | RigidBodyA.ApplyTorqueImpulse(motorImp + angularLimit); |
231 | RigidBodyB.ApplyTorqueImpulse(-motorImp - angularLimit); | 231 | RigidBodyB.ApplyTorqueImpulse(-motorImp - angularLimit); |
232 | } | 232 | } |
233 | } | 233 | } |
234 | 234 | ||
235 | public void EnableAngularMotor(bool enableMotor, float targetVelocity, float maxMotorImpulse) | 235 | public void EnableAngularMotor(bool enableMotor, float targetVelocity, float maxMotorImpulse) |
236 | { | 236 | { |
237 | _enableAngularMotor = enableMotor; | 237 | _enableAngularMotor = enableMotor; |
238 | _motorTargetVelocity = targetVelocity; | 238 | _motorTargetVelocity = targetVelocity; |
239 | _maxMotorImpulse = maxMotorImpulse; | 239 | _maxMotorImpulse = maxMotorImpulse; |
240 | } | 240 | } |
241 | 241 | ||
242 | public void UpdateRHS(float timeStep) | 242 | public void UpdateRHS(float timeStep) |
243 | { | 243 | { |
244 | } | 244 | } |
245 | } | 245 | } |
246 | } | 246 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs index de25225..a83eaa5 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs | |||
@@ -1,32 +1,32 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX.Dynamics | 26 | namespace XnaDevRu.BulletX.Dynamics |
27 | { | 27 | { |
28 | public interface IConstraintSolver | 28 | public interface IConstraintSolver |
29 | { | 29 | { |
30 | float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo info, IDebugDraw debugDrawer); | 30 | float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo info, IDebugDraw debugDrawer); |
31 | } | 31 | } |
32 | } | 32 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs index 3b8d8d8..6249998 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs | |||
@@ -1,155 +1,155 @@ | |||
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.Dynamics | 27 | namespace XnaDevRu.BulletX.Dynamics |
28 | { | 28 | { |
29 | /// <summary> | 29 | /// <summary> |
30 | /// Jacobian entry is an abstraction that allows to describe constraints | 30 | /// Jacobian entry is an abstraction that allows to describe constraints |
31 | /// it can be used in combination with a constraint solver | 31 | /// it can be used in combination with a constraint solver |
32 | /// Can be used to relate the effect of an impulse to the constraint error | 32 | /// Can be used to relate the effect of an impulse to the constraint error |
33 | /// </summary> | 33 | /// </summary> |
34 | public class JacobianEntry | 34 | public class JacobianEntry |
35 | { | 35 | { |
36 | private Vector3 _linearJointAxis; | 36 | private Vector3 _linearJointAxis; |
37 | private Vector3 _aJ; | 37 | private Vector3 _aJ; |
38 | private Vector3 _bJ; | 38 | private Vector3 _bJ; |
39 | private Vector3 _0MinvJt; | 39 | private Vector3 _0MinvJt; |
40 | private Vector3 _1MinvJt; | 40 | private Vector3 _1MinvJt; |
41 | private float _adiag; | 41 | private float _adiag; |
42 | 42 | ||
43 | public JacobianEntry() { } | 43 | public JacobianEntry() { } |
44 | 44 | ||
45 | //constraint between two different rigidbodies | 45 | //constraint between two different rigidbodies |
46 | public JacobianEntry( | 46 | public JacobianEntry( |
47 | Matrix world2A, | 47 | Matrix world2A, |
48 | Matrix world2B, | 48 | Matrix world2B, |
49 | Vector3 relPosA, Vector3 relPosB, | 49 | Vector3 relPosA, Vector3 relPosB, |
50 | Vector3 jointAxis, | 50 | Vector3 jointAxis, |
51 | Vector3 inertiaInvA, | 51 | Vector3 inertiaInvA, |
52 | float massInvA, | 52 | float massInvA, |
53 | Vector3 inertiaInvB, | 53 | Vector3 inertiaInvB, |
54 | float massInvB) | 54 | float massInvB) |
55 | { | 55 | { |
56 | _linearJointAxis = jointAxis; | 56 | _linearJointAxis = jointAxis; |
57 | _aJ = Vector3.TransformNormal(Vector3.Cross(relPosA, _linearJointAxis), world2A); | 57 | _aJ = Vector3.TransformNormal(Vector3.Cross(relPosA, _linearJointAxis), world2A); |
58 | _bJ = Vector3.TransformNormal(Vector3.Cross(relPosB, -_linearJointAxis), world2B); | 58 | _bJ = Vector3.TransformNormal(Vector3.Cross(relPosB, -_linearJointAxis), world2B); |
59 | _0MinvJt = inertiaInvA * _aJ; | 59 | _0MinvJt = inertiaInvA * _aJ; |
60 | _1MinvJt = inertiaInvB * _bJ; | 60 | _1MinvJt = inertiaInvB * _bJ; |
61 | _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ) + massInvB + Vector3.Dot(_1MinvJt, _bJ); | 61 | _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ) + massInvB + Vector3.Dot(_1MinvJt, _bJ); |
62 | 62 | ||
63 | if (_adiag <= 0.0f) | 63 | if (_adiag <= 0.0f) |
64 | throw new BulletException(); | 64 | throw new BulletException(); |
65 | } | 65 | } |
66 | 66 | ||
67 | //angular constraint between two different rigidbodies | 67 | //angular constraint between two different rigidbodies |
68 | public JacobianEntry(Vector3 jointAxis, | 68 | public JacobianEntry(Vector3 jointAxis, |
69 | Matrix world2A, | 69 | Matrix world2A, |
70 | Matrix world2B, | 70 | Matrix world2B, |
71 | Vector3 inertiaInvA, | 71 | Vector3 inertiaInvA, |
72 | Vector3 inertiaInvB) | 72 | Vector3 inertiaInvB) |
73 | { | 73 | { |
74 | _linearJointAxis = new Vector3(); | 74 | _linearJointAxis = new Vector3(); |
75 | _aJ = Vector3.TransformNormal(jointAxis, world2A); | 75 | _aJ = Vector3.TransformNormal(jointAxis, world2A); |
76 | _bJ = Vector3.TransformNormal(-jointAxis, world2B); | 76 | _bJ = Vector3.TransformNormal(-jointAxis, world2B); |
77 | _0MinvJt = inertiaInvA * _aJ; | 77 | _0MinvJt = inertiaInvA * _aJ; |
78 | _1MinvJt = inertiaInvB * _bJ; | 78 | _1MinvJt = inertiaInvB * _bJ; |
79 | _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ); | 79 | _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ); |
80 | 80 | ||
81 | if (_adiag <= 0.0f) | 81 | if (_adiag <= 0.0f) |
82 | throw new BulletException(); | 82 | throw new BulletException(); |
83 | } | 83 | } |
84 | 84 | ||
85 | //angular constraint between two different rigidbodies | 85 | //angular constraint between two different rigidbodies |
86 | public JacobianEntry(Vector3 axisInA, | 86 | public JacobianEntry(Vector3 axisInA, |
87 | Vector3 axisInB, | 87 | Vector3 axisInB, |
88 | Vector3 inertiaInvA, | 88 | Vector3 inertiaInvA, |
89 | Vector3 inertiaInvB) | 89 | Vector3 inertiaInvB) |
90 | { | 90 | { |
91 | _linearJointAxis = new Vector3(); | 91 | _linearJointAxis = new Vector3(); |
92 | _aJ = axisInA; | 92 | _aJ = axisInA; |
93 | _bJ = -axisInB; | 93 | _bJ = -axisInB; |
94 | _0MinvJt = inertiaInvA * _aJ; | 94 | _0MinvJt = inertiaInvA * _aJ; |
95 | _1MinvJt = inertiaInvB * _bJ; | 95 | _1MinvJt = inertiaInvB * _bJ; |
96 | _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ); | 96 | _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ); |
97 | 97 | ||
98 | if (_adiag <= 0.0f) | 98 | if (_adiag <= 0.0f) |
99 | throw new BulletException(); | 99 | throw new BulletException(); |
100 | } | 100 | } |
101 | 101 | ||
102 | //constraint on one rigidbody | 102 | //constraint on one rigidbody |
103 | public JacobianEntry( | 103 | public JacobianEntry( |
104 | Matrix world2A, | 104 | Matrix world2A, |
105 | Vector3 rel_pos1, Vector3 rel_pos2, | 105 | Vector3 rel_pos1, Vector3 rel_pos2, |
106 | Vector3 jointAxis, | 106 | Vector3 jointAxis, |
107 | Vector3 inertiaInvA, | 107 | Vector3 inertiaInvA, |
108 | float massInvA) | 108 | float massInvA) |
109 | { | 109 | { |
110 | _linearJointAxis = jointAxis; | 110 | _linearJointAxis = jointAxis; |
111 | _aJ = Vector3.TransformNormal(Vector3.Cross(rel_pos1, jointAxis), world2A); | 111 | _aJ = Vector3.TransformNormal(Vector3.Cross(rel_pos1, jointAxis), world2A); |
112 | _bJ = Vector3.TransformNormal(Vector3.Cross(rel_pos2, -jointAxis), world2A); | 112 | _bJ = Vector3.TransformNormal(Vector3.Cross(rel_pos2, -jointAxis), world2A); |
113 | _0MinvJt = inertiaInvA * _aJ; | 113 | _0MinvJt = inertiaInvA * _aJ; |
114 | _1MinvJt = new Vector3(); | 114 | _1MinvJt = new Vector3(); |
115 | _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ); | 115 | _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ); |
116 | 116 | ||
117 | if (_adiag <= 0.0f) | 117 | if (_adiag <= 0.0f) |
118 | throw new BulletException(); | 118 | throw new BulletException(); |
119 | } | 119 | } |
120 | 120 | ||
121 | public float Diagonal { get { return _adiag; } } | 121 | public float Diagonal { get { return _adiag; } } |
122 | 122 | ||
123 | // for two constraints on the same rigidbody (for example vehicle friction) | 123 | // for two constraints on the same rigidbody (for example vehicle friction) |
124 | public float GetNonDiagonal(JacobianEntry jacB, float massInvA) | 124 | public float GetNonDiagonal(JacobianEntry jacB, float massInvA) |
125 | { | 125 | { |
126 | float lin = massInvA * Vector3.Dot(_linearJointAxis, jacB._linearJointAxis); | 126 | float lin = massInvA * Vector3.Dot(_linearJointAxis, jacB._linearJointAxis); |
127 | float ang = Vector3.Dot(_0MinvJt, jacB._aJ); | 127 | float ang = Vector3.Dot(_0MinvJt, jacB._aJ); |
128 | return lin + ang; | 128 | return lin + ang; |
129 | } | 129 | } |
130 | 130 | ||
131 | // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) | 131 | // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) |
132 | public float GetNonDiagonal(JacobianEntry jacB, float massInvA, float massInvB) | 132 | public float GetNonDiagonal(JacobianEntry jacB, float massInvA, float massInvB) |
133 | { | 133 | { |
134 | Vector3 lin = _linearJointAxis * jacB._linearJointAxis; | 134 | Vector3 lin = _linearJointAxis * jacB._linearJointAxis; |
135 | Vector3 ang0 = _0MinvJt * jacB._aJ; | 135 | Vector3 ang0 = _0MinvJt * jacB._aJ; |
136 | Vector3 ang1 = _1MinvJt * jacB._bJ; | 136 | Vector3 ang1 = _1MinvJt * jacB._bJ; |
137 | Vector3 lin0 = massInvA * lin; | 137 | Vector3 lin0 = massInvA * lin; |
138 | Vector3 lin1 = massInvB * lin; | 138 | Vector3 lin1 = massInvB * lin; |
139 | Vector3 sum = ang0 + ang1 + lin0 + lin1; | 139 | Vector3 sum = ang0 + ang1 + lin0 + lin1; |
140 | return sum.X + sum.Y + sum.Z; | 140 | return sum.X + sum.Y + sum.Z; |
141 | } | 141 | } |
142 | 142 | ||
143 | public float GetRelativeVelocity(Vector3 linvelA, Vector3 angvelA, Vector3 linvelB, Vector3 angvelB) | 143 | public float GetRelativeVelocity(Vector3 linvelA, Vector3 angvelA, Vector3 linvelB, Vector3 angvelB) |
144 | { | 144 | { |
145 | Vector3 linrel = linvelA - linvelB; | 145 | Vector3 linrel = linvelA - linvelB; |
146 | Vector3 angvela = angvelA * _aJ; | 146 | Vector3 angvela = angvelA * _aJ; |
147 | Vector3 angvelb = angvelB * _bJ; | 147 | Vector3 angvelb = angvelB * _bJ; |
148 | linrel *= _linearJointAxis; | 148 | linrel *= _linearJointAxis; |
149 | angvela += angvelb; | 149 | angvela += angvelb; |
150 | angvela += linrel; | 150 | angvela += linrel; |
151 | float rel_vel2 = angvela.X + angvela.Y + angvela.Z; | 151 | float rel_vel2 = angvela.X + angvela.Y + angvela.Z; |
152 | return rel_vel2 + float.Epsilon; | 152 | return rel_vel2 + float.Epsilon; |
153 | } | 153 | } |
154 | } | 154 | } |
155 | } | 155 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs index d4206ea..8ffd3d5 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs | |||
@@ -1,157 +1,157 @@ | |||
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.Dynamics | 27 | namespace XnaDevRu.BulletX.Dynamics |
28 | { | 28 | { |
29 | public class ConstraintSetting | 29 | public class ConstraintSetting |
30 | { | 30 | { |
31 | private float _tau, _damping; | 31 | private float _tau, _damping; |
32 | 32 | ||
33 | public ConstraintSetting() | 33 | public ConstraintSetting() |
34 | { | 34 | { |
35 | _tau = 0.3f; | 35 | _tau = 0.3f; |
36 | _damping = 1.0f; | 36 | _damping = 1.0f; |
37 | } | 37 | } |
38 | 38 | ||
39 | public float Damping | 39 | public float Damping |
40 | { | 40 | { |
41 | get { return _damping; } | 41 | get { return _damping; } |
42 | set { _damping = value; } | 42 | set { _damping = value; } |
43 | } | 43 | } |
44 | 44 | ||
45 | public float Tau | 45 | public float Tau |
46 | { | 46 | { |
47 | get { return _tau; } | 47 | get { return _tau; } |
48 | set { _tau = value; } | 48 | set { _tau = value; } |
49 | } | 49 | } |
50 | } | 50 | } |
51 | 51 | ||
52 | public class Point2PointConstraint : TypedConstraint | 52 | public class Point2PointConstraint : TypedConstraint |
53 | { | 53 | { |
54 | private JacobianEntry[] _jacobian; | 54 | private JacobianEntry[] _jacobian; |
55 | private Vector3 _pivotInA, _pivotInB; | 55 | private Vector3 _pivotInA, _pivotInB; |
56 | 56 | ||
57 | private ConstraintSetting _setting = new ConstraintSetting(); | 57 | private ConstraintSetting _setting = new ConstraintSetting(); |
58 | 58 | ||
59 | public Point2PointConstraint() | 59 | public Point2PointConstraint() |
60 | { | 60 | { |
61 | _jacobian = new JacobianEntry[3]; | 61 | _jacobian = new JacobianEntry[3]; |
62 | } | 62 | } |
63 | 63 | ||
64 | public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB) | 64 | public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB) |
65 | : base(rbA, rbB) | 65 | : base(rbA, rbB) |
66 | { | 66 | { |
67 | _jacobian = new JacobianEntry[3]; | 67 | _jacobian = new JacobianEntry[3]; |
68 | 68 | ||
69 | _pivotInA = pivotInA; | 69 | _pivotInA = pivotInA; |
70 | _pivotInB = pivotInB; | 70 | _pivotInB = pivotInB; |
71 | } | 71 | } |
72 | 72 | ||
73 | public Point2PointConstraint(RigidBody rbA, Vector3 pivotInA) | 73 | public Point2PointConstraint(RigidBody rbA, Vector3 pivotInA) |
74 | : base(rbA) | 74 | : base(rbA) |
75 | { | 75 | { |
76 | _jacobian = new JacobianEntry[3]; | 76 | _jacobian = new JacobianEntry[3]; |
77 | 77 | ||
78 | _pivotInA = pivotInA; | 78 | _pivotInA = pivotInA; |
79 | _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, _pivotInA); | 79 | _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, _pivotInA); |
80 | } | 80 | } |
81 | 81 | ||
82 | public ConstraintSetting Settings { get { return _setting; } set { _setting = value; } } | 82 | public ConstraintSetting Settings { get { return _setting; } set { _setting = value; } } |
83 | 83 | ||
84 | public Vector3 PivotInA | 84 | public Vector3 PivotInA |
85 | { | 85 | { |
86 | set | 86 | set |
87 | { | 87 | { |
88 | _pivotInA = value; | 88 | _pivotInA = value; |
89 | } | 89 | } |
90 | } | 90 | } |
91 | 91 | ||
92 | public Vector3 PivotInB | 92 | public Vector3 PivotInB |
93 | { | 93 | { |
94 | set | 94 | set |
95 | { | 95 | { |
96 | _pivotInB = value; | 96 | _pivotInB = value; |
97 | } | 97 | } |
98 | } | 98 | } |
99 | 99 | ||
100 | public override void BuildJacobian() | 100 | public override void BuildJacobian() |
101 | { | 101 | { |
102 | Vector3 normal = new Vector3(); | 102 | Vector3 normal = new Vector3(); |
103 | 103 | ||
104 | for (int i = 0; i < 3; i++) | 104 | for (int i = 0; i < 3; i++) |
105 | { | 105 | { |
106 | MathHelper.SetElement(ref normal, i, 1); | 106 | MathHelper.SetElement(ref normal, i, 1); |
107 | _jacobian[i] = new JacobianEntry( | 107 | _jacobian[i] = new JacobianEntry( |
108 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | 108 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), |
109 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | 109 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), |
110 | MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, | 110 | MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, |
111 | MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, | 111 | MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, |
112 | normal, | 112 | normal, |
113 | RigidBodyA.InvInertiaDiagLocal, | 113 | RigidBodyA.InvInertiaDiagLocal, |
114 | RigidBodyA.InverseMass, | 114 | RigidBodyA.InverseMass, |
115 | RigidBodyB.InvInertiaDiagLocal, | 115 | RigidBodyB.InvInertiaDiagLocal, |
116 | RigidBodyB.InverseMass | 116 | RigidBodyB.InverseMass |
117 | ); | 117 | ); |
118 | MathHelper.SetElement(ref normal, i, 0); | 118 | MathHelper.SetElement(ref normal, i, 0); |
119 | } | 119 | } |
120 | } | 120 | } |
121 | 121 | ||
122 | public override void SolveConstraint(float timeStep) | 122 | public override void SolveConstraint(float timeStep) |
123 | { | 123 | { |
124 | Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform); | 124 | Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform); |
125 | Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform); | 125 | Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform); |
126 | 126 | ||
127 | Vector3 normal = new Vector3(); | 127 | Vector3 normal = new Vector3(); |
128 | 128 | ||
129 | for (int i = 0; i < 3; i++) | 129 | for (int i = 0; i < 3; i++) |
130 | { | 130 | { |
131 | MathHelper.SetElement(ref normal, i, 1); | 131 | MathHelper.SetElement(ref normal, i, 1); |
132 | 132 | ||
133 | float jacDiagABInv = 1.0f / _jacobian[i].Diagonal; | 133 | float jacDiagABInv = 1.0f / _jacobian[i].Diagonal; |
134 | 134 | ||
135 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; | 135 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; |
136 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; | 136 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; |
137 | 137 | ||
138 | Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1); | 138 | Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1); |
139 | Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2); | 139 | Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2); |
140 | 140 | ||
141 | Vector3 vel = vel1 - vel2; | 141 | Vector3 vel = vel1 - vel2; |
142 | 142 | ||
143 | float rel_vel = Vector3.Dot(normal, vel); | 143 | float rel_vel = Vector3.Dot(normal, vel); |
144 | float depth = -Vector3.Dot((pivotAInW - pivotBInW), normal); | 144 | float depth = -Vector3.Dot((pivotAInW - pivotBInW), normal); |
145 | 145 | ||
146 | float impulse = depth * _setting.Tau / timeStep * jacDiagABInv - _setting.Damping * rel_vel * jacDiagABInv; | 146 | float impulse = depth * _setting.Tau / timeStep * jacDiagABInv - _setting.Damping * rel_vel * jacDiagABInv; |
147 | AppliedImpulse += impulse; | 147 | AppliedImpulse += impulse; |
148 | Vector3 impulseVector = normal * impulse; | 148 | Vector3 impulseVector = normal * impulse; |
149 | 149 | ||
150 | RigidBodyA.ApplyImpulse(impulseVector, pivotAInW - RigidBodyA.CenterOfMassPosition); | 150 | RigidBodyA.ApplyImpulse(impulseVector, pivotAInW - RigidBodyA.CenterOfMassPosition); |
151 | RigidBodyB.ApplyImpulse(-impulseVector, pivotBInW - RigidBodyB.CenterOfMassPosition); | 151 | RigidBodyB.ApplyImpulse(-impulseVector, pivotBInW - RigidBodyB.CenterOfMassPosition); |
152 | 152 | ||
153 | MathHelper.SetElement(ref normal, i, 0); | 153 | MathHelper.SetElement(ref normal, i, 0); |
154 | } | 154 | } |
155 | } | 155 | } |
156 | } | 156 | } |
157 | } \ No newline at end of file | 157 | } \ No newline at end of file |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs index 4d5e93d..f275550 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs | |||
@@ -1,915 +1,915 @@ | |||
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.Dynamics | 27 | namespace XnaDevRu.BulletX.Dynamics |
28 | { | 28 | { |
29 | [Flags] | 29 | [Flags] |
30 | public enum SolverMode | 30 | public enum SolverMode |
31 | { | 31 | { |
32 | None = 0, | 32 | None = 0, |
33 | RandomizeOrder = 1, | 33 | RandomizeOrder = 1, |
34 | FrictionSeperate = 2, | 34 | FrictionSeperate = 2, |
35 | UseWarmstarting = 4, | 35 | UseWarmstarting = 4, |
36 | CacheFriendly = 8, | 36 | CacheFriendly = 8, |
37 | } | 37 | } |
38 | 38 | ||
39 | public class SequentialImpulseConstraintSolver : IConstraintSolver | 39 | public class SequentialImpulseConstraintSolver : IConstraintSolver |
40 | { | 40 | { |
41 | private static int _totalContactPoints = 0; | 41 | private static int _totalContactPoints = 0; |
42 | 42 | ||
43 | private SolverMode _solverMode; | 43 | private SolverMode _solverMode; |
44 | private int _totalCpd = 0; | 44 | private int _totalCpd = 0; |
45 | private ContactSolverFunc[,] _contactDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType]; | 45 | private ContactSolverFunc[,] _contactDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType]; |
46 | private ContactSolverFunc[,] _frictionDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType]; | 46 | private ContactSolverFunc[,] _frictionDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType]; |
47 | 47 | ||
48 | private float _penetrationResolveFactor = 0.9f; | 48 | private float _penetrationResolveFactor = 0.9f; |
49 | private List<SolverBody> _tmpSolverBodyPool = new List<SolverBody>(); | 49 | private List<SolverBody> _tmpSolverBodyPool = new List<SolverBody>(); |
50 | private List<SolverConstraint> _tmpSolverConstraintPool = new List<SolverConstraint>(); | 50 | private List<SolverConstraint> _tmpSolverConstraintPool = new List<SolverConstraint>(); |
51 | private List<SolverConstraint> _tmpSolverFrictionConstraintPool = new List<SolverConstraint>(); | 51 | private List<SolverConstraint> _tmpSolverFrictionConstraintPool = new List<SolverConstraint>(); |
52 | 52 | ||
53 | private const int _sequentialImpulseMaxSolverPoints = 16384; | 53 | private const int _sequentialImpulseMaxSolverPoints = 16384; |
54 | private static OrderIndex[] _order = new OrderIndex[SequentialImpulseMaxSolverPoints]; | 54 | private static OrderIndex[] _order = new OrderIndex[SequentialImpulseMaxSolverPoints]; |
55 | private static long _seed2 = 0; | 55 | private static long _seed2 = 0; |
56 | 56 | ||
57 | public SequentialImpulseConstraintSolver() | 57 | public SequentialImpulseConstraintSolver() |
58 | { | 58 | { |
59 | _solverMode = SolverMode.RandomizeOrder | SolverMode.CacheFriendly; | 59 | _solverMode = SolverMode.RandomizeOrder | SolverMode.CacheFriendly; |
60 | PersistentManifold.ContactDestroyedCallback = MyContactDestroyedCallback; | 60 | PersistentManifold.ContactDestroyedCallback = MyContactDestroyedCallback; |
61 | 61 | ||
62 | //initialize default friction/contact funcs | 62 | //initialize default friction/contact funcs |
63 | int i, j; | 63 | int i, j; |
64 | for (i = 0; i < (int)ContactSolverType.MaxContactSolverType; i++) | 64 | for (i = 0; i < (int)ContactSolverType.MaxContactSolverType; i++) |
65 | for (j = 0; j < (int)ContactSolverType.MaxContactSolverType; j++) | 65 | for (j = 0; j < (int)ContactSolverType.MaxContactSolverType; j++) |
66 | { | 66 | { |
67 | 67 | ||
68 | _contactDispatch[i, j] = ContactConstraint.ResolveSingleCollision; | 68 | _contactDispatch[i, j] = ContactConstraint.ResolveSingleCollision; |
69 | _frictionDispatch[i, j] = ContactConstraint.ResolveSingleFriction; | 69 | _frictionDispatch[i, j] = ContactConstraint.ResolveSingleFriction; |
70 | } | 70 | } |
71 | } | 71 | } |
72 | 72 | ||
73 | public SolverMode SolverMode { get { return _solverMode; } set { _solverMode = value; } } | 73 | public SolverMode SolverMode { get { return _solverMode; } set { _solverMode = value; } } |
74 | public static int SequentialImpulseMaxSolverPoints { get { return _sequentialImpulseMaxSolverPoints; } } | 74 | public static int SequentialImpulseMaxSolverPoints { get { return _sequentialImpulseMaxSolverPoints; } } |
75 | protected static OrderIndex[] Order { get { return _order; } set { _order = value; } } | 75 | protected static OrderIndex[] Order { get { return _order; } set { _order = value; } } |
76 | public static long RandSeed { get { return _seed2; } set { _seed2 = value; } } | 76 | public static long RandSeed { get { return _seed2; } set { _seed2 = value; } } |
77 | 77 | ||
78 | ///<summary> | 78 | ///<summary> |
79 | /// Advanced: Override the default contact solving function for contacts, for certain types of rigidbody | 79 | /// Advanced: Override the default contact solving function for contacts, for certain types of rigidbody |
80 | /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType | 80 | /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType |
81 | ///</summary> | 81 | ///</summary> |
82 | public void SetContactSolverFunc(ContactSolverFunc func, int typeA, int typeB) | 82 | public void SetContactSolverFunc(ContactSolverFunc func, int typeA, int typeB) |
83 | { | 83 | { |
84 | _contactDispatch[typeA, typeB] = func; | 84 | _contactDispatch[typeA, typeB] = func; |
85 | } | 85 | } |
86 | 86 | ||
87 | /// <summary> | 87 | /// <summary> |
88 | /// Advanced: Override the default friction solving function for contacts, for certain types of rigidbody | 88 | /// Advanced: Override the default friction solving function for contacts, for certain types of rigidbody |
89 | /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType | 89 | /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType |
90 | ///</summary> | 90 | ///</summary> |
91 | public void SetFrictionSolverFunc(ContactSolverFunc func, int typeA, int typeB) | 91 | public void SetFrictionSolverFunc(ContactSolverFunc func, int typeA, int typeB) |
92 | { | 92 | { |
93 | _frictionDispatch[typeA, typeB] = func; | 93 | _frictionDispatch[typeA, typeB] = func; |
94 | } | 94 | } |
95 | 95 | ||
96 | protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) | 96 | protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) |
97 | { | 97 | { |
98 | float maxImpulse = 0; | 98 | float maxImpulse = 0; |
99 | 99 | ||
100 | Vector3 color = new Vector3(0, 1, 0); | 100 | Vector3 color = new Vector3(0, 1, 0); |
101 | if (cp.Distance <= 0) | 101 | if (cp.Distance <= 0) |
102 | { | 102 | { |
103 | if (iter == 0) | 103 | if (iter == 0) |
104 | if(debugDraw != null) | 104 | if(debugDraw != null) |
105 | debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); | 105 | debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); |
106 | 106 | ||
107 | ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; | 107 | ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; |
108 | float impulse = cpd.ContactSolverFunc( | 108 | float impulse = cpd.ContactSolverFunc( |
109 | bodyA, bodyB, | 109 | bodyA, bodyB, |
110 | cp, | 110 | cp, |
111 | info); | 111 | info); |
112 | 112 | ||
113 | if (maxImpulse < impulse) | 113 | if (maxImpulse < impulse) |
114 | maxImpulse = impulse; | 114 | maxImpulse = impulse; |
115 | } | 115 | } |
116 | return maxImpulse; | 116 | return maxImpulse; |
117 | } | 117 | } |
118 | 118 | ||
119 | protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter) | 119 | protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter) |
120 | { | 120 | { |
121 | return Solve(bodyA, bodyB, cp, info, iter, null); | 121 | return Solve(bodyA, bodyB, cp, info, iter, null); |
122 | } | 122 | } |
123 | 123 | ||
124 | protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) | 124 | protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) |
125 | { | 125 | { |
126 | float maxImpulse = 0; | 126 | float maxImpulse = 0; |
127 | 127 | ||
128 | Vector3 color = new Vector3(0, 1, 0); | 128 | Vector3 color = new Vector3(0, 1, 0); |
129 | if (cp.Distance <= 0) | 129 | if (cp.Distance <= 0) |
130 | { | 130 | { |
131 | if (iter == 0) | 131 | if (iter == 0) |
132 | if (debugDraw != null) | 132 | if (debugDraw != null) |
133 | debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); | 133 | debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); |
134 | 134 | ||
135 | float impulse = ContactConstraint.ResolveSingleCollisionCombined( | 135 | float impulse = ContactConstraint.ResolveSingleCollisionCombined( |
136 | bodyA, bodyB, | 136 | bodyA, bodyB, |
137 | cp, | 137 | cp, |
138 | info); | 138 | info); |
139 | 139 | ||
140 | if (maxImpulse < impulse) | 140 | if (maxImpulse < impulse) |
141 | maxImpulse = impulse; | 141 | maxImpulse = impulse; |
142 | } | 142 | } |
143 | return maxImpulse; | 143 | return maxImpulse; |
144 | } | 144 | } |
145 | 145 | ||
146 | protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) | 146 | protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) |
147 | { | 147 | { |
148 | Vector3 color = new Vector3(0, 1, 0); | 148 | Vector3 color = new Vector3(0, 1, 0); |
149 | 149 | ||
150 | if (cp.Distance <= 0) | 150 | if (cp.Distance <= 0) |
151 | { | 151 | { |
152 | 152 | ||
153 | ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; | 153 | ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; |
154 | cpd.FrictionSolverFunc( | 154 | cpd.FrictionSolverFunc( |
155 | bodyA, bodyB, | 155 | bodyA, bodyB, |
156 | cp, | 156 | cp, |
157 | info); | 157 | info); |
158 | } | 158 | } |
159 | return 0; | 159 | return 0; |
160 | } | 160 | } |
161 | 161 | ||
162 | protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info) | 162 | protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info) |
163 | { | 163 | { |
164 | RigidBody body0 = manifold.BodyA as RigidBody; | 164 | RigidBody body0 = manifold.BodyA as RigidBody; |
165 | RigidBody body1 = manifold.BodyB as RigidBody; | 165 | RigidBody body1 = manifold.BodyB as RigidBody; |
166 | 166 | ||
167 | 167 | ||
168 | //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop | 168 | //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop |
169 | { | 169 | { |
170 | manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform); | 170 | manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform); |
171 | 171 | ||
172 | int numpoints = manifold.ContactsCount; | 172 | int numpoints = manifold.ContactsCount; |
173 | 173 | ||
174 | _totalContactPoints += numpoints; | 174 | _totalContactPoints += numpoints; |
175 | 175 | ||
176 | Vector3 color = new Vector3(0, 1, 0); | 176 | Vector3 color = new Vector3(0, 1, 0); |
177 | for (int i = 0; i < numpoints; i++) | 177 | for (int i = 0; i < numpoints; i++) |
178 | { | 178 | { |
179 | ManifoldPoint cp = manifold.GetContactPoint(i); | 179 | ManifoldPoint cp = manifold.GetContactPoint(i); |
180 | if (cp.Distance <= 0) | 180 | if (cp.Distance <= 0) |
181 | { | 181 | { |
182 | Vector3 pos1 = cp.PositionWorldOnA; | 182 | Vector3 pos1 = cp.PositionWorldOnA; |
183 | Vector3 pos2 = cp.PositionWorldOnB; | 183 | Vector3 pos2 = cp.PositionWorldOnB; |
184 | 184 | ||
185 | Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition; | 185 | Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition; |
186 | Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition; | 186 | Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition; |
187 | 187 | ||
188 | 188 | ||
189 | //this jacobian entry is re-used for all iterations | 189 | //this jacobian entry is re-used for all iterations |
190 | JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform), | 190 | JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform), |
191 | MatrixOperations.Transpose(body1.CenterOfMassTransform), | 191 | MatrixOperations.Transpose(body1.CenterOfMassTransform), |
192 | rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass, | 192 | rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass, |
193 | body1.InvInertiaDiagLocal, body1.InverseMass); | 193 | body1.InvInertiaDiagLocal, body1.InverseMass); |
194 | 194 | ||
195 | float jacDiagAB = jac.Diagonal; | 195 | float jacDiagAB = jac.Diagonal; |
196 | 196 | ||
197 | ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; | 197 | ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; |
198 | if (cpd != null) | 198 | if (cpd != null) |
199 | { | 199 | { |
200 | //might be invalid | 200 | //might be invalid |
201 | cpd.PersistentLifeTime++; | 201 | cpd.PersistentLifeTime++; |
202 | if (cpd.PersistentLifeTime != cp.LifeTime) | 202 | if (cpd.PersistentLifeTime != cp.LifeTime) |
203 | { | 203 | { |
204 | //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); | 204 | //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); |
205 | cpd = new ConstraintPersistentData(); | 205 | cpd = new ConstraintPersistentData(); |
206 | cpd.PersistentLifeTime = cp.LifeTime; | 206 | cpd.PersistentLifeTime = cp.LifeTime; |
207 | 207 | ||
208 | } | 208 | } |
209 | } | 209 | } |
210 | else | 210 | else |
211 | { | 211 | { |
212 | 212 | ||
213 | cpd = new ConstraintPersistentData(); | 213 | cpd = new ConstraintPersistentData(); |
214 | _totalCpd++; | 214 | _totalCpd++; |
215 | //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); | 215 | //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); |
216 | cp.UserPersistentData = cpd; | 216 | cp.UserPersistentData = cpd; |
217 | cpd.PersistentLifeTime = cp.LifeTime; | 217 | cpd.PersistentLifeTime = cp.LifeTime; |
218 | //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime()); | 218 | //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime()); |
219 | 219 | ||
220 | } | 220 | } |
221 | if (cpd == null) | 221 | if (cpd == null) |
222 | throw new BulletException(); | 222 | throw new BulletException(); |
223 | 223 | ||
224 | cpd.JacDiagABInv = 1f / jacDiagAB; | 224 | cpd.JacDiagABInv = 1f / jacDiagAB; |
225 | 225 | ||
226 | //Dependent on Rigidbody A and B types, fetch the contact/friction response func | 226 | //Dependent on Rigidbody A and B types, fetch the contact/friction response func |
227 | //perhaps do a similar thing for friction/restutution combiner funcs... | 227 | //perhaps do a similar thing for friction/restutution combiner funcs... |
228 | 228 | ||
229 | cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType]; | 229 | cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType]; |
230 | cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType]; | 230 | cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType]; |
231 | 231 | ||
232 | Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1); | 232 | Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1); |
233 | Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2); | 233 | Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2); |
234 | Vector3 vel = vel1 - vel2; | 234 | Vector3 vel = vel1 - vel2; |
235 | float rel_vel; | 235 | float rel_vel; |
236 | rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); | 236 | rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); |
237 | 237 | ||
238 | float combinedRestitution = cp.CombinedRestitution; | 238 | float combinedRestitution = cp.CombinedRestitution; |
239 | 239 | ||
240 | cpd.Penetration = cp.Distance; | 240 | cpd.Penetration = cp.Distance; |
241 | cpd.Friction = cp.CombinedFriction; | 241 | cpd.Friction = cp.CombinedFriction; |
242 | cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution); | 242 | cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution); |
243 | if (cpd.Restitution < 0f) | 243 | if (cpd.Restitution < 0f) |
244 | { | 244 | { |
245 | cpd.Restitution = 0.0f; | 245 | cpd.Restitution = 0.0f; |
246 | 246 | ||
247 | }; | 247 | }; |
248 | 248 | ||
249 | //restitution and penetration work in same direction so | 249 | //restitution and penetration work in same direction so |
250 | //rel_vel | 250 | //rel_vel |
251 | 251 | ||
252 | float penVel = -cpd.Penetration / info.TimeStep; | 252 | float penVel = -cpd.Penetration / info.TimeStep; |
253 | 253 | ||
254 | if (cpd.Restitution > penVel) | 254 | if (cpd.Restitution > penVel) |
255 | { | 255 | { |
256 | cpd.Penetration = 0; | 256 | cpd.Penetration = 0; |
257 | } | 257 | } |
258 | 258 | ||
259 | 259 | ||
260 | float relaxation = info.Damping; | 260 | float relaxation = info.Damping; |
261 | if ((_solverMode & SolverMode.UseWarmstarting) != 0) | 261 | if ((_solverMode & SolverMode.UseWarmstarting) != 0) |
262 | { | 262 | { |
263 | cpd.AppliedImpulse *= relaxation; | 263 | cpd.AppliedImpulse *= relaxation; |
264 | } | 264 | } |
265 | else | 265 | else |
266 | { | 266 | { |
267 | cpd.AppliedImpulse = 0f; | 267 | cpd.AppliedImpulse = 0f; |
268 | } | 268 | } |
269 | 269 | ||
270 | //for friction | 270 | //for friction |
271 | cpd.PreviousAppliedImpulse = cpd.AppliedImpulse; | 271 | cpd.PreviousAppliedImpulse = cpd.AppliedImpulse; |
272 | 272 | ||
273 | //re-calculate friction direction every frame, todo: check if this is really needed | 273 | //re-calculate friction direction every frame, todo: check if this is really needed |
274 | Vector3 fwta = cpd.FrictionWorldTangentialA; | 274 | Vector3 fwta = cpd.FrictionWorldTangentialA; |
275 | Vector3 fwtb = cpd.FrictionWorldTangentialB; | 275 | Vector3 fwtb = cpd.FrictionWorldTangentialB; |
276 | MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb); | 276 | MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb); |
277 | cpd.FrictionWorldTangentialA = fwta; | 277 | cpd.FrictionWorldTangentialA = fwta; |
278 | cpd.FrictionWorldTangentialB = fwtb; | 278 | cpd.FrictionWorldTangentialB = fwtb; |
279 | 279 | ||
280 | cpd.AccumulatedTangentImpulseA = 0; | 280 | cpd.AccumulatedTangentImpulseA = 0; |
281 | cpd.AccumulatedTangentImpulseB = 0; | 281 | cpd.AccumulatedTangentImpulseB = 0; |
282 | float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA); | 282 | float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA); |
283 | float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA); | 283 | float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA); |
284 | float denom = relaxation / (denom0 + denom1); | 284 | float denom = relaxation / (denom0 + denom1); |
285 | cpd.JacDiagABInvTangentA = denom; | 285 | cpd.JacDiagABInvTangentA = denom; |
286 | 286 | ||
287 | 287 | ||
288 | denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB); | 288 | denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB); |
289 | denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB); | 289 | denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB); |
290 | denom = relaxation / (denom0 + denom1); | 290 | denom = relaxation / (denom0 + denom1); |
291 | cpd.JacDiagABInvTangentB = denom; | 291 | cpd.JacDiagABInvTangentB = denom; |
292 | 292 | ||
293 | Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse; | 293 | Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse; |
294 | 294 | ||
295 | { | 295 | { |
296 | Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); | 296 | Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); |
297 | cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld); | 297 | cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld); |
298 | Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); | 298 | Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); |
299 | cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld); | 299 | cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld); |
300 | } | 300 | } |
301 | { | 301 | { |
302 | Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA); | 302 | Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA); |
303 | cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld); | 303 | cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld); |
304 | } | 304 | } |
305 | { | 305 | { |
306 | Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB); | 306 | Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB); |
307 | cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld); | 307 | cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld); |
308 | } | 308 | } |
309 | { | 309 | { |
310 | Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA); | 310 | Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA); |
311 | cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld); | 311 | cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld); |
312 | } | 312 | } |
313 | { | 313 | { |
314 | Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB); | 314 | Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB); |
315 | cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld); | 315 | cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld); |
316 | } | 316 | } |
317 | 317 | ||
318 | 318 | ||
319 | //apply previous frames impulse on both bodies | 319 | //apply previous frames impulse on both bodies |
320 | body0.ApplyImpulse(totalImpulse, rel_pos1); | 320 | body0.ApplyImpulse(totalImpulse, rel_pos1); |
321 | body1.ApplyImpulse(-totalImpulse, rel_pos2); | 321 | body1.ApplyImpulse(-totalImpulse, rel_pos2); |
322 | } | 322 | } |
323 | } | 323 | } |
324 | } | 324 | } |
325 | } | 325 | } |
326 | 326 | ||
327 | private bool MyContactDestroyedCallback(object userPersistentData) | 327 | private bool MyContactDestroyedCallback(object userPersistentData) |
328 | { | 328 | { |
329 | if (userPersistentData == null) | 329 | if (userPersistentData == null) |
330 | throw new BulletException(); | 330 | throw new BulletException(); |
331 | ConstraintPersistentData cpd = userPersistentData as ConstraintPersistentData; | 331 | ConstraintPersistentData cpd = userPersistentData as ConstraintPersistentData; |
332 | _totalCpd--; | 332 | _totalCpd--; |
333 | return true; | 333 | return true; |
334 | } | 334 | } |
335 | 335 | ||
336 | private float RestitutionCurve(float relVel, float restitution) | 336 | private float RestitutionCurve(float relVel, float restitution) |
337 | { | 337 | { |
338 | float rest = restitution * -relVel; | 338 | float rest = restitution * -relVel; |
339 | return rest; | 339 | return rest; |
340 | } | 340 | } |
341 | 341 | ||
342 | //velocity + friction | 342 | //velocity + friction |
343 | //response between two dynamic objects with friction | 343 | //response between two dynamic objects with friction |
344 | public virtual float ResolveSingleCollisionCombinedCacheFriendly( | 344 | public virtual float ResolveSingleCollisionCombinedCacheFriendly( |
345 | SolverBody bodyA, | 345 | SolverBody bodyA, |
346 | SolverBody bodyB, | 346 | SolverBody bodyB, |
347 | SolverConstraint contactConstraint, | 347 | SolverConstraint contactConstraint, |
348 | ContactSolverInfo solverInfo) | 348 | ContactSolverInfo solverInfo) |
349 | { | 349 | { |
350 | float normalImpulse = 0; | 350 | float normalImpulse = 0; |
351 | 351 | ||
352 | if (contactConstraint.Penetration < 0) | 352 | if (contactConstraint.Penetration < 0) |
353 | return 0; | 353 | return 0; |
354 | 354 | ||
355 | float relVel; | 355 | float relVel; |
356 | float velADotn = Vector3.Dot(contactConstraint.ContactNormal,bodyA.LinearVelocity) | 356 | float velADotn = Vector3.Dot(contactConstraint.ContactNormal,bodyA.LinearVelocity) |
357 | + Vector3.Dot(contactConstraint.RelPosACrossNormal,bodyA.AngularVelocity); | 357 | + Vector3.Dot(contactConstraint.RelPosACrossNormal,bodyA.AngularVelocity); |
358 | float velBDotn = Vector3.Dot(contactConstraint.ContactNormal,bodyB.LinearVelocity) | 358 | float velBDotn = Vector3.Dot(contactConstraint.ContactNormal,bodyB.LinearVelocity) |
359 | + Vector3.Dot(contactConstraint.RelPosBCrossNormal,bodyB.AngularVelocity); | 359 | + Vector3.Dot(contactConstraint.RelPosBCrossNormal,bodyB.AngularVelocity); |
360 | 360 | ||
361 | relVel = velADotn - velBDotn; | 361 | relVel = velADotn - velBDotn; |
362 | 362 | ||
363 | float positionalError = contactConstraint.Penetration; | 363 | float positionalError = contactConstraint.Penetration; |
364 | float velocityError = contactConstraint.Restitution - relVel;// * damping; | 364 | float velocityError = contactConstraint.Restitution - relVel;// * damping; |
365 | 365 | ||
366 | float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv; | 366 | float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv; |
367 | float velocityImpulse = velocityError * contactConstraint.JacDiagABInv; | 367 | float velocityImpulse = velocityError * contactConstraint.JacDiagABInv; |
368 | normalImpulse = penetrationImpulse + velocityImpulse; | 368 | normalImpulse = penetrationImpulse + velocityImpulse; |
369 | 369 | ||
370 | // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse | 370 | // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse |
371 | float oldNormalImpulse = contactConstraint.AppliedImpulse; | 371 | float oldNormalImpulse = contactConstraint.AppliedImpulse; |
372 | float sum = oldNormalImpulse + normalImpulse; | 372 | float sum = oldNormalImpulse + normalImpulse; |
373 | contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum; | 373 | contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum; |
374 | 374 | ||
375 | float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse; | 375 | float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse; |
376 | float velocitySum = oldVelocityImpulse + velocityImpulse; | 376 | float velocitySum = oldVelocityImpulse + velocityImpulse; |
377 | contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum; | 377 | contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum; |
378 | 378 | ||
379 | normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse; | 379 | normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse; |
380 | 380 | ||
381 | if (bodyA.InvMass != 0) | 381 | if (bodyA.InvMass != 0) |
382 | { | 382 | { |
383 | bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, | 383 | bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, |
384 | contactConstraint.AngularComponentA, normalImpulse); | 384 | contactConstraint.AngularComponentA, normalImpulse); |
385 | } | 385 | } |
386 | if (bodyB.InvMass != 0) | 386 | if (bodyB.InvMass != 0) |
387 | { | 387 | { |
388 | bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, | 388 | bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, |
389 | contactConstraint.AngularComponentB, -normalImpulse); | 389 | contactConstraint.AngularComponentB, -normalImpulse); |
390 | } | 390 | } |
391 | 391 | ||
392 | return normalImpulse; | 392 | return normalImpulse; |
393 | } | 393 | } |
394 | 394 | ||
395 | public virtual float ResolveSingleFrictionCacheFriendly( | 395 | public virtual float ResolveSingleFrictionCacheFriendly( |
396 | SolverBody bodyA, | 396 | SolverBody bodyA, |
397 | SolverBody bodyB, | 397 | SolverBody bodyB, |
398 | SolverConstraint contactConstraint, | 398 | SolverConstraint contactConstraint, |
399 | ContactSolverInfo solverInfo, | 399 | ContactSolverInfo solverInfo, |
400 | float appliedNormalImpulse) | 400 | float appliedNormalImpulse) |
401 | { | 401 | { |
402 | float combinedFriction = contactConstraint.Friction; | 402 | float combinedFriction = contactConstraint.Friction; |
403 | float limit = appliedNormalImpulse * combinedFriction; | 403 | float limit = appliedNormalImpulse * combinedFriction; |
404 | 404 | ||
405 | if (appliedNormalImpulse > 0) | 405 | if (appliedNormalImpulse > 0) |
406 | //friction | 406 | //friction |
407 | { | 407 | { |
408 | float j1; | 408 | float j1; |
409 | { | 409 | { |
410 | float relVel; | 410 | float relVel; |
411 | float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity) | 411 | float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity) |
412 | + Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity); | 412 | + Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity); |
413 | float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity) | 413 | float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity) |
414 | + Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity); | 414 | + Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity); |
415 | relVel = velADotn - velBDotn; | 415 | relVel = velADotn - velBDotn; |
416 | 416 | ||
417 | // calculate j that moves us to zero relative velocity | 417 | // calculate j that moves us to zero relative velocity |
418 | j1 = -relVel * contactConstraint.JacDiagABInv; | 418 | j1 = -relVel * contactConstraint.JacDiagABInv; |
419 | float oldTangentImpulse = contactConstraint.AppliedImpulse; | 419 | float oldTangentImpulse = contactConstraint.AppliedImpulse; |
420 | contactConstraint.AppliedImpulse = oldTangentImpulse + j1; | 420 | contactConstraint.AppliedImpulse = oldTangentImpulse + j1; |
421 | 421 | ||
422 | float test = contactConstraint.AppliedImpulse; | 422 | float test = contactConstraint.AppliedImpulse; |
423 | MathHelper.SetMin(ref test, limit); | 423 | MathHelper.SetMin(ref test, limit); |
424 | MathHelper.SetMax(ref test, -limit); | 424 | MathHelper.SetMax(ref test, -limit); |
425 | contactConstraint.AppliedImpulse = test; | 425 | contactConstraint.AppliedImpulse = test; |
426 | 426 | ||
427 | j1 = contactConstraint.AppliedImpulse - oldTangentImpulse; | 427 | j1 = contactConstraint.AppliedImpulse - oldTangentImpulse; |
428 | } | 428 | } |
429 | 429 | ||
430 | if (bodyA.InvMass != 0) | 430 | if (bodyA.InvMass != 0) |
431 | { | 431 | { |
432 | bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1); | 432 | bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1); |
433 | } | 433 | } |
434 | if (bodyB.InvMass != 0) | 434 | if (bodyB.InvMass != 0) |
435 | { | 435 | { |
436 | bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1); | 436 | bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1); |
437 | } | 437 | } |
438 | } | 438 | } |
439 | return 0; | 439 | return 0; |
440 | } | 440 | } |
441 | 441 | ||
442 | public virtual float SolveGroupCacheFriendly(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) | 442 | public virtual float SolveGroupCacheFriendly(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) |
443 | { | 443 | { |
444 | if (constraints.Count + numManifolds == 0) | 444 | if (constraints.Count + numManifolds == 0) |
445 | { | 445 | { |
446 | return 0; | 446 | return 0; |
447 | } | 447 | } |
448 | 448 | ||
449 | for (int i = 0; i < numManifolds; i++) | 449 | for (int i = 0; i < numManifolds; i++) |
450 | { | 450 | { |
451 | PersistentManifold manifold = manifolds[i]; | 451 | PersistentManifold manifold = manifolds[i]; |
452 | RigidBody rbA = (RigidBody)manifold.BodyA; | 452 | RigidBody rbA = (RigidBody)manifold.BodyA; |
453 | RigidBody rbB = (RigidBody)manifold.BodyB; | 453 | RigidBody rbB = (RigidBody)manifold.BodyB; |
454 | 454 | ||
455 | manifold.RefreshContactPoints(rbA.CenterOfMassTransform, rbB.CenterOfMassTransform); | 455 | manifold.RefreshContactPoints(rbA.CenterOfMassTransform, rbB.CenterOfMassTransform); |
456 | } | 456 | } |
457 | 457 | ||
458 | int minReservation = manifolds.Count * 2; | 458 | int minReservation = manifolds.Count * 2; |
459 | 459 | ||
460 | _tmpSolverBodyPool = new List<SolverBody>(minReservation); | 460 | _tmpSolverBodyPool = new List<SolverBody>(minReservation); |
461 | 461 | ||
462 | for (int i = 0; i < bodies.Count; i++) | 462 | for (int i = 0; i < bodies.Count; i++) |
463 | { | 463 | { |
464 | RigidBody rb = RigidBody.Upcast(bodies[i]); | 464 | RigidBody rb = RigidBody.Upcast(bodies[i]); |
465 | if (rb != null && rb.IslandTag >= 0) | 465 | if (rb != null && rb.IslandTag >= 0) |
466 | { | 466 | { |
467 | BulletDebug.Assert(rb.CompanionID < 0); | 467 | BulletDebug.Assert(rb.CompanionID < 0); |
468 | int solverBodyId = _tmpSolverBodyPool.Count; | 468 | int solverBodyId = _tmpSolverBodyPool.Count; |
469 | SolverBody solverBody; | 469 | SolverBody solverBody; |
470 | InitSolverBody(out solverBody, rb); | 470 | InitSolverBody(out solverBody, rb); |
471 | _tmpSolverBodyPool.Add(solverBody); | 471 | _tmpSolverBodyPool.Add(solverBody); |
472 | rb.CompanionID = solverBodyId; | 472 | rb.CompanionID = solverBodyId; |
473 | } | 473 | } |
474 | } | 474 | } |
475 | 475 | ||
476 | _tmpSolverConstraintPool = new List<SolverConstraint>(minReservation); | 476 | _tmpSolverConstraintPool = new List<SolverConstraint>(minReservation); |
477 | _tmpSolverFrictionConstraintPool = new List<SolverConstraint>(minReservation); | 477 | _tmpSolverFrictionConstraintPool = new List<SolverConstraint>(minReservation); |
478 | 478 | ||
479 | for (int i = 0; i < numManifolds; i++) | 479 | for (int i = 0; i < numManifolds; i++) |
480 | { | 480 | { |
481 | PersistentManifold manifold = manifolds[i]; | 481 | PersistentManifold manifold = manifolds[i]; |
482 | RigidBody rb0 = (RigidBody)manifold.BodyA; | 482 | RigidBody rb0 = (RigidBody)manifold.BodyA; |
483 | RigidBody rb1 = (RigidBody)manifold.BodyB; | 483 | RigidBody rb1 = (RigidBody)manifold.BodyB; |
484 | 484 | ||
485 | int solverBodyIdA = -1; | 485 | int solverBodyIdA = -1; |
486 | int solverBodyIdB = -1; | 486 | int solverBodyIdB = -1; |
487 | 487 | ||
488 | //if (i == 89) | 488 | //if (i == 89) |
489 | // System.Diagnostics.Debugger.Break(); | 489 | // System.Diagnostics.Debugger.Break(); |
490 | 490 | ||
491 | if (manifold.ContactsCount != 0) | 491 | if (manifold.ContactsCount != 0) |
492 | { | 492 | { |
493 | if (rb0.IslandTag >= 0) | 493 | if (rb0.IslandTag >= 0) |
494 | { | 494 | { |
495 | solverBodyIdA = rb0.CompanionID; | 495 | solverBodyIdA = rb0.CompanionID; |
496 | } | 496 | } |
497 | else | 497 | else |
498 | { | 498 | { |
499 | //create a static body | 499 | //create a static body |
500 | solverBodyIdA = _tmpSolverBodyPool.Count; | 500 | solverBodyIdA = _tmpSolverBodyPool.Count; |
501 | SolverBody solverBody; | 501 | SolverBody solverBody; |
502 | InitSolverBody(out solverBody, rb0); | 502 | InitSolverBody(out solverBody, rb0); |
503 | _tmpSolverBodyPool.Add(solverBody); | 503 | _tmpSolverBodyPool.Add(solverBody); |
504 | } | 504 | } |
505 | 505 | ||
506 | if (rb1.IslandTag >= 0) | 506 | if (rb1.IslandTag >= 0) |
507 | { | 507 | { |
508 | solverBodyIdB = rb1.CompanionID; | 508 | solverBodyIdB = rb1.CompanionID; |
509 | } | 509 | } |
510 | else | 510 | else |
511 | { | 511 | { |
512 | //create a static body | 512 | //create a static body |
513 | solverBodyIdB = _tmpSolverBodyPool.Count; | 513 | solverBodyIdB = _tmpSolverBodyPool.Count; |
514 | SolverBody solverBody; | 514 | SolverBody solverBody; |
515 | InitSolverBody(out solverBody, rb1); | 515 | InitSolverBody(out solverBody, rb1); |
516 | _tmpSolverBodyPool.Add(solverBody); | 516 | _tmpSolverBodyPool.Add(solverBody); |
517 | } | 517 | } |
518 | } | 518 | } |
519 | 519 | ||
520 | if (solverBodyIdB == -1 || solverBodyIdA == -1) | 520 | if (solverBodyIdB == -1 || solverBodyIdA == -1) |
521 | System.Diagnostics.Debug.WriteLine(string.Format("We're in ass ! {0}", i)); | 521 | System.Diagnostics.Debug.WriteLine(string.Format("We're in ass ! {0}", i)); |
522 | 522 | ||
523 | for (int j = 0; j < manifold.ContactsCount; j++) | 523 | for (int j = 0; j < manifold.ContactsCount; j++) |
524 | { | 524 | { |
525 | ManifoldPoint cp = manifold.GetContactPoint(j); | 525 | ManifoldPoint cp = manifold.GetContactPoint(j); |
526 | 526 | ||
527 | int frictionIndex = _tmpSolverConstraintPool.Count; | 527 | int frictionIndex = _tmpSolverConstraintPool.Count; |
528 | 528 | ||
529 | if (cp.Distance <= 0) | 529 | if (cp.Distance <= 0) |
530 | { | 530 | { |
531 | 531 | ||
532 | Vector3 pos1 = cp.PositionWorldOnA; | 532 | Vector3 pos1 = cp.PositionWorldOnA; |
533 | Vector3 pos2 = cp.PositionWorldOnB; | 533 | Vector3 pos2 = cp.PositionWorldOnB; |
534 | 534 | ||
535 | Vector3 rel_pos1 = pos1 - rb0.CenterOfMassPosition; | 535 | Vector3 rel_pos1 = pos1 - rb0.CenterOfMassPosition; |
536 | Vector3 rel_pos2 = pos2 - rb1.CenterOfMassPosition; | 536 | Vector3 rel_pos2 = pos2 - rb1.CenterOfMassPosition; |
537 | 537 | ||
538 | float relaxation = 1; | 538 | float relaxation = 1; |
539 | { | 539 | { |
540 | SolverConstraint solverConstraint = new SolverConstraint(); | 540 | SolverConstraint solverConstraint = new SolverConstraint(); |
541 | _tmpSolverConstraintPool.Add(solverConstraint); | 541 | _tmpSolverConstraintPool.Add(solverConstraint); |
542 | 542 | ||
543 | solverConstraint.SolverBodyIdA = solverBodyIdA; | 543 | solverConstraint.SolverBodyIdA = solverBodyIdA; |
544 | solverConstraint.SolverBodyIdB = solverBodyIdB; | 544 | solverConstraint.SolverBodyIdB = solverBodyIdB; |
545 | solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Contact; | 545 | solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Contact; |
546 | 546 | ||
547 | //can be optimized, the cross products are already calculated | 547 | //can be optimized, the cross products are already calculated |
548 | float denom0 = rb0.ComputeImpulseDenominator(pos1, cp.NormalWorldOnB); | 548 | float denom0 = rb0.ComputeImpulseDenominator(pos1, cp.NormalWorldOnB); |
549 | float denom1 = rb1.ComputeImpulseDenominator(pos2, cp.NormalWorldOnB); | 549 | float denom1 = rb1.ComputeImpulseDenominator(pos2, cp.NormalWorldOnB); |
550 | float denom = relaxation / (denom0 + denom1); | 550 | float denom = relaxation / (denom0 + denom1); |
551 | solverConstraint.JacDiagABInv = denom; | 551 | solverConstraint.JacDiagABInv = denom; |
552 | 552 | ||
553 | solverConstraint.ContactNormal = cp.NormalWorldOnB; | 553 | solverConstraint.ContactNormal = cp.NormalWorldOnB; |
554 | solverConstraint.RelPosACrossNormal = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); | 554 | solverConstraint.RelPosACrossNormal = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); |
555 | solverConstraint.RelPosBCrossNormal = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); | 555 | solverConstraint.RelPosBCrossNormal = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); |
556 | 556 | ||
557 | Vector3 vel1 = rb0.GetVelocityInLocalPoint(rel_pos1); | 557 | Vector3 vel1 = rb0.GetVelocityInLocalPoint(rel_pos1); |
558 | Vector3 vel2 = rb1.GetVelocityInLocalPoint(rel_pos2); | 558 | Vector3 vel2 = rb1.GetVelocityInLocalPoint(rel_pos2); |
559 | 559 | ||
560 | Vector3 vel = vel1 - vel2; | 560 | Vector3 vel = vel1 - vel2; |
561 | float rel_vel; | 561 | float rel_vel; |
562 | rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); | 562 | rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); |
563 | 563 | ||
564 | 564 | ||
565 | solverConstraint.Penetration = cp.Distance;//btScalar(infoGlobal.m_numIterations); | 565 | solverConstraint.Penetration = cp.Distance;//btScalar(infoGlobal.m_numIterations); |
566 | solverConstraint.Friction = cp.CombinedFriction; | 566 | solverConstraint.Friction = cp.CombinedFriction; |
567 | float rest = RestitutionCurve(rel_vel, cp.CombinedRestitution); | 567 | float rest = RestitutionCurve(rel_vel, cp.CombinedRestitution); |
568 | if (rest <= 0) | 568 | if (rest <= 0) |
569 | { | 569 | { |
570 | rest = 0; | 570 | rest = 0; |
571 | } | 571 | } |
572 | 572 | ||
573 | float penVel = -solverConstraint.Penetration / infoGlobal.TimeStep; | 573 | float penVel = -solverConstraint.Penetration / infoGlobal.TimeStep; |
574 | if (rest > penVel) | 574 | if (rest > penVel) |
575 | { | 575 | { |
576 | rest = 0; | 576 | rest = 0; |
577 | } | 577 | } |
578 | solverConstraint.Restitution = rest; | 578 | solverConstraint.Restitution = rest; |
579 | 579 | ||
580 | solverConstraint.Penetration *= -(infoGlobal.Erp / infoGlobal.TimeStep); | 580 | solverConstraint.Penetration *= -(infoGlobal.Erp / infoGlobal.TimeStep); |
581 | 581 | ||
582 | solverConstraint.AppliedImpulse = 0f; | 582 | solverConstraint.AppliedImpulse = 0f; |
583 | solverConstraint.AppliedVelocityImpulse = 0f; | 583 | solverConstraint.AppliedVelocityImpulse = 0f; |
584 | 584 | ||
585 | #warning Check to see if we need Vector3.Transform | 585 | #warning Check to see if we need Vector3.Transform |
586 | Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); | 586 | Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); |
587 | solverConstraint.AngularComponentA = Vector3.TransformNormal(torqueAxis0, rb0.InvInertiaTensorWorld); | 587 | solverConstraint.AngularComponentA = Vector3.TransformNormal(torqueAxis0, rb0.InvInertiaTensorWorld); |
588 | Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); | 588 | Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); |
589 | solverConstraint.AngularComponentB = Vector3.TransformNormal(torqueAxis1, rb1.InvInertiaTensorWorld); | 589 | solverConstraint.AngularComponentB = Vector3.TransformNormal(torqueAxis1, rb1.InvInertiaTensorWorld); |
590 | } | 590 | } |
591 | //create 2 '1d axis' constraints for 2 tangential friction directions | 591 | //create 2 '1d axis' constraints for 2 tangential friction directions |
592 | 592 | ||
593 | //re-calculate friction direction every frame, todo: check if this is really needed | 593 | //re-calculate friction direction every frame, todo: check if this is really needed |
594 | Vector3 frictionTangential0a = new Vector3(), | 594 | Vector3 frictionTangential0a = new Vector3(), |
595 | frictionTangential1b = new Vector3(); | 595 | frictionTangential1b = new Vector3(); |
596 | 596 | ||
597 | MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref frictionTangential0a, ref frictionTangential1b); | 597 | MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref frictionTangential0a, ref frictionTangential1b); |
598 | { | 598 | { |
599 | SolverConstraint solverConstraint = new SolverConstraint(); | 599 | SolverConstraint solverConstraint = new SolverConstraint(); |
600 | _tmpSolverFrictionConstraintPool.Add(solverConstraint); | 600 | _tmpSolverFrictionConstraintPool.Add(solverConstraint); |
601 | solverConstraint.ContactNormal = frictionTangential0a; | 601 | solverConstraint.ContactNormal = frictionTangential0a; |
602 | 602 | ||
603 | solverConstraint.SolverBodyIdA = solverBodyIdA; | 603 | solverConstraint.SolverBodyIdA = solverBodyIdA; |
604 | solverConstraint.SolverBodyIdB = solverBodyIdB; | 604 | solverConstraint.SolverBodyIdB = solverBodyIdB; |
605 | solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; | 605 | solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; |
606 | solverConstraint.FrictionIndex = frictionIndex; | 606 | solverConstraint.FrictionIndex = frictionIndex; |
607 | 607 | ||
608 | solverConstraint.Friction = cp.CombinedFriction; | 608 | solverConstraint.Friction = cp.CombinedFriction; |
609 | 609 | ||
610 | solverConstraint.AppliedImpulse = 0; | 610 | solverConstraint.AppliedImpulse = 0; |
611 | solverConstraint.AppliedVelocityImpulse = 0; | 611 | solverConstraint.AppliedVelocityImpulse = 0; |
612 | 612 | ||
613 | float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); | 613 | float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); |
614 | float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); | 614 | float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); |
615 | float denom = relaxation / (denom0 + denom1); | 615 | float denom = relaxation / (denom0 + denom1); |
616 | solverConstraint.JacDiagABInv = denom; | 616 | solverConstraint.JacDiagABInv = denom; |
617 | 617 | ||
618 | { | 618 | { |
619 | Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); | 619 | Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); |
620 | solverConstraint.RelPosACrossNormal = ftorqueAxis0; | 620 | solverConstraint.RelPosACrossNormal = ftorqueAxis0; |
621 | solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis0, rb0.InvInertiaTensorWorld); | 621 | solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis0, rb0.InvInertiaTensorWorld); |
622 | } | 622 | } |
623 | { | 623 | { |
624 | Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); | 624 | Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); |
625 | solverConstraint.RelPosBCrossNormal = ftorqueAxis0; | 625 | solverConstraint.RelPosBCrossNormal = ftorqueAxis0; |
626 | solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis0, rb1.InvInertiaTensorWorld); | 626 | solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis0, rb1.InvInertiaTensorWorld); |
627 | } | 627 | } |
628 | } | 628 | } |
629 | 629 | ||
630 | 630 | ||
631 | { | 631 | { |
632 | 632 | ||
633 | SolverConstraint solverConstraint = new SolverConstraint(); | 633 | SolverConstraint solverConstraint = new SolverConstraint(); |
634 | _tmpSolverFrictionConstraintPool.Add(solverConstraint); | 634 | _tmpSolverFrictionConstraintPool.Add(solverConstraint); |
635 | solverConstraint.ContactNormal = frictionTangential1b; | 635 | solverConstraint.ContactNormal = frictionTangential1b; |
636 | 636 | ||
637 | solverConstraint.SolverBodyIdA = solverBodyIdA; | 637 | solverConstraint.SolverBodyIdA = solverBodyIdA; |
638 | solverConstraint.SolverBodyIdB = solverBodyIdB; | 638 | solverConstraint.SolverBodyIdB = solverBodyIdB; |
639 | solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; | 639 | solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; |
640 | solverConstraint.FrictionIndex = frictionIndex; | 640 | solverConstraint.FrictionIndex = frictionIndex; |
641 | 641 | ||
642 | solverConstraint.Friction = cp.CombinedFriction; | 642 | solverConstraint.Friction = cp.CombinedFriction; |
643 | 643 | ||
644 | solverConstraint.AppliedImpulse = 0; | 644 | solverConstraint.AppliedImpulse = 0; |
645 | solverConstraint.AppliedVelocityImpulse = 0; | 645 | solverConstraint.AppliedVelocityImpulse = 0; |
646 | 646 | ||
647 | float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); | 647 | float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); |
648 | float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); | 648 | float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); |
649 | float denom = relaxation / (denom0 + denom1); | 649 | float denom = relaxation / (denom0 + denom1); |
650 | solverConstraint.JacDiagABInv = denom; | 650 | solverConstraint.JacDiagABInv = denom; |
651 | { | 651 | { |
652 | Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); | 652 | Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); |
653 | solverConstraint.RelPosACrossNormal = ftorqueAxis1; | 653 | solverConstraint.RelPosACrossNormal = ftorqueAxis1; |
654 | solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis1, rb0.InvInertiaTensorWorld); | 654 | solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis1, rb0.InvInertiaTensorWorld); |
655 | } | 655 | } |
656 | { | 656 | { |
657 | Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); | 657 | Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); |
658 | solverConstraint.RelPosBCrossNormal = ftorqueAxis1; | 658 | solverConstraint.RelPosBCrossNormal = ftorqueAxis1; |
659 | solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis1, rb1.InvInertiaTensorWorld); | 659 | solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis1, rb1.InvInertiaTensorWorld); |
660 | } | 660 | } |
661 | } | 661 | } |
662 | } | 662 | } |
663 | } | 663 | } |
664 | } | 664 | } |
665 | 665 | ||
666 | ContactSolverInfo info = infoGlobal; | 666 | ContactSolverInfo info = infoGlobal; |
667 | { | 667 | { |
668 | for (int j = 0; j < constraints.Count; j++) | 668 | for (int j = 0; j < constraints.Count; j++) |
669 | { | 669 | { |
670 | TypedConstraint constraint = constraints[j]; | 670 | TypedConstraint constraint = constraints[j]; |
671 | constraint.BuildJacobian(); | 671 | constraint.BuildJacobian(); |
672 | } | 672 | } |
673 | } | 673 | } |
674 | 674 | ||
675 | int numConstraintPool = _tmpSolverConstraintPool.Count; | 675 | int numConstraintPool = _tmpSolverConstraintPool.Count; |
676 | int numFrictionPool = _tmpSolverFrictionConstraintPool.Count; | 676 | int numFrictionPool = _tmpSolverFrictionConstraintPool.Count; |
677 | 677 | ||
678 | //todo: use stack allocator for such temporarily memory, same for solver bodies/constraints | 678 | //todo: use stack allocator for such temporarily memory, same for solver bodies/constraints |
679 | List<int> gOrderTmpConstraintPool = new List<int>(numConstraintPool); | 679 | List<int> gOrderTmpConstraintPool = new List<int>(numConstraintPool); |
680 | List<int> gOrderFrictionConstraintPool = new List<int>(numFrictionPool); | 680 | List<int> gOrderFrictionConstraintPool = new List<int>(numFrictionPool); |
681 | { | 681 | { |
682 | for (int i = 0; i < numConstraintPool; i++) | 682 | for (int i = 0; i < numConstraintPool; i++) |
683 | { | 683 | { |
684 | gOrderTmpConstraintPool.Add(i); | 684 | gOrderTmpConstraintPool.Add(i); |
685 | } | 685 | } |
686 | for (int i = 0; i < numFrictionPool; i++) | 686 | for (int i = 0; i < numFrictionPool; i++) |
687 | { | 687 | { |
688 | gOrderFrictionConstraintPool.Add(i); | 688 | gOrderFrictionConstraintPool.Add(i); |
689 | } | 689 | } |
690 | } | 690 | } |
691 | 691 | ||
692 | //should traverse the contacts random order... | 692 | //should traverse the contacts random order... |
693 | int iteration; | 693 | int iteration; |
694 | { | 694 | { |
695 | for (iteration = 0; iteration < info.IterationsCount; iteration++) | 695 | for (iteration = 0; iteration < info.IterationsCount; iteration++) |
696 | { | 696 | { |
697 | 697 | ||
698 | int j; | 698 | int j; |
699 | if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) | 699 | if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) |
700 | { | 700 | { |
701 | if ((iteration & 7) == 0) | 701 | if ((iteration & 7) == 0) |
702 | { | 702 | { |
703 | for (j = 0; j < numConstraintPool; ++j) | 703 | for (j = 0; j < numConstraintPool; ++j) |
704 | { | 704 | { |
705 | int tmp = gOrderTmpConstraintPool[j]; | 705 | int tmp = gOrderTmpConstraintPool[j]; |
706 | int swapi = RandInt2(j + 1); | 706 | int swapi = RandInt2(j + 1); |
707 | gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi]; | 707 | gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi]; |
708 | gOrderTmpConstraintPool[swapi] = tmp; | 708 | gOrderTmpConstraintPool[swapi] = tmp; |
709 | } | 709 | } |
710 | 710 | ||
711 | for (j = 0; j < numFrictionPool; ++j) | 711 | for (j = 0; j < numFrictionPool; ++j) |
712 | { | 712 | { |
713 | int tmp = gOrderFrictionConstraintPool[j]; | 713 | int tmp = gOrderFrictionConstraintPool[j]; |
714 | int swapi = RandInt2(j + 1); | 714 | int swapi = RandInt2(j + 1); |
715 | gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi]; | 715 | gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi]; |
716 | gOrderFrictionConstraintPool[swapi] = tmp; | 716 | gOrderFrictionConstraintPool[swapi] = tmp; |
717 | } | 717 | } |
718 | } | 718 | } |
719 | } | 719 | } |
720 | 720 | ||
721 | for (j = 0; j < constraints.Count; j++) | 721 | for (j = 0; j < constraints.Count; j++) |
722 | { | 722 | { |
723 | TypedConstraint constraint = constraints[j]; | 723 | TypedConstraint constraint = constraints[j]; |
724 | //todo: use solver bodies, so we don't need to copy from/to btRigidBody | 724 | //todo: use solver bodies, so we don't need to copy from/to btRigidBody |
725 | 725 | ||
726 | if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) | 726 | if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) |
727 | { | 727 | { |
728 | _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].WriteBackVelocity(); | 728 | _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].WriteBackVelocity(); |
729 | } | 729 | } |
730 | if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) | 730 | if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) |
731 | { | 731 | { |
732 | _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].WriteBackVelocity(); | 732 | _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].WriteBackVelocity(); |
733 | } | 733 | } |
734 | 734 | ||
735 | constraint.SolveConstraint(info.TimeStep); | 735 | constraint.SolveConstraint(info.TimeStep); |
736 | 736 | ||
737 | if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) | 737 | if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) |
738 | { | 738 | { |
739 | _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].ReadVelocity(); | 739 | _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].ReadVelocity(); |
740 | } | 740 | } |
741 | if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) | 741 | if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) |
742 | { | 742 | { |
743 | _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].ReadVelocity(); | 743 | _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].ReadVelocity(); |
744 | } | 744 | } |
745 | 745 | ||
746 | } | 746 | } |
747 | 747 | ||
748 | { | 748 | { |
749 | int numPoolConstraints = _tmpSolverConstraintPool.Count; | 749 | int numPoolConstraints = _tmpSolverConstraintPool.Count; |
750 | for (j = 0; j < numPoolConstraints; j++) | 750 | for (j = 0; j < numPoolConstraints; j++) |
751 | { | 751 | { |
752 | SolverConstraint solveManifold = _tmpSolverConstraintPool[gOrderTmpConstraintPool[j]]; | 752 | SolverConstraint solveManifold = _tmpSolverConstraintPool[gOrderTmpConstraintPool[j]]; |
753 | ResolveSingleCollisionCombinedCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], | 753 | ResolveSingleCollisionCombinedCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], |
754 | _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info); | 754 | _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info); |
755 | } | 755 | } |
756 | } | 756 | } |
757 | 757 | ||
758 | { | 758 | { |
759 | int numFrictionPoolConstraints = _tmpSolverFrictionConstraintPool.Count; | 759 | int numFrictionPoolConstraints = _tmpSolverFrictionConstraintPool.Count; |
760 | for (j = 0; j < numFrictionPoolConstraints; j++) | 760 | for (j = 0; j < numFrictionPoolConstraints; j++) |
761 | { | 761 | { |
762 | SolverConstraint solveManifold = _tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]]; | 762 | SolverConstraint solveManifold = _tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]]; |
763 | float appliedNormalImpulse = _tmpSolverConstraintPool[solveManifold.FrictionIndex].AppliedImpulse; | 763 | float appliedNormalImpulse = _tmpSolverConstraintPool[solveManifold.FrictionIndex].AppliedImpulse; |
764 | 764 | ||
765 | ResolveSingleFrictionCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], | 765 | ResolveSingleFrictionCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], |
766 | _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info, appliedNormalImpulse); | 766 | _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info, appliedNormalImpulse); |
767 | } | 767 | } |
768 | } | 768 | } |
769 | } | 769 | } |
770 | } | 770 | } |
771 | 771 | ||
772 | for (int i = 0; i < _tmpSolverBodyPool.Count; i++) | 772 | for (int i = 0; i < _tmpSolverBodyPool.Count; i++) |
773 | { | 773 | { |
774 | _tmpSolverBodyPool[i].WriteBackVelocity(); | 774 | _tmpSolverBodyPool[i].WriteBackVelocity(); |
775 | } | 775 | } |
776 | 776 | ||
777 | _tmpSolverBodyPool.Clear(); | 777 | _tmpSolverBodyPool.Clear(); |
778 | _tmpSolverConstraintPool.Clear(); | 778 | _tmpSolverConstraintPool.Clear(); |
779 | _tmpSolverFrictionConstraintPool.Clear(); | 779 | _tmpSolverFrictionConstraintPool.Clear(); |
780 | 780 | ||
781 | return 0; | 781 | return 0; |
782 | } | 782 | } |
783 | 783 | ||
784 | public virtual float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) | 784 | public virtual float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) |
785 | { | 785 | { |
786 | if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None) | 786 | if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None) |
787 | { | 787 | { |
788 | return SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer); | 788 | return SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer); |
789 | } | 789 | } |
790 | 790 | ||
791 | ContactSolverInfo info = infoGlobal; | 791 | ContactSolverInfo info = infoGlobal; |
792 | int totalPoints = 0; | 792 | int totalPoints = 0; |
793 | 793 | ||
794 | int numiter = infoGlobal.IterationsCount; | 794 | int numiter = infoGlobal.IterationsCount; |
795 | 795 | ||
796 | for (int j = 0; j < manifolds.Count; j++) | 796 | for (int j = 0; j < manifolds.Count; j++) |
797 | { | 797 | { |
798 | PersistentManifold manifold = manifolds[j]; | 798 | PersistentManifold manifold = manifolds[j]; |
799 | PrepareConstraints(manifold, info); | 799 | PrepareConstraints(manifold, info); |
800 | 800 | ||
801 | for (int p = 0; p < manifolds[j].ContactsCount; p++) | 801 | for (int p = 0; p < manifolds[j].ContactsCount; p++) |
802 | { | 802 | { |
803 | _order[totalPoints].ManifoldIndex = j; | 803 | _order[totalPoints].ManifoldIndex = j; |
804 | _order[totalPoints].PointIndex = p; | 804 | _order[totalPoints].PointIndex = p; |
805 | totalPoints++; | 805 | totalPoints++; |
806 | } | 806 | } |
807 | } | 807 | } |
808 | 808 | ||
809 | for (int j = 0; j < constraints.Count; j++) | 809 | for (int j = 0; j < constraints.Count; j++) |
810 | { | 810 | { |
811 | constraints[j].BuildJacobian(); | 811 | constraints[j].BuildJacobian(); |
812 | } | 812 | } |
813 | 813 | ||
814 | //should traverse the contacts random order... | 814 | //should traverse the contacts random order... |
815 | int iteration; | 815 | int iteration; |
816 | 816 | ||
817 | for (iteration = 0; iteration < numiter; iteration++) | 817 | for (iteration = 0; iteration < numiter; iteration++) |
818 | { | 818 | { |
819 | int j; | 819 | int j; |
820 | if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) | 820 | if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) |
821 | { | 821 | { |
822 | if ((iteration & 7) == 0) | 822 | if ((iteration & 7) == 0) |
823 | { | 823 | { |
824 | for (j = 0; j < totalPoints; ++j) | 824 | for (j = 0; j < totalPoints; ++j) |
825 | { | 825 | { |
826 | OrderIndex tmp = _order[j]; | 826 | OrderIndex tmp = _order[j]; |
827 | int swapi = RandInt2(j + 1); | 827 | int swapi = RandInt2(j + 1); |
828 | _order[j] = _order[swapi]; | 828 | _order[j] = _order[swapi]; |
829 | _order[swapi] = tmp; | 829 | _order[swapi] = tmp; |
830 | } | 830 | } |
831 | } | 831 | } |
832 | } | 832 | } |
833 | 833 | ||
834 | for (j = 0; j < constraints.Count; j++) | 834 | for (j = 0; j < constraints.Count; j++) |
835 | { | 835 | { |
836 | constraints[j].SolveConstraint(info.TimeStep); | 836 | constraints[j].SolveConstraint(info.TimeStep); |
837 | } | 837 | } |
838 | 838 | ||
839 | for (j = 0; j < totalPoints; j++) | 839 | for (j = 0; j < totalPoints; j++) |
840 | { | 840 | { |
841 | PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; | 841 | PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; |
842 | Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB, | 842 | Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB, |
843 | manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); | 843 | manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); |
844 | } | 844 | } |
845 | 845 | ||
846 | for (j = 0; j < totalPoints; j++) | 846 | for (j = 0; j < totalPoints; j++) |
847 | { | 847 | { |
848 | PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; | 848 | PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; |
849 | SolveFriction((RigidBody)manifold.BodyA, | 849 | SolveFriction((RigidBody)manifold.BodyA, |
850 | (RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); | 850 | (RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); |
851 | } | 851 | } |
852 | } | 852 | } |
853 | 853 | ||
854 | return 0; | 854 | return 0; |
855 | } | 855 | } |
856 | 856 | ||
857 | private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody) | 857 | private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody) |
858 | { | 858 | { |
859 | solverBody = new SolverBody(); | 859 | solverBody = new SolverBody(); |
860 | solverBody.AngularVelocity = rigidBody.AngularVelocity; | 860 | solverBody.AngularVelocity = rigidBody.AngularVelocity; |
861 | solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition; | 861 | solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition; |
862 | solverBody.Friction = rigidBody.Friction; | 862 | solverBody.Friction = rigidBody.Friction; |
863 | solverBody.InvMass = rigidBody.InverseMass; | 863 | solverBody.InvMass = rigidBody.InverseMass; |
864 | solverBody.LinearVelocity = rigidBody.LinearVelocity; | 864 | solverBody.LinearVelocity = rigidBody.LinearVelocity; |
865 | solverBody.OriginalBody = rigidBody; | 865 | solverBody.OriginalBody = rigidBody; |
866 | solverBody.AngularFactor = rigidBody.AngularFactor; | 866 | solverBody.AngularFactor = rigidBody.AngularFactor; |
867 | } | 867 | } |
868 | 868 | ||
869 | private long Rand2() | 869 | private long Rand2() |
870 | { | 870 | { |
871 | _seed2 = (1664525L * _seed2 + 1013904223L) & 0xffffffff; | 871 | _seed2 = (1664525L * _seed2 + 1013904223L) & 0xffffffff; |
872 | return _seed2; | 872 | return _seed2; |
873 | } | 873 | } |
874 | 874 | ||
875 | private int RandInt2(int n) | 875 | private int RandInt2(int n) |
876 | { | 876 | { |
877 | // seems good; xor-fold and modulus | 877 | // seems good; xor-fold and modulus |
878 | long un = n; | 878 | long un = n; |
879 | long r = Rand2(); | 879 | long r = Rand2(); |
880 | 880 | ||
881 | // note: probably more aggressive than it needs to be -- might be | 881 | // note: probably more aggressive than it needs to be -- might be |
882 | // able to get away without one or two of the innermost branches. | 882 | // able to get away without one or two of the innermost branches. |
883 | if (un <= 0x00010000L) | 883 | if (un <= 0x00010000L) |
884 | { | 884 | { |
885 | r ^= (r >> 16); | 885 | r ^= (r >> 16); |
886 | if (un <= 0x00000100L) | 886 | if (un <= 0x00000100L) |
887 | { | 887 | { |
888 | r ^= (r >> 8); | 888 | r ^= (r >> 8); |
889 | if (un <= 0x00000010L) | 889 | if (un <= 0x00000010L) |
890 | { | 890 | { |
891 | r ^= (r >> 4); | 891 | r ^= (r >> 4); |
892 | if (un <= 0x00000004L) | 892 | if (un <= 0x00000004L) |
893 | { | 893 | { |
894 | r ^= (r >> 2); | 894 | r ^= (r >> 2); |
895 | if (un <= 0x00000002L) | 895 | if (un <= 0x00000002L) |
896 | { | 896 | { |
897 | r ^= (r >> 1); | 897 | r ^= (r >> 1); |
898 | } | 898 | } |
899 | } | 899 | } |
900 | } | 900 | } |
901 | } | 901 | } |
902 | } | 902 | } |
903 | return (int)(r % un); | 903 | return (int)(r % un); |
904 | } | 904 | } |
905 | 905 | ||
906 | protected struct OrderIndex | 906 | protected struct OrderIndex |
907 | { | 907 | { |
908 | private int _manifoldIndex; | 908 | private int _manifoldIndex; |
909 | private int _pointIndex; | 909 | private int _pointIndex; |
910 | 910 | ||
911 | public int ManifoldIndex { get { return _manifoldIndex; } set { _manifoldIndex = value; } } | 911 | public int ManifoldIndex { get { return _manifoldIndex; } set { _manifoldIndex = value; } } |
912 | public int PointIndex { get { return _pointIndex; } set { _pointIndex = value; } } | 912 | public int PointIndex { get { return _pointIndex; } set { _pointIndex = value; } } |
913 | } | 913 | } |
914 | } | 914 | } |
915 | } | 915 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs index 6ced783..9d4060d 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs | |||
@@ -1,188 +1,188 @@ | |||
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.Dynamics | 27 | namespace XnaDevRu.BulletX.Dynamics |
28 | { | 28 | { |
29 | /// <summary> | 29 | /// <summary> |
30 | /// constraint class used for lateral tyre friction | 30 | /// constraint class used for lateral tyre friction |
31 | /// </summary> | 31 | /// </summary> |
32 | public class Solve2LinearConstraint | 32 | public class Solve2LinearConstraint |
33 | { | 33 | { |
34 | private float _tau; | 34 | private float _tau; |
35 | private float _damping; | 35 | private float _damping; |
36 | 36 | ||
37 | public Solve2LinearConstraint(float tau, float damping) | 37 | public Solve2LinearConstraint(float tau, float damping) |
38 | { | 38 | { |
39 | _tau = tau; | 39 | _tau = tau; |
40 | _damping = damping; | 40 | _damping = damping; |
41 | } | 41 | } |
42 | 42 | ||
43 | // solve unilateral constraint (equality, direct method) | 43 | // solve unilateral constraint (equality, direct method) |
44 | public void ResolveUnilateralPairConstraint( | 44 | public void ResolveUnilateralPairConstraint( |
45 | RigidBody body1, RigidBody body2, | 45 | RigidBody body1, RigidBody body2, |
46 | Matrix world2A, | 46 | Matrix world2A, |
47 | Matrix world2B, | 47 | Matrix world2B, |
48 | Vector3 invInertiaADiag, | 48 | Vector3 invInertiaADiag, |
49 | float invMassA, | 49 | float invMassA, |
50 | Vector3 linvelA, Vector3 angvelA, | 50 | Vector3 linvelA, Vector3 angvelA, |
51 | Vector3 rel_posA1, | 51 | Vector3 rel_posA1, |
52 | Vector3 invInertiaBDiag, | 52 | Vector3 invInertiaBDiag, |
53 | float invMassB, | 53 | float invMassB, |
54 | Vector3 linvelB, Vector3 angvelB, | 54 | Vector3 linvelB, Vector3 angvelB, |
55 | Vector3 rel_posA2, | 55 | Vector3 rel_posA2, |
56 | float depthA, Vector3 normalA, | 56 | float depthA, Vector3 normalA, |
57 | Vector3 rel_posB1, Vector3 rel_posB2, | 57 | Vector3 rel_posB1, Vector3 rel_posB2, |
58 | float depthB, Vector3 normalB, | 58 | float depthB, Vector3 normalB, |
59 | out float imp0, out float imp1) | 59 | out float imp0, out float imp1) |
60 | { | 60 | { |
61 | imp0 = 0; | 61 | imp0 = 0; |
62 | imp1 = 0; | 62 | imp1 = 0; |
63 | 63 | ||
64 | float len = Math.Abs(normalA.Length()) - 1f; | 64 | float len = Math.Abs(normalA.Length()) - 1f; |
65 | if (Math.Abs(len) >= float.Epsilon) | 65 | if (Math.Abs(len) >= float.Epsilon) |
66 | return; | 66 | return; |
67 | 67 | ||
68 | BulletDebug.Assert(len < float.Epsilon); | 68 | BulletDebug.Assert(len < float.Epsilon); |
69 | 69 | ||
70 | //this jacobian entry could be re-used for all iterations | 70 | //this jacobian entry could be re-used for all iterations |
71 | JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, | 71 | JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, |
72 | invInertiaBDiag, invMassB); | 72 | invInertiaBDiag, invMassB); |
73 | JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, | 73 | JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, |
74 | invInertiaBDiag, invMassB); | 74 | invInertiaBDiag, invMassB); |
75 | 75 | ||
76 | float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); | 76 | float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); |
77 | float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1)); | 77 | float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1)); |
78 | 78 | ||
79 | // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv | 79 | // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv |
80 | float massTerm = 1f / (invMassA + invMassB); | 80 | float massTerm = 1f / (invMassA + invMassB); |
81 | 81 | ||
82 | // calculate rhs (or error) terms | 82 | // calculate rhs (or error) terms |
83 | float dv0 = depthA * _tau * massTerm - vel0 * _damping; | 83 | float dv0 = depthA * _tau * massTerm - vel0 * _damping; |
84 | float dv1 = depthB * _tau * massTerm - vel1 * _damping; | 84 | float dv1 = depthB * _tau * massTerm - vel1 * _damping; |
85 | 85 | ||
86 | float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); | 86 | float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); |
87 | float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); | 87 | float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); |
88 | 88 | ||
89 | imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; | 89 | imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; |
90 | imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; | 90 | imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; |
91 | } | 91 | } |
92 | 92 | ||
93 | // solving 2x2 lcp problem (inequality, direct solution ) | 93 | // solving 2x2 lcp problem (inequality, direct solution ) |
94 | public void ResolveBilateralPairConstraint( | 94 | public void ResolveBilateralPairConstraint( |
95 | RigidBody body1, RigidBody body2, | 95 | RigidBody body1, RigidBody body2, |
96 | Matrix world2A, Matrix world2B, | 96 | Matrix world2A, Matrix world2B, |
97 | Vector3 invInertiaADiag, | 97 | Vector3 invInertiaADiag, |
98 | float invMassA, | 98 | float invMassA, |
99 | Vector3 linvelA, Vector3 angvelA, | 99 | Vector3 linvelA, Vector3 angvelA, |
100 | Vector3 rel_posA1, | 100 | Vector3 rel_posA1, |
101 | Vector3 invInertiaBDiag, | 101 | Vector3 invInertiaBDiag, |
102 | float invMassB, | 102 | float invMassB, |
103 | Vector3 linvelB, Vector3 angvelB, | 103 | Vector3 linvelB, Vector3 angvelB, |
104 | Vector3 rel_posA2, | 104 | Vector3 rel_posA2, |
105 | float depthA, Vector3 normalA, | 105 | float depthA, Vector3 normalA, |
106 | Vector3 rel_posB1, Vector3 rel_posB2, | 106 | Vector3 rel_posB1, Vector3 rel_posB2, |
107 | float depthB, Vector3 normalB, | 107 | float depthB, Vector3 normalB, |
108 | out float imp0, out float imp1) | 108 | out float imp0, out float imp1) |
109 | { | 109 | { |
110 | imp0 = 0f; | 110 | imp0 = 0f; |
111 | imp1 = 0f; | 111 | imp1 = 0f; |
112 | 112 | ||
113 | float len = Math.Abs(normalA.Length()) - 1f; | 113 | float len = Math.Abs(normalA.Length()) - 1f; |
114 | if (Math.Abs(len) >= float.Epsilon) | 114 | if (Math.Abs(len) >= float.Epsilon) |
115 | return; | 115 | return; |
116 | 116 | ||
117 | BulletDebug.Assert(len < float.Epsilon); | 117 | BulletDebug.Assert(len < float.Epsilon); |
118 | 118 | ||
119 | JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, | 119 | JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, |
120 | invInertiaBDiag, invMassB); | 120 | invInertiaBDiag, invMassB); |
121 | JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, | 121 | JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, |
122 | invInertiaBDiag, invMassB); | 122 | invInertiaBDiag, invMassB); |
123 | 123 | ||
124 | float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); | 124 | float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); |
125 | float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1)); | 125 | float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1)); |
126 | 126 | ||
127 | // calculate rhs (or error) terms | 127 | // calculate rhs (or error) terms |
128 | float dv0 = depthA * _tau - vel0 * _damping; | 128 | float dv0 = depthA * _tau - vel0 * _damping; |
129 | float dv1 = depthB * _tau - vel1 * _damping; | 129 | float dv1 = depthB * _tau - vel1 * _damping; |
130 | 130 | ||
131 | float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); | 131 | float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); |
132 | float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); | 132 | float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); |
133 | 133 | ||
134 | imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; | 134 | imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; |
135 | imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; | 135 | imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; |
136 | 136 | ||
137 | if (imp0 > 0.0f) | 137 | if (imp0 > 0.0f) |
138 | { | 138 | { |
139 | if (imp1 <= 0.0f) | 139 | if (imp1 <= 0.0f) |
140 | { | 140 | { |
141 | imp1 = 0f; | 141 | imp1 = 0f; |
142 | 142 | ||
143 | // now imp0>0 imp1<0 | 143 | // now imp0>0 imp1<0 |
144 | imp0 = dv0 / jacA.Diagonal; | 144 | imp0 = dv0 / jacA.Diagonal; |
145 | if (imp0 < 0.0f) | 145 | if (imp0 < 0.0f) |
146 | imp0 = 0f; | 146 | imp0 = 0f; |
147 | } | 147 | } |
148 | } | 148 | } |
149 | else | 149 | else |
150 | { | 150 | { |
151 | imp0 = 0f; | 151 | imp0 = 0f; |
152 | 152 | ||
153 | imp1 = dv1 / jacB.Diagonal; | 153 | imp1 = dv1 / jacB.Diagonal; |
154 | if (imp1 <= 0.0f) | 154 | if (imp1 <= 0.0f) |
155 | { | 155 | { |
156 | imp1 = 0f; | 156 | imp1 = 0f; |
157 | // now imp0>0 imp1<0 | 157 | // now imp0>0 imp1<0 |
158 | imp0 = dv0 / jacA.Diagonal; | 158 | imp0 = dv0 / jacA.Diagonal; |
159 | if (imp0 > 0.0f) | 159 | if (imp0 > 0.0f) |
160 | { | 160 | { |
161 | } | 161 | } |
162 | else | 162 | else |
163 | { | 163 | { |
164 | imp0 = 0f; | 164 | imp0 = 0f; |
165 | } | 165 | } |
166 | } | 166 | } |
167 | } | 167 | } |
168 | } | 168 | } |
169 | 169 | ||
170 | //public void ResolveAngularConstraint( | 170 | //public void ResolveAngularConstraint( |
171 | // Matrix invInertiaAWS, | 171 | // Matrix invInertiaAWS, |
172 | // float invMassA, | 172 | // float invMassA, |
173 | // Vector3 linvelA, Vector3 angvelA, | 173 | // Vector3 linvelA, Vector3 angvelA, |
174 | // Vector3 rel_posA1, | 174 | // Vector3 rel_posA1, |
175 | // Matrix invInertiaBWS, | 175 | // Matrix invInertiaBWS, |
176 | // float invMassB, | 176 | // float invMassB, |
177 | // Vector3 linvelB, Vector3 angvelB, | 177 | // Vector3 linvelB, Vector3 angvelB, |
178 | // Vector3 rel_posA2, | 178 | // Vector3 rel_posA2, |
179 | // float depthA, Vector3 normalA, | 179 | // float depthA, Vector3 normalA, |
180 | // Vector3 rel_posB1, Vector3 rel_posB2, | 180 | // Vector3 rel_posB1, Vector3 rel_posB2, |
181 | // float depthB, Vector3 normalB, | 181 | // float depthB, Vector3 normalB, |
182 | // out float imp0, out float imp1) | 182 | // out float imp0, out float imp1) |
183 | //{ | 183 | //{ |
184 | // imp0 = 0; | 184 | // imp0 = 0; |
185 | // imp1 = 0; | 185 | // imp1 = 0; |
186 | //} | 186 | //} |
187 | } | 187 | } |
188 | } | 188 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs index 09b73cc..6996272 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs | |||
@@ -1,78 +1,78 @@ | |||
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 | using System.Runtime.InteropServices; | 26 | using System.Runtime.InteropServices; |
27 | 27 | ||
28 | namespace XnaDevRu.BulletX.Dynamics | 28 | namespace XnaDevRu.BulletX.Dynamics |
29 | { | 29 | { |
30 | public class SolverBody | 30 | public class SolverBody |
31 | { | 31 | { |
32 | private Vector3 _centerOfMassPosition = new Vector3(); | 32 | private Vector3 _centerOfMassPosition = new Vector3(); |
33 | private Vector3 _linearVelocity = new Vector3(); | 33 | private Vector3 _linearVelocity = new Vector3(); |
34 | private Vector3 _angularVelocity = new Vector3(); | 34 | private Vector3 _angularVelocity = new Vector3(); |
35 | private RigidBody _originalBody = null; | 35 | private RigidBody _originalBody = null; |
36 | private float _invMass; | 36 | private float _invMass; |
37 | private float _friction; | 37 | private float _friction; |
38 | private float _angularFactor; | 38 | private float _angularFactor; |
39 | 39 | ||
40 | public Vector3 CenterOfMassPosition { get { return _centerOfMassPosition; } set { _centerOfMassPosition = value; } } | 40 | public Vector3 CenterOfMassPosition { get { return _centerOfMassPosition; } set { _centerOfMassPosition = value; } } |
41 | public Vector3 LinearVelocity { get { return _linearVelocity; } set { _linearVelocity = value; } } | 41 | public Vector3 LinearVelocity { get { return _linearVelocity; } set { _linearVelocity = value; } } |
42 | public Vector3 AngularVelocity { get { return _angularVelocity; } set { _angularVelocity = value; } } | 42 | public Vector3 AngularVelocity { get { return _angularVelocity; } set { _angularVelocity = value; } } |
43 | public RigidBody OriginalBody { get { return _originalBody; } set { _originalBody = value; } } | 43 | public RigidBody OriginalBody { get { return _originalBody; } set { _originalBody = value; } } |
44 | public float InvMass { get { return _invMass; } set { _invMass = value; } } | 44 | public float InvMass { get { return _invMass; } set { _invMass = value; } } |
45 | public float Friction { get { return _friction; } set { _friction = value; } } | 45 | public float Friction { get { return _friction; } set { _friction = value; } } |
46 | public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } } | 46 | public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } } |
47 | 47 | ||
48 | public void GetVelocityInLocalPoint(Vector3 relPos, out Vector3 velocity) | 48 | public void GetVelocityInLocalPoint(Vector3 relPos, out Vector3 velocity) |
49 | { | 49 | { |
50 | velocity = _linearVelocity + Vector3.Cross(_angularVelocity, relPos); | 50 | velocity = _linearVelocity + Vector3.Cross(_angularVelocity, relPos); |
51 | } | 51 | } |
52 | 52 | ||
53 | public void WriteBackVelocity() | 53 | public void WriteBackVelocity() |
54 | { | 54 | { |
55 | if (_invMass != 0) | 55 | if (_invMass != 0) |
56 | { | 56 | { |
57 | _originalBody.LinearVelocity = _linearVelocity; | 57 | _originalBody.LinearVelocity = _linearVelocity; |
58 | _originalBody.AngularVelocity = _angularVelocity; | 58 | _originalBody.AngularVelocity = _angularVelocity; |
59 | } | 59 | } |
60 | } | 60 | } |
61 | 61 | ||
62 | public void ReadVelocity() | 62 | public void ReadVelocity() |
63 | { | 63 | { |
64 | if (_invMass != 0) | 64 | if (_invMass != 0) |
65 | { | 65 | { |
66 | _linearVelocity = _originalBody.LinearVelocity; | 66 | _linearVelocity = _originalBody.LinearVelocity; |
67 | _angularVelocity = _originalBody.AngularVelocity; | 67 | _angularVelocity = _originalBody.AngularVelocity; |
68 | } | 68 | } |
69 | } | 69 | } |
70 | 70 | ||
71 | //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position | 71 | //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position |
72 | internal void ApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude) | 72 | internal void ApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude) |
73 | { | 73 | { |
74 | _linearVelocity += linearComponent * impulseMagnitude; | 74 | _linearVelocity += linearComponent * impulseMagnitude; |
75 | _angularVelocity += angularComponent * impulseMagnitude * _angularFactor; | 75 | _angularVelocity += angularComponent * impulseMagnitude * _angularFactor; |
76 | } | 76 | } |
77 | } | 77 | } |
78 | } | 78 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs index 3e3a64a..4fd0361 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs | |||
@@ -1,77 +1,77 @@ | |||
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 | using System.Runtime.InteropServices; | 26 | using System.Runtime.InteropServices; |
27 | 27 | ||
28 | namespace XnaDevRu.BulletX.Dynamics | 28 | namespace XnaDevRu.BulletX.Dynamics |
29 | { | 29 | { |
30 | //1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. | 30 | //1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. |
31 | public class SolverConstraint | 31 | public class SolverConstraint |
32 | { | 32 | { |
33 | private Vector3 _relpos1CrossNormal = new Vector3(); | 33 | private Vector3 _relpos1CrossNormal = new Vector3(); |
34 | private Vector3 _relpos2CrossNormal = new Vector3(); | 34 | private Vector3 _relpos2CrossNormal = new Vector3(); |
35 | private Vector3 _contactNormal = new Vector3(); | 35 | private Vector3 _contactNormal = new Vector3(); |
36 | private Vector3 _angularComponentA = new Vector3(); | 36 | private Vector3 _angularComponentA = new Vector3(); |
37 | private Vector3 _angularComponentB = new Vector3(); | 37 | private Vector3 _angularComponentB = new Vector3(); |
38 | 38 | ||
39 | private float _appliedVelocityImpulse; | 39 | private float _appliedVelocityImpulse; |
40 | private int _solverBodyIdA; | 40 | private int _solverBodyIdA; |
41 | int _solverBodyIdB; | 41 | int _solverBodyIdB; |
42 | private float _friction; | 42 | private float _friction; |
43 | private float _restitution; | 43 | private float _restitution; |
44 | private float _jacDiagABInv; | 44 | private float _jacDiagABInv; |
45 | private float _penetration; | 45 | private float _penetration; |
46 | private float _appliedImpulse; | 46 | private float _appliedImpulse; |
47 | 47 | ||
48 | private SolverConstraintType _constraintType = SolverConstraintType.Contact; | 48 | private SolverConstraintType _constraintType = SolverConstraintType.Contact; |
49 | private int _frictionIndex; | 49 | private int _frictionIndex; |
50 | private int[] _unusedPadding = new int[2]; | 50 | private int[] _unusedPadding = new int[2]; |
51 | 51 | ||
52 | public Vector3 RelPosACrossNormal { get { return _relpos1CrossNormal; } set { _relpos1CrossNormal = value; } } | 52 | public Vector3 RelPosACrossNormal { get { return _relpos1CrossNormal; } set { _relpos1CrossNormal = value; } } |
53 | public Vector3 RelPosBCrossNormal { get { return _relpos2CrossNormal; } set { _relpos2CrossNormal = value; } } | 53 | public Vector3 RelPosBCrossNormal { get { return _relpos2CrossNormal; } set { _relpos2CrossNormal = value; } } |
54 | public Vector3 ContactNormal { get { return _contactNormal; } set { _contactNormal = value; } } | 54 | public Vector3 ContactNormal { get { return _contactNormal; } set { _contactNormal = value; } } |
55 | public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } } | 55 | public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } } |
56 | public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } } | 56 | public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } } |
57 | 57 | ||
58 | public float AppliedVelocityImpulse { get { return _appliedVelocityImpulse; } set { _appliedVelocityImpulse = value; } } | 58 | public float AppliedVelocityImpulse { get { return _appliedVelocityImpulse; } set { _appliedVelocityImpulse = value; } } |
59 | public int SolverBodyIdA { get { return _solverBodyIdA; } set { _solverBodyIdA = value; } } | 59 | public int SolverBodyIdA { get { return _solverBodyIdA; } set { _solverBodyIdA = value; } } |
60 | public int SolverBodyIdB { get { return _solverBodyIdB; } set { _solverBodyIdB = value; } } | 60 | public int SolverBodyIdB { get { return _solverBodyIdB; } set { _solverBodyIdB = value; } } |
61 | public float Friction { get { return _friction; } set { _friction = value; } } | 61 | public float Friction { get { return _friction; } set { _friction = value; } } |
62 | public float Restitution { get { return _restitution; } set { _restitution = value; } } | 62 | public float Restitution { get { return _restitution; } set { _restitution = value; } } |
63 | public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } } | 63 | public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } } |
64 | public float Penetration { get { return _penetration; } set { _penetration = value; } } | 64 | public float Penetration { get { return _penetration; } set { _penetration = value; } } |
65 | public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } } | 65 | public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } } |
66 | 66 | ||
67 | public SolverConstraintType ConstraintType { get { return _constraintType; } set { _constraintType = value; } } | 67 | public SolverConstraintType ConstraintType { get { return _constraintType; } set { _constraintType = value; } } |
68 | public int FrictionIndex { get { return _frictionIndex; } set { _frictionIndex = value; } } | 68 | public int FrictionIndex { get { return _frictionIndex; } set { _frictionIndex = value; } } |
69 | public int[] UnusedPadding { get { return _unusedPadding; } set { _unusedPadding = value; } } | 69 | public int[] UnusedPadding { get { return _unusedPadding; } set { _unusedPadding = value; } } |
70 | 70 | ||
71 | public enum SolverConstraintType | 71 | public enum SolverConstraintType |
72 | { | 72 | { |
73 | Contact = 0, | 73 | Contact = 0, |
74 | Friction, | 74 | Friction, |
75 | } | 75 | } |
76 | } | 76 | } |
77 | } | 77 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs index abd10f1..8131463 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs | |||
@@ -1,88 +1,88 @@ | |||
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.Dynamics | 27 | namespace XnaDevRu.BulletX.Dynamics |
28 | { | 28 | { |
29 | public abstract class TypedConstraint | 29 | public abstract class TypedConstraint |
30 | { | 30 | { |
31 | private static RigidBody _fixed = new RigidBody(0, null, null, new Vector3(), 0, 0, 0.5f, 0); | 31 | private static RigidBody _fixed = new RigidBody(0, null, null, new Vector3(), 0, 0, 0.5f, 0); |
32 | private int _userConstraintType; | 32 | private int _userConstraintType; |
33 | private int _userConstraintId; | 33 | private int _userConstraintId; |
34 | 34 | ||
35 | private RigidBody _rbA; | 35 | private RigidBody _rbA; |
36 | private RigidBody _rbB; | 36 | private RigidBody _rbB; |
37 | private float _appliedImpulse; | 37 | private float _appliedImpulse; |
38 | 38 | ||
39 | public TypedConstraint() | 39 | public TypedConstraint() |
40 | : this(_fixed, _fixed) { } | 40 | : this(_fixed, _fixed) { } |
41 | 41 | ||
42 | public TypedConstraint(RigidBody rbA) | 42 | public TypedConstraint(RigidBody rbA) |
43 | : this(rbA, _fixed) { } | 43 | : this(rbA, _fixed) { } |
44 | 44 | ||
45 | public TypedConstraint(RigidBody rbA, RigidBody rbB) | 45 | public TypedConstraint(RigidBody rbA, RigidBody rbB) |
46 | { | 46 | { |
47 | _userConstraintType = -1; | 47 | _userConstraintType = -1; |
48 | _userConstraintId = -1; | 48 | _userConstraintId = -1; |
49 | _rbA = rbA; | 49 | _rbA = rbA; |
50 | _rbB = rbB; | 50 | _rbB = rbB; |
51 | _appliedImpulse = 0; | 51 | _appliedImpulse = 0; |
52 | 52 | ||
53 | _fixed.SetMassProps(0, new Vector3()); | 53 | _fixed.SetMassProps(0, new Vector3()); |
54 | } | 54 | } |
55 | 55 | ||
56 | public virtual RigidBody RigidBodyA { get { return _rbA; } protected set { _rbA = value; } } | 56 | public virtual RigidBody RigidBodyA { get { return _rbA; } protected set { _rbA = value; } } |
57 | public virtual RigidBody RigidBodyB { get { return _rbB; } protected set { _rbB = value; } } | 57 | public virtual RigidBody RigidBodyB { get { return _rbB; } protected set { _rbB = value; } } |
58 | 58 | ||
59 | public float AppliedImpulse { get { return _appliedImpulse; } protected set { _appliedImpulse = value; } } | 59 | public float AppliedImpulse { get { return _appliedImpulse; } protected set { _appliedImpulse = value; } } |
60 | public int UserConstraintId { get { return _userConstraintId; } set { _userConstraintId = value; } } | 60 | public int UserConstraintId { get { return _userConstraintId; } set { _userConstraintId = value; } } |
61 | public int UserConstraintType { get { return _userConstraintType; } set { _userConstraintType = value; } } | 61 | public int UserConstraintType { get { return _userConstraintType; } set { _userConstraintType = value; } } |
62 | 62 | ||
63 | public abstract void BuildJacobian(); | 63 | public abstract void BuildJacobian(); |
64 | public abstract void SolveConstraint(float timeStep); | 64 | public abstract void SolveConstraint(float timeStep); |
65 | 65 | ||
66 | public static int SortConstraintOnIslandPredicate(TypedConstraint left, TypedConstraint right) | 66 | public static int SortConstraintOnIslandPredicate(TypedConstraint left, TypedConstraint right) |
67 | { | 67 | { |
68 | int rightIslandID, leftIslandID; | 68 | int rightIslandID, leftIslandID; |
69 | rightIslandID = GetConstraintIslandId(right); | 69 | rightIslandID = GetConstraintIslandId(right); |
70 | leftIslandID = GetConstraintIslandId(left); | 70 | leftIslandID = GetConstraintIslandId(left); |
71 | if (leftIslandID < rightIslandID) | 71 | if (leftIslandID < rightIslandID) |
72 | return -1; | 72 | return -1; |
73 | else | 73 | else |
74 | return 1; | 74 | return 1; |
75 | return 0; | 75 | return 0; |
76 | } | 76 | } |
77 | 77 | ||
78 | internal static int GetConstraintIslandId(TypedConstraint lhs) | 78 | internal static int GetConstraintIslandId(TypedConstraint lhs) |
79 | { | 79 | { |
80 | int islandId; | 80 | int islandId; |
81 | 81 | ||
82 | CollisionObject colObjA = lhs.RigidBodyA; | 82 | CollisionObject colObjA = lhs.RigidBodyA; |
83 | CollisionObject colObjB = lhs.RigidBodyB; | 83 | CollisionObject colObjB = lhs.RigidBodyB; |
84 | islandId = colObjA.IslandTag >= 0 ? colObjA.IslandTag : colObjB.IslandTag; | 84 | islandId = colObjA.IslandTag >= 0 ? colObjA.IslandTag : colObjB.IslandTag; |
85 | return islandId; | 85 | return islandId; |
86 | } | 86 | } |
87 | } | 87 | } |
88 | } | 88 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs index acceb98..3702c2a 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs | |||
@@ -1,790 +1,790 @@ | |||
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.Dynamics | 27 | namespace XnaDevRu.BulletX.Dynamics |
28 | { | 28 | { |
29 | /// <summary> | 29 | /// <summary> |
30 | /// DiscreteDynamicsWorld provides discrete rigid body simulation | 30 | /// DiscreteDynamicsWorld provides discrete rigid body simulation |
31 | /// those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController | 31 | /// those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController |
32 | /// </summary> | 32 | /// </summary> |
33 | public class DiscreteDynamicsWorld : DynamicsWorld | 33 | public class DiscreteDynamicsWorld : DynamicsWorld |
34 | { | 34 | { |
35 | private static bool _reportMe = true; | 35 | private static bool _reportMe = true; |
36 | 36 | ||
37 | private IConstraintSolver _constraintSolver; | 37 | private IConstraintSolver _constraintSolver; |
38 | private SimulationIslandManager _islandManager; | 38 | private SimulationIslandManager _islandManager; |
39 | private List<TypedConstraint> _constraints = new List<TypedConstraint>(); | 39 | private List<TypedConstraint> _constraints = new List<TypedConstraint>(); |
40 | private IDebugDraw _debugDrawer; | 40 | private IDebugDraw _debugDrawer; |
41 | private ContactSolverInfo _solverInfo = new ContactSolverInfo(); | 41 | private ContactSolverInfo _solverInfo = new ContactSolverInfo(); |
42 | private Vector3 _gravity; | 42 | private Vector3 _gravity; |
43 | //for variable timesteps | 43 | //for variable timesteps |
44 | private float _localTime; | 44 | private float _localTime; |
45 | //for variable timesteps | 45 | //for variable timesteps |
46 | private bool _ownsIslandManager; | 46 | private bool _ownsIslandManager; |
47 | private bool _ownsConstraintSolver; | 47 | private bool _ownsConstraintSolver; |
48 | private List<RaycastVehicle> _vehicles = new List<RaycastVehicle>(); | 48 | private List<RaycastVehicle> _vehicles = new List<RaycastVehicle>(); |
49 | private int _profileTimings; | 49 | private int _profileTimings; |
50 | 50 | ||
51 | public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) | 51 | public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) |
52 | : this(dispatcher, pairCache, null) { } | 52 | : this(dispatcher, pairCache, null) { } |
53 | 53 | ||
54 | //this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those | 54 | //this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those |
55 | public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver) | 55 | public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver) |
56 | : base(dispatcher, pairCache) | 56 | : base(dispatcher, pairCache) |
57 | { | 57 | { |
58 | _constraintSolver = constraintSolver != null ? constraintSolver : new SequentialImpulseConstraintSolver(); | 58 | _constraintSolver = constraintSolver != null ? constraintSolver : new SequentialImpulseConstraintSolver(); |
59 | _debugDrawer = null; | 59 | _debugDrawer = null; |
60 | _gravity = new Vector3(0, -10, 0); | 60 | _gravity = new Vector3(0, -10, 0); |
61 | _localTime = 1f / 60f; | 61 | _localTime = 1f / 60f; |
62 | _profileTimings = 0; | 62 | _profileTimings = 0; |
63 | _islandManager = new SimulationIslandManager(); | 63 | _islandManager = new SimulationIslandManager(); |
64 | _ownsIslandManager = true; | 64 | _ownsIslandManager = true; |
65 | _ownsConstraintSolver = constraintSolver == null; | 65 | _ownsConstraintSolver = constraintSolver == null; |
66 | } | 66 | } |
67 | 67 | ||
68 | public ContactSolverInfo SolverInfo { get { return _solverInfo; } } | 68 | public ContactSolverInfo SolverInfo { get { return _solverInfo; } } |
69 | public SimulationIslandManager SimulationIslandManager { get { return _islandManager; } } | 69 | public SimulationIslandManager SimulationIslandManager { get { return _islandManager; } } |
70 | public CollisionWorld CollisionWorld { get { return this; } } | 70 | public CollisionWorld CollisionWorld { get { return this; } } |
71 | 71 | ||
72 | public override IDebugDraw DebugDrawer | 72 | public override IDebugDraw DebugDrawer |
73 | { | 73 | { |
74 | get | 74 | get |
75 | { | 75 | { |
76 | return _debugDrawer; | 76 | return _debugDrawer; |
77 | } | 77 | } |
78 | set | 78 | set |
79 | { | 79 | { |
80 | _debugDrawer = value; | 80 | _debugDrawer = value; |
81 | } | 81 | } |
82 | } | 82 | } |
83 | 83 | ||
84 | public override Vector3 Gravity | 84 | public override Vector3 Gravity |
85 | { | 85 | { |
86 | set | 86 | set |
87 | { | 87 | { |
88 | _gravity = value; | 88 | _gravity = value; |
89 | for (int i = 0; i < CollisionObjects.Count; i++) | 89 | for (int i = 0; i < CollisionObjects.Count; i++) |
90 | { | 90 | { |
91 | CollisionObject colObj = CollisionObjects[i]; | 91 | CollisionObject colObj = CollisionObjects[i]; |
92 | RigidBody body = RigidBody.Upcast(colObj); | 92 | RigidBody body = RigidBody.Upcast(colObj); |
93 | if (body != null) | 93 | if (body != null) |
94 | { | 94 | { |
95 | body.Gravity = value; | 95 | body.Gravity = value; |
96 | } | 96 | } |
97 | } | 97 | } |
98 | } | 98 | } |
99 | } | 99 | } |
100 | 100 | ||
101 | public override IConstraintSolver ConstraintSolver | 101 | public override IConstraintSolver ConstraintSolver |
102 | { | 102 | { |
103 | set | 103 | set |
104 | { | 104 | { |
105 | _ownsConstraintSolver = false; | 105 | _ownsConstraintSolver = false; |
106 | _constraintSolver = value; | 106 | _constraintSolver = value; |
107 | } | 107 | } |
108 | } | 108 | } |
109 | 109 | ||
110 | public override int ConstraintsCount | 110 | public override int ConstraintsCount |
111 | { | 111 | { |
112 | get | 112 | get |
113 | { | 113 | { |
114 | return _constraints.Count; | 114 | return _constraints.Count; |
115 | } | 115 | } |
116 | } | 116 | } |
117 | 117 | ||
118 | //if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's | 118 | //if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's |
119 | public override void StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep) | 119 | public override void StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep) |
120 | { | 120 | { |
121 | int numSimulationSubSteps = 0; | 121 | int numSimulationSubSteps = 0; |
122 | 122 | ||
123 | if (maxSubSteps != 0) | 123 | if (maxSubSteps != 0) |
124 | { | 124 | { |
125 | //fixed timestep with interpolation | 125 | //fixed timestep with interpolation |
126 | _localTime += timeStep; | 126 | _localTime += timeStep; |
127 | if (_localTime >= fixedTimeStep) | 127 | if (_localTime >= fixedTimeStep) |
128 | { | 128 | { |
129 | numSimulationSubSteps = (int)(_localTime / fixedTimeStep); | 129 | numSimulationSubSteps = (int)(_localTime / fixedTimeStep); |
130 | _localTime -= numSimulationSubSteps * fixedTimeStep; | 130 | _localTime -= numSimulationSubSteps * fixedTimeStep; |
131 | } | 131 | } |
132 | } | 132 | } |
133 | else | 133 | else |
134 | { | 134 | { |
135 | //variable timestep | 135 | //variable timestep |
136 | fixedTimeStep = timeStep; | 136 | fixedTimeStep = timeStep; |
137 | _localTime = timeStep; | 137 | _localTime = timeStep; |
138 | if (Math.Abs(timeStep) < float.Epsilon) | 138 | if (Math.Abs(timeStep) < float.Epsilon) |
139 | { | 139 | { |
140 | numSimulationSubSteps = 0; | 140 | numSimulationSubSteps = 0; |
141 | maxSubSteps = 0; | 141 | maxSubSteps = 0; |
142 | } | 142 | } |
143 | else | 143 | else |
144 | { | 144 | { |
145 | numSimulationSubSteps = 1; | 145 | numSimulationSubSteps = 1; |
146 | maxSubSteps = 1; | 146 | maxSubSteps = 1; |
147 | } | 147 | } |
148 | } | 148 | } |
149 | 149 | ||
150 | //process some debugging flags | 150 | //process some debugging flags |
151 | if (DebugDrawer != null) | 151 | if (DebugDrawer != null) |
152 | { | 152 | { |
153 | RigidBody.DisableDeactivation = (DebugDrawer.DebugMode & DebugDrawModes.NoDeactivation) != 0; | 153 | RigidBody.DisableDeactivation = (DebugDrawer.DebugMode & DebugDrawModes.NoDeactivation) != 0; |
154 | } | 154 | } |
155 | if (numSimulationSubSteps != 0) | 155 | if (numSimulationSubSteps != 0) |
156 | { | 156 | { |
157 | 157 | ||
158 | SaveKinematicState(fixedTimeStep); | 158 | SaveKinematicState(fixedTimeStep); |
159 | 159 | ||
160 | //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt | 160 | //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt |
161 | int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; | 161 | int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; |
162 | 162 | ||
163 | for (int i = 0; i < clampedSimulationSteps; i++) | 163 | for (int i = 0; i < clampedSimulationSteps; i++) |
164 | { | 164 | { |
165 | InternalSingleStepSimulation(fixedTimeStep); | 165 | InternalSingleStepSimulation(fixedTimeStep); |
166 | SynchronizeMotionStates(); | 166 | SynchronizeMotionStates(); |
167 | } | 167 | } |
168 | 168 | ||
169 | } | 169 | } |
170 | 170 | ||
171 | SynchronizeMotionStates(); | 171 | SynchronizeMotionStates(); |
172 | 172 | ||
173 | //return numSimulationSubSteps; | 173 | //return numSimulationSubSteps; |
174 | } | 174 | } |
175 | 175 | ||
176 | public void StepSimulation(float timeStep, int maxSubSteps) | 176 | public void StepSimulation(float timeStep, int maxSubSteps) |
177 | { | 177 | { |
178 | StepSimulation(timeStep, maxSubSteps, 1f / 60f); | 178 | StepSimulation(timeStep, maxSubSteps, 1f / 60f); |
179 | } | 179 | } |
180 | 180 | ||
181 | public override void UpdateAabbs() | 181 | public override void UpdateAabbs() |
182 | { | 182 | { |
183 | Vector3 colorvec = new Vector3(1, 0, 0); | 183 | Vector3 colorvec = new Vector3(1, 0, 0); |
184 | for (int i = 0; i < CollisionObjects.Count; i++) | 184 | for (int i = 0; i < CollisionObjects.Count; i++) |
185 | { | 185 | { |
186 | CollisionObject colObj = CollisionObjects[i]; | 186 | CollisionObject colObj = CollisionObjects[i]; |
187 | RigidBody body = RigidBody.Upcast(colObj); | 187 | RigidBody body = RigidBody.Upcast(colObj); |
188 | 188 | ||
189 | if (body != null) | 189 | if (body != null) |
190 | { | 190 | { |
191 | // if (body->IsActive() && (!body->IsStatic())) | 191 | // if (body->IsActive() && (!body->IsStatic())) |
192 | { | 192 | { |
193 | Vector3 minAabb, maxAabb; | 193 | Vector3 minAabb, maxAabb; |
194 | colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb); | 194 | colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb); |
195 | OverlappingPairCache bp = BroadphasePairCache; | 195 | OverlappingPairCache bp = BroadphasePairCache; |
196 | 196 | ||
197 | //moving objects should be moderately sized, probably something wrong if not | 197 | //moving objects should be moderately sized, probably something wrong if not |
198 | if (colObj.IsStaticObject || ((maxAabb - minAabb).LengthSquared() < 1e12f)) | 198 | if (colObj.IsStaticObject || ((maxAabb - minAabb).LengthSquared() < 1e12f)) |
199 | { | 199 | { |
200 | bp.SetAabb(body.Broadphase, minAabb, maxAabb); | 200 | bp.SetAabb(body.Broadphase, minAabb, maxAabb); |
201 | } | 201 | } |
202 | else | 202 | else |
203 | { | 203 | { |
204 | //something went wrong, investigate | 204 | //something went wrong, investigate |
205 | //this assert is unwanted in 3D modelers (danger of loosing work) | 205 | //this assert is unwanted in 3D modelers (danger of loosing work) |
206 | BulletDebug.Assert(false); | 206 | BulletDebug.Assert(false); |
207 | body.ActivationState = ActivationState.DisableSimulation; | 207 | body.ActivationState = ActivationState.DisableSimulation; |
208 | 208 | ||
209 | if (_reportMe) | 209 | if (_reportMe) |
210 | { | 210 | { |
211 | _reportMe = false; | 211 | _reportMe = false; |
212 | Console.WriteLine("Overflow in AABB, object removed from simulation \n"); | 212 | Console.WriteLine("Overflow in AABB, object removed from simulation \n"); |
213 | Console.WriteLine("If you can reproduce this, please email bugs@continuousphysics.com\n"); | 213 | Console.WriteLine("If you can reproduce this, please email bugs@continuousphysics.com\n"); |
214 | Console.WriteLine("Please include above information, your Platform, version of OS.\n"); | 214 | Console.WriteLine("Please include above information, your Platform, version of OS.\n"); |
215 | Console.WriteLine("Thanks.\n"); | 215 | Console.WriteLine("Thanks.\n"); |
216 | } | 216 | } |
217 | } | 217 | } |
218 | if (_debugDrawer != null && (_debugDrawer.DebugMode & DebugDrawModes.DrawAabb) != 0) | 218 | if (_debugDrawer != null && (_debugDrawer.DebugMode & DebugDrawModes.DrawAabb) != 0) |
219 | DrawAabb(_debugDrawer, minAabb, maxAabb, colorvec); | 219 | DrawAabb(_debugDrawer, minAabb, maxAabb, colorvec); |
220 | } | 220 | } |
221 | } | 221 | } |
222 | } | 222 | } |
223 | } | 223 | } |
224 | 224 | ||
225 | public override void AddConstraint(TypedConstraint constraint) | 225 | public override void AddConstraint(TypedConstraint constraint) |
226 | { | 226 | { |
227 | _constraints.Add(constraint); | 227 | _constraints.Add(constraint); |
228 | } | 228 | } |
229 | 229 | ||
230 | public override void RemoveConstraint(TypedConstraint constraint) | 230 | public override void RemoveConstraint(TypedConstraint constraint) |
231 | { | 231 | { |
232 | _constraints.Remove(constraint); | 232 | _constraints.Remove(constraint); |
233 | } | 233 | } |
234 | 234 | ||
235 | public void AddVehicle(RaycastVehicle vehicle) | 235 | public void AddVehicle(RaycastVehicle vehicle) |
236 | { | 236 | { |
237 | _vehicles.Add(vehicle); | 237 | _vehicles.Add(vehicle); |
238 | } | 238 | } |
239 | 239 | ||
240 | public void RemoveVehicle(RaycastVehicle vehicle) | 240 | public void RemoveVehicle(RaycastVehicle vehicle) |
241 | { | 241 | { |
242 | _vehicles.Remove(vehicle); | 242 | _vehicles.Remove(vehicle); |
243 | } | 243 | } |
244 | 244 | ||
245 | public override void AddRigidBody(RigidBody body) | 245 | public override void AddRigidBody(RigidBody body) |
246 | { | 246 | { |
247 | if (!body.IsStaticOrKinematicObject) | 247 | if (!body.IsStaticOrKinematicObject) |
248 | { | 248 | { |
249 | body.Gravity = _gravity; | 249 | body.Gravity = _gravity; |
250 | } | 250 | } |
251 | 251 | ||
252 | if (body.CollisionShape != null) | 252 | if (body.CollisionShape != null) |
253 | { | 253 | { |
254 | bool isDynamic = !(body.IsStaticObject || body.IsKinematicObject); | 254 | bool isDynamic = !(body.IsStaticObject || body.IsKinematicObject); |
255 | BroadphaseProxy.CollisionFilterGroups collisionFilterGroup = isDynamic ? BroadphaseProxy.CollisionFilterGroups.Default : BroadphaseProxy.CollisionFilterGroups.Static; | 255 | BroadphaseProxy.CollisionFilterGroups collisionFilterGroup = isDynamic ? BroadphaseProxy.CollisionFilterGroups.Default : BroadphaseProxy.CollisionFilterGroups.Static; |
256 | BroadphaseProxy.CollisionFilterGroups collisionFilterMask = isDynamic ? BroadphaseProxy.CollisionFilterGroups.All : (BroadphaseProxy.CollisionFilterGroups.All ^ BroadphaseProxy.CollisionFilterGroups.Static); | 256 | BroadphaseProxy.CollisionFilterGroups collisionFilterMask = isDynamic ? BroadphaseProxy.CollisionFilterGroups.All : (BroadphaseProxy.CollisionFilterGroups.All ^ BroadphaseProxy.CollisionFilterGroups.Static); |
257 | 257 | ||
258 | AddCollisionObject(body, collisionFilterGroup, collisionFilterMask); | 258 | AddCollisionObject(body, collisionFilterGroup, collisionFilterMask); |
259 | } | 259 | } |
260 | } | 260 | } |
261 | 261 | ||
262 | public override void RemoveRigidBody(RigidBody body) | 262 | public override void RemoveRigidBody(RigidBody body) |
263 | { | 263 | { |
264 | RemoveCollisionObject(body); | 264 | RemoveCollisionObject(body); |
265 | } | 265 | } |
266 | 266 | ||
267 | public void DebugDrawObject(Matrix worldTransform, CollisionShape shape, Vector3 color) | 267 | public void DebugDrawObject(Matrix worldTransform, CollisionShape shape, Vector3 color) |
268 | { | 268 | { |
269 | if (shape.ShapeType == BroadphaseNativeTypes.Compound) | 269 | if (shape.ShapeType == BroadphaseNativeTypes.Compound) |
270 | { | 270 | { |
271 | CompoundShape compoundShape = shape as CompoundShape; | 271 | CompoundShape compoundShape = shape as CompoundShape; |
272 | for (int i = compoundShape.ChildShapeCount - 1; i >= 0; i--) | 272 | for (int i = compoundShape.ChildShapeCount - 1; i >= 0; i--) |
273 | { | 273 | { |
274 | Matrix childTrans = compoundShape.GetChildTransform(i); | 274 | Matrix childTrans = compoundShape.GetChildTransform(i); |
275 | CollisionShape colShape = compoundShape.GetChildShape(i); | 275 | CollisionShape colShape = compoundShape.GetChildShape(i); |
276 | DebugDrawObject(worldTransform * childTrans, colShape, color); | 276 | DebugDrawObject(worldTransform * childTrans, colShape, color); |
277 | } | 277 | } |
278 | 278 | ||
279 | } | 279 | } |
280 | else | 280 | else |
281 | { | 281 | { |
282 | switch (shape.ShapeType) | 282 | switch (shape.ShapeType) |
283 | { | 283 | { |
284 | 284 | ||
285 | case BroadphaseNativeTypes.Sphere: | 285 | case BroadphaseNativeTypes.Sphere: |
286 | { | 286 | { |
287 | SphereShape sphereShape = shape as SphereShape; | 287 | SphereShape sphereShape = shape as SphereShape; |
288 | float radius = sphereShape.Margin;//radius doesn't include the margin, so draw with margin | 288 | float radius = sphereShape.Margin;//radius doesn't include the margin, so draw with margin |
289 | Vector3 start = worldTransform.Translation; | 289 | Vector3 start = worldTransform.Translation; |
290 | DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(radius, 0, 0), worldTransform), color); | 290 | DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(radius, 0, 0), worldTransform), color); |
291 | DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, radius, 0), worldTransform), color); | 291 | DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, radius, 0), worldTransform), color); |
292 | DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, 0, radius), worldTransform), color); | 292 | DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, 0, radius), worldTransform), color); |
293 | //drawSphere | 293 | //drawSphere |
294 | break; | 294 | break; |
295 | } | 295 | } |
296 | case BroadphaseNativeTypes.MultiSphere: | 296 | case BroadphaseNativeTypes.MultiSphere: |
297 | case BroadphaseNativeTypes.Cone: | 297 | case BroadphaseNativeTypes.Cone: |
298 | { | 298 | { |
299 | ConeShape coneShape = shape as ConeShape; | 299 | ConeShape coneShape = shape as ConeShape; |
300 | float radius = coneShape.Radius;//+coneShape->getMargin(); | 300 | float radius = coneShape.Radius;//+coneShape->getMargin(); |
301 | float height = coneShape.Height;//+coneShape->getMargin(); | 301 | float height = coneShape.Height;//+coneShape->getMargin(); |
302 | Vector3 start = worldTransform.Translation; | 302 | Vector3 start = worldTransform.Translation; |
303 | DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(radius, 0f, -0.5f * height), worldTransform), color); | 303 | DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(radius, 0f, -0.5f * height), worldTransform), color); |
304 | DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(-radius, 0f, -0.5f * height), worldTransform), color); | 304 | DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(-radius, 0f, -0.5f * height), worldTransform), color); |
305 | DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, radius, -0.5f * height), worldTransform), color); | 305 | DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, radius, -0.5f * height), worldTransform), color); |
306 | DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, -radius, -0.5f * height), worldTransform), color); | 306 | DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, -radius, -0.5f * height), worldTransform), color); |
307 | break; | 307 | break; |
308 | } | 308 | } |
309 | case BroadphaseNativeTypes.Cylinder: | 309 | case BroadphaseNativeTypes.Cylinder: |
310 | { | 310 | { |
311 | CylinderShape cylinder = shape as CylinderShape; | 311 | CylinderShape cylinder = shape as CylinderShape; |
312 | int upAxis = cylinder.UpAxis; | 312 | int upAxis = cylinder.UpAxis; |
313 | float radius = cylinder.Radius; | 313 | float radius = cylinder.Radius; |
314 | float halfHeight = MathHelper.GetElement(cylinder.HalfExtents, upAxis); | 314 | float halfHeight = MathHelper.GetElement(cylinder.HalfExtents, upAxis); |
315 | Vector3 start = worldTransform.Translation; | 315 | Vector3 start = worldTransform.Translation; |
316 | Vector3 offsetHeight = new Vector3(); | 316 | Vector3 offsetHeight = new Vector3(); |
317 | MathHelper.SetElement(ref offsetHeight, upAxis, halfHeight); | 317 | MathHelper.SetElement(ref offsetHeight, upAxis, halfHeight); |
318 | Vector3 offsetRadius = new Vector3(); | 318 | Vector3 offsetRadius = new Vector3(); |
319 | MathHelper.SetElement(ref offsetRadius, (upAxis + 1) % 3, radius); | 319 | MathHelper.SetElement(ref offsetRadius, (upAxis + 1) % 3, radius); |
320 | DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight + offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight + offsetRadius, worldTransform), color); | 320 | DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight + offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight + offsetRadius, worldTransform), color); |
321 | DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight - offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color); | 321 | DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight - offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color); |
322 | break; | 322 | break; |
323 | } | 323 | } |
324 | default: | 324 | default: |
325 | { | 325 | { |
326 | if (shape.ShapeType == BroadphaseNativeTypes.TriangleMesh) | 326 | if (shape.ShapeType == BroadphaseNativeTypes.TriangleMesh) |
327 | { | 327 | { |
328 | TriangleMeshShape concaveMesh = shape as TriangleMeshShape; | 328 | TriangleMeshShape concaveMesh = shape as TriangleMeshShape; |
329 | //btVector3 aabbMax(1e30f,1e30f,1e30f); | 329 | //btVector3 aabbMax(1e30f,1e30f,1e30f); |
330 | //btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f); | 330 | //btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f); |
331 | 331 | ||
332 | //todo pass camera, for some culling | 332 | //todo pass camera, for some culling |
333 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | 333 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); |
334 | Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); | 334 | Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); |
335 | 335 | ||
336 | DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color); | 336 | DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color); |
337 | concaveMesh.ProcessAllTriangles(drawCallback, aabbMin, aabbMax); | 337 | concaveMesh.ProcessAllTriangles(drawCallback, aabbMin, aabbMax); |
338 | } | 338 | } |
339 | 339 | ||
340 | if (shape.ShapeType == BroadphaseNativeTypes.ConvexTriangleMesh) | 340 | if (shape.ShapeType == BroadphaseNativeTypes.ConvexTriangleMesh) |
341 | { | 341 | { |
342 | ConvexTriangleMeshShape convexMesh = shape as ConvexTriangleMeshShape; | 342 | ConvexTriangleMeshShape convexMesh = shape as ConvexTriangleMeshShape; |
343 | //todo: pass camera for some culling | 343 | //todo: pass camera for some culling |
344 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | 344 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); |
345 | Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); | 345 | Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); |
346 | //DebugDrawcallback drawCallback; | 346 | //DebugDrawcallback drawCallback; |
347 | DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color); | 347 | DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color); |
348 | convexMesh.getStridingMesh().InternalProcessAllTriangles(drawCallback, aabbMin, aabbMax); | 348 | convexMesh.getStridingMesh().InternalProcessAllTriangles(drawCallback, aabbMin, aabbMax); |
349 | } | 349 | } |
350 | 350 | ||
351 | // for polyhedral shapes | 351 | // for polyhedral shapes |
352 | if (shape.IsPolyhedral) | 352 | if (shape.IsPolyhedral) |
353 | { | 353 | { |
354 | PolyhedralConvexShape polyshape = shape as PolyhedralConvexShape; | 354 | PolyhedralConvexShape polyshape = shape as PolyhedralConvexShape; |
355 | 355 | ||
356 | for (int i = 0; i < polyshape.EdgeCount; i++) | 356 | for (int i = 0; i < polyshape.EdgeCount; i++) |
357 | { | 357 | { |
358 | Vector3 a, b; | 358 | Vector3 a, b; |
359 | polyshape.GetEdge(i, out a, out b); | 359 | polyshape.GetEdge(i, out a, out b); |
360 | a = Vector3.TransformNormal(a, worldTransform); | 360 | a = Vector3.TransformNormal(a, worldTransform); |
361 | b = Vector3.TransformNormal(b, worldTransform); | 361 | b = Vector3.TransformNormal(b, worldTransform); |
362 | DebugDrawer.DrawLine(a, b, color); | 362 | DebugDrawer.DrawLine(a, b, color); |
363 | } | 363 | } |
364 | } | 364 | } |
365 | break; | 365 | break; |
366 | } | 366 | } |
367 | } | 367 | } |
368 | } | 368 | } |
369 | } | 369 | } |
370 | 370 | ||
371 | public override TypedConstraint GetConstraint(int index) | 371 | public override TypedConstraint GetConstraint(int index) |
372 | { | 372 | { |
373 | return _constraints[index]; | 373 | return _constraints[index]; |
374 | } | 374 | } |
375 | 375 | ||
376 | public static void DrawAabb(IDebugDraw debugDrawer, Vector3 from, Vector3 to, Vector3 color) | 376 | public static void DrawAabb(IDebugDraw debugDrawer, Vector3 from, Vector3 to, Vector3 color) |
377 | { | 377 | { |
378 | Vector3 halfExtents = (to - from) * 0.5f; | 378 | Vector3 halfExtents = (to - from) * 0.5f; |
379 | Vector3 center = (to + from) * 0.5f; | 379 | Vector3 center = (to + from) * 0.5f; |
380 | 380 | ||
381 | Vector3 edgecoord = new Vector3(1f, 1f, 1f), pa, pb; | 381 | Vector3 edgecoord = new Vector3(1f, 1f, 1f), pa, pb; |
382 | for (int i = 0; i < 4; i++) | 382 | for (int i = 0; i < 4; i++) |
383 | { | 383 | { |
384 | for (int j = 0; j < 3; j++) | 384 | for (int j = 0; j < 3; j++) |
385 | { | 385 | { |
386 | pa = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y, | 386 | pa = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y, |
387 | edgecoord.Z * halfExtents.Z); | 387 | edgecoord.Z * halfExtents.Z); |
388 | pa += center; | 388 | pa += center; |
389 | 389 | ||
390 | int othercoord = j % 3; | 390 | int othercoord = j % 3; |
391 | MathHelper.SetElement(ref edgecoord, othercoord, MathHelper.GetElement(edgecoord, othercoord) * -1f); | 391 | MathHelper.SetElement(ref edgecoord, othercoord, MathHelper.GetElement(edgecoord, othercoord) * -1f); |
392 | pb = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y, | 392 | pb = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y, |
393 | edgecoord.Z * halfExtents.Z); | 393 | edgecoord.Z * halfExtents.Z); |
394 | pb += center; | 394 | pb += center; |
395 | 395 | ||
396 | debugDrawer.DrawLine(pa, pb, color); | 396 | debugDrawer.DrawLine(pa, pb, color); |
397 | } | 397 | } |
398 | edgecoord = new Vector3(-1f, -1f, -1f); | 398 | edgecoord = new Vector3(-1f, -1f, -1f); |
399 | if (i < 3) | 399 | if (i < 3) |
400 | MathHelper.SetElement(ref edgecoord, i, MathHelper.GetElement(edgecoord, i) * -1f); | 400 | MathHelper.SetElement(ref edgecoord, i, MathHelper.GetElement(edgecoord, i) * -1f); |
401 | } | 401 | } |
402 | } | 402 | } |
403 | 403 | ||
404 | protected void PredictUnconstraintMotion(float timeStep) | 404 | protected void PredictUnconstraintMotion(float timeStep) |
405 | { | 405 | { |
406 | for (int i = 0; i < CollisionObjects.Count; i++) | 406 | for (int i = 0; i < CollisionObjects.Count; i++) |
407 | { | 407 | { |
408 | CollisionObject colObj = CollisionObjects[i]; | 408 | CollisionObject colObj = CollisionObjects[i]; |
409 | RigidBody body = RigidBody.Upcast(colObj); | 409 | RigidBody body = RigidBody.Upcast(colObj); |
410 | if (body != null) | 410 | if (body != null) |
411 | { | 411 | { |
412 | if (!body.IsStaticOrKinematicObject) | 412 | if (!body.IsStaticOrKinematicObject) |
413 | { | 413 | { |
414 | if (body.IsActive) | 414 | if (body.IsActive) |
415 | { | 415 | { |
416 | body.ApplyForces(timeStep); | 416 | body.ApplyForces(timeStep); |
417 | body.IntegrateVelocities(timeStep); | 417 | body.IntegrateVelocities(timeStep); |
418 | Matrix temp = body.InterpolationWorldTransform; | 418 | Matrix temp = body.InterpolationWorldTransform; |
419 | body.PredictIntegratedTransform(timeStep, ref temp); | 419 | body.PredictIntegratedTransform(timeStep, ref temp); |
420 | body.InterpolationWorldTransform = temp; | 420 | body.InterpolationWorldTransform = temp; |
421 | } | 421 | } |
422 | } | 422 | } |
423 | } | 423 | } |
424 | } | 424 | } |
425 | } | 425 | } |
426 | 426 | ||
427 | protected void IntegrateTransforms(float timeStep) | 427 | protected void IntegrateTransforms(float timeStep) |
428 | { | 428 | { |
429 | Matrix predictedTrans = new Matrix(); | 429 | Matrix predictedTrans = new Matrix(); |
430 | for (int i = 0; i < CollisionObjects.Count; i++) | 430 | for (int i = 0; i < CollisionObjects.Count; i++) |
431 | { | 431 | { |
432 | CollisionObject colObj = CollisionObjects[i]; | 432 | CollisionObject colObj = CollisionObjects[i]; |
433 | RigidBody body = RigidBody.Upcast(colObj); | 433 | RigidBody body = RigidBody.Upcast(colObj); |
434 | if (body != null) | 434 | if (body != null) |
435 | { | 435 | { |
436 | if (body.IsActive && (!body.IsStaticOrKinematicObject)) | 436 | if (body.IsActive && (!body.IsStaticOrKinematicObject)) |
437 | { | 437 | { |
438 | body.PredictIntegratedTransform(timeStep, ref predictedTrans); | 438 | body.PredictIntegratedTransform(timeStep, ref predictedTrans); |
439 | body.ProceedToTransform(predictedTrans); | 439 | body.ProceedToTransform(predictedTrans); |
440 | } | 440 | } |
441 | } | 441 | } |
442 | } | 442 | } |
443 | } | 443 | } |
444 | 444 | ||
445 | protected void CalculateSimulationIslands() | 445 | protected void CalculateSimulationIslands() |
446 | { | 446 | { |
447 | SimulationIslandManager.UpdateActivationState(this, Dispatcher); | 447 | SimulationIslandManager.UpdateActivationState(this, Dispatcher); |
448 | 448 | ||
449 | for (int i = 0; i < _constraints.Count; i++) | 449 | for (int i = 0; i < _constraints.Count; i++) |
450 | { | 450 | { |
451 | TypedConstraint constraint = _constraints[i]; | 451 | TypedConstraint constraint = _constraints[i]; |
452 | 452 | ||
453 | RigidBody colObj0 = constraint.RigidBodyA; | 453 | RigidBody colObj0 = constraint.RigidBodyA; |
454 | RigidBody colObj1 = constraint.RigidBodyB; | 454 | RigidBody colObj1 = constraint.RigidBodyB; |
455 | 455 | ||
456 | if (((colObj0 != null) && (colObj0.MergesSimulationIslands)) && | 456 | if (((colObj0 != null) && (colObj0.MergesSimulationIslands)) && |
457 | ((colObj1 != null) && (colObj1.MergesSimulationIslands))) | 457 | ((colObj1 != null) && (colObj1.MergesSimulationIslands))) |
458 | { | 458 | { |
459 | if (colObj0.IsActive || colObj1.IsActive) | 459 | if (colObj0.IsActive || colObj1.IsActive) |
460 | { | 460 | { |
461 | 461 | ||
462 | SimulationIslandManager.UnionFind.Unite((colObj0).IslandTag, | 462 | SimulationIslandManager.UnionFind.Unite((colObj0).IslandTag, |
463 | (colObj1).IslandTag); | 463 | (colObj1).IslandTag); |
464 | } | 464 | } |
465 | } | 465 | } |
466 | } | 466 | } |
467 | 467 | ||
468 | //Store the island id in each body | 468 | //Store the island id in each body |
469 | SimulationIslandManager.StoreIslandActivationState(this); | 469 | SimulationIslandManager.StoreIslandActivationState(this); |
470 | } | 470 | } |
471 | 471 | ||
472 | //protected void SolveNonContactConstraints(ContactSolverInfo solverInfo) | 472 | //protected void SolveNonContactConstraints(ContactSolverInfo solverInfo) |
473 | //{ | 473 | //{ |
474 | // //constraint preparation: building jacobians | 474 | // //constraint preparation: building jacobians |
475 | // for (int i = 0; i < _constraints.Count; i++) | 475 | // for (int i = 0; i < _constraints.Count; i++) |
476 | // { | 476 | // { |
477 | // TypedConstraint constraint = _constraints[i]; | 477 | // TypedConstraint constraint = _constraints[i]; |
478 | // constraint.BuildJacobian(); | 478 | // constraint.BuildJacobian(); |
479 | // } | 479 | // } |
480 | 480 | ||
481 | // //solve the regular non-contact constraints (point 2 point, hinge, generic d6) | 481 | // //solve the regular non-contact constraints (point 2 point, hinge, generic d6) |
482 | // for (int g = 0; g < solverInfo.IterationsCount; g++) | 482 | // for (int g = 0; g < solverInfo.IterationsCount; g++) |
483 | // { | 483 | // { |
484 | // for (int i = 0; i < _constraints.Count; i++) | 484 | // for (int i = 0; i < _constraints.Count; i++) |
485 | // { | 485 | // { |
486 | // TypedConstraint constraint = _constraints[i]; | 486 | // TypedConstraint constraint = _constraints[i]; |
487 | // constraint.SolveConstraint(solverInfo.TimeStep); | 487 | // constraint.SolveConstraint(solverInfo.TimeStep); |
488 | // } | 488 | // } |
489 | // } | 489 | // } |
490 | //} | 490 | //} |
491 | 491 | ||
492 | //protected void SolveContactConstraints(ContactSolverInfo solverInfo) | 492 | //protected void SolveContactConstraints(ContactSolverInfo solverInfo) |
493 | //{ | 493 | //{ |
494 | // InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, _debugDrawer); | 494 | // InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, _debugDrawer); |
495 | 495 | ||
496 | // // solve all the contact points and contact friction | 496 | // // solve all the contact points and contact friction |
497 | // _islandManager.BuildAndProcessIslands(Dispatcher, CollisionObjects, solverCallback); | 497 | // _islandManager.BuildAndProcessIslands(Dispatcher, CollisionObjects, solverCallback); |
498 | //} | 498 | //} |
499 | 499 | ||
500 | protected void SolveConstraints(ContactSolverInfo solverInfo) | 500 | protected void SolveConstraints(ContactSolverInfo solverInfo) |
501 | { | 501 | { |
502 | //sorted version of all btTypedConstraint, based on islandId | 502 | //sorted version of all btTypedConstraint, based on islandId |
503 | List<TypedConstraint> sortedConstraints = new List<TypedConstraint>(ConstraintsCount); | 503 | List<TypedConstraint> sortedConstraints = new List<TypedConstraint>(ConstraintsCount); |
504 | 504 | ||
505 | for (int i = 0; i < ConstraintsCount; i++) | 505 | for (int i = 0; i < ConstraintsCount; i++) |
506 | { | 506 | { |
507 | sortedConstraints.Add(_constraints[i]); | 507 | sortedConstraints.Add(_constraints[i]); |
508 | } | 508 | } |
509 | 509 | ||
510 | sortedConstraints.Sort(new Comparison<TypedConstraint>(TypedConstraint.SortConstraintOnIslandPredicate)); | 510 | sortedConstraints.Sort(new Comparison<TypedConstraint>(TypedConstraint.SortConstraintOnIslandPredicate)); |
511 | List<TypedConstraint> constraintsPtr = ConstraintsCount != 0 ? sortedConstraints : new List<TypedConstraint>(); | 511 | List<TypedConstraint> constraintsPtr = ConstraintsCount != 0 ? sortedConstraints : new List<TypedConstraint>(); |
512 | 512 | ||
513 | InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, constraintsPtr, _debugDrawer); | 513 | InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, constraintsPtr, _debugDrawer); |
514 | 514 | ||
515 | // solve all the constraints for this island | 515 | // solve all the constraints for this island |
516 | _islandManager.BuildAndProcessIslands(CollisionWorld.Dispatcher, CollisionWorld.CollisionObjects, solverCallback); | 516 | _islandManager.BuildAndProcessIslands(CollisionWorld.Dispatcher, CollisionWorld.CollisionObjects, solverCallback); |
517 | } | 517 | } |
518 | 518 | ||
519 | protected void UpdateActivationState(float timeStep) | 519 | protected void UpdateActivationState(float timeStep) |
520 | { | 520 | { |
521 | for (int i = 0; i < CollisionObjects.Count; i++) | 521 | for (int i = 0; i < CollisionObjects.Count; i++) |
522 | { | 522 | { |
523 | CollisionObject colObj = CollisionObjects[i]; | 523 | CollisionObject colObj = CollisionObjects[i]; |
524 | RigidBody body = RigidBody.Upcast(colObj); | 524 | RigidBody body = RigidBody.Upcast(colObj); |
525 | if (body != null) | 525 | if (body != null) |
526 | { | 526 | { |
527 | body.UpdateDeactivation(timeStep); | 527 | body.UpdateDeactivation(timeStep); |
528 | 528 | ||
529 | if (body.WantsSleeping()) | 529 | if (body.WantsSleeping()) |
530 | { | 530 | { |
531 | if (body.IsStaticOrKinematicObject) | 531 | if (body.IsStaticOrKinematicObject) |
532 | { | 532 | { |
533 | body.ActivationState = ActivationState.IslandSleeping; | 533 | body.ActivationState = ActivationState.IslandSleeping; |
534 | } | 534 | } |
535 | else | 535 | else |
536 | { | 536 | { |
537 | if (body.ActivationState == ActivationState.Active) | 537 | if (body.ActivationState == ActivationState.Active) |
538 | body.ActivationState = ActivationState.WantsDeactivation; | 538 | body.ActivationState = ActivationState.WantsDeactivation; |
539 | } | 539 | } |
540 | } | 540 | } |
541 | else | 541 | else |
542 | { | 542 | { |
543 | if (body.ActivationState != ActivationState.DisableDeactivation) | 543 | if (body.ActivationState != ActivationState.DisableDeactivation) |
544 | body.ActivationState = ActivationState.Active; | 544 | body.ActivationState = ActivationState.Active; |
545 | } | 545 | } |
546 | } | 546 | } |
547 | } | 547 | } |
548 | } | 548 | } |
549 | 549 | ||
550 | protected void UpdateVehicles(float timeStep) | 550 | protected void UpdateVehicles(float timeStep) |
551 | { | 551 | { |
552 | for (int i = 0; i < _vehicles.Count; i++) | 552 | for (int i = 0; i < _vehicles.Count; i++) |
553 | { | 553 | { |
554 | RaycastVehicle vehicle = _vehicles[i]; | 554 | RaycastVehicle vehicle = _vehicles[i]; |
555 | vehicle.updateVehicle(timeStep); | 555 | vehicle.updateVehicle(timeStep); |
556 | } | 556 | } |
557 | } | 557 | } |
558 | 558 | ||
559 | protected void StartProfiling(float timeStep) { } | 559 | protected void StartProfiling(float timeStep) { } |
560 | 560 | ||
561 | protected virtual void InternalSingleStepSimulation(float timeStep) | 561 | protected virtual void InternalSingleStepSimulation(float timeStep) |
562 | { | 562 | { |
563 | StartProfiling(timeStep); | 563 | StartProfiling(timeStep); |
564 | 564 | ||
565 | //update aabbs information | 565 | //update aabbs information |
566 | UpdateAabbs(); | 566 | UpdateAabbs(); |
567 | 567 | ||
568 | //apply gravity, predict motion | 568 | //apply gravity, predict motion |
569 | PredictUnconstraintMotion(timeStep); | 569 | PredictUnconstraintMotion(timeStep); |
570 | 570 | ||
571 | DispatcherInfo dispatchInfo = DispatchInfo; | 571 | DispatcherInfo dispatchInfo = DispatchInfo; |
572 | dispatchInfo.TimeStep = timeStep; | 572 | dispatchInfo.TimeStep = timeStep; |
573 | dispatchInfo.StepCount = 0; | 573 | dispatchInfo.StepCount = 0; |
574 | dispatchInfo.DebugDraw = DebugDrawer; | 574 | dispatchInfo.DebugDraw = DebugDrawer; |
575 | 575 | ||
576 | //perform collision detection | 576 | //perform collision detection |
577 | PerformDiscreteCollisionDetection(); | 577 | PerformDiscreteCollisionDetection(); |
578 | 578 | ||
579 | CalculateSimulationIslands(); | 579 | CalculateSimulationIslands(); |
580 | 580 | ||
581 | SolverInfo.TimeStep = timeStep; | 581 | SolverInfo.TimeStep = timeStep; |
582 | 582 | ||
583 | //solve contact and other joint constraints | 583 | //solve contact and other joint constraints |
584 | SolveConstraints(SolverInfo); | 584 | SolveConstraints(SolverInfo); |
585 | 585 | ||
586 | //CallbackTriggers(); | 586 | //CallbackTriggers(); |
587 | 587 | ||
588 | //integrate transforms | 588 | //integrate transforms |
589 | IntegrateTransforms(timeStep); | 589 | IntegrateTransforms(timeStep); |
590 | 590 | ||
591 | //update vehicle simulation | 591 | //update vehicle simulation |
592 | UpdateVehicles(timeStep); | 592 | UpdateVehicles(timeStep); |
593 | 593 | ||
594 | UpdateActivationState(timeStep); | 594 | UpdateActivationState(timeStep); |
595 | } | 595 | } |
596 | 596 | ||
597 | protected void SynchronizeMotionStates() | 597 | protected void SynchronizeMotionStates() |
598 | { | 598 | { |
599 | //debug vehicle wheels | 599 | //debug vehicle wheels |
600 | { | 600 | { |
601 | //todo: iterate over awake simulation islands! | 601 | //todo: iterate over awake simulation islands! |
602 | for (int i = 0; i < CollisionObjects.Count; i++) | 602 | for (int i = 0; i < CollisionObjects.Count; i++) |
603 | { | 603 | { |
604 | CollisionObject colObj = CollisionObjects[i]; | 604 | CollisionObject colObj = CollisionObjects[i]; |
605 | if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0) | 605 | if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0) |
606 | { | 606 | { |
607 | Vector3 color = new Vector3(255f, 255f, 255f); | 607 | Vector3 color = new Vector3(255f, 255f, 255f); |
608 | switch (colObj.ActivationState) | 608 | switch (colObj.ActivationState) |
609 | { | 609 | { |
610 | case ActivationState.Active: | 610 | case ActivationState.Active: |
611 | color = new Vector3(255f, 255f, 255f); break; | 611 | color = new Vector3(255f, 255f, 255f); break; |
612 | case ActivationState.IslandSleeping: | 612 | case ActivationState.IslandSleeping: |
613 | color = new Vector3(0f, 255f, 0f); break; | 613 | color = new Vector3(0f, 255f, 0f); break; |
614 | case ActivationState.WantsDeactivation: | 614 | case ActivationState.WantsDeactivation: |
615 | color = new Vector3(0f, 255f, 255f); break; | 615 | color = new Vector3(0f, 255f, 255f); break; |
616 | case ActivationState.DisableDeactivation: | 616 | case ActivationState.DisableDeactivation: |
617 | color = new Vector3(255f, 0f, 0f); break; | 617 | color = new Vector3(255f, 0f, 0f); break; |
618 | case ActivationState.DisableSimulation: | 618 | case ActivationState.DisableSimulation: |
619 | color = new Vector3(255f, 255f, 0f); break; | 619 | color = new Vector3(255f, 255f, 0f); break; |
620 | default: | 620 | default: |
621 | color = new Vector3(255f, 0f, 0f); break; | 621 | color = new Vector3(255f, 0f, 0f); break; |
622 | } | 622 | } |
623 | 623 | ||
624 | DebugDrawObject(colObj.WorldTransform, colObj.CollisionShape, color); | 624 | DebugDrawObject(colObj.WorldTransform, colObj.CollisionShape, color); |
625 | } | 625 | } |
626 | RigidBody body = RigidBody.Upcast(colObj); | 626 | RigidBody body = RigidBody.Upcast(colObj); |
627 | if (body != null && body.MotionState != null && !body.IsStaticOrKinematicObject) | 627 | if (body != null && body.MotionState != null && !body.IsStaticOrKinematicObject) |
628 | { | 628 | { |
629 | //if (body.ActivationState != ActivationState.IslandSleeping) | 629 | //if (body.ActivationState != ActivationState.IslandSleeping) |
630 | { | 630 | { |
631 | Matrix interpolatedTransform = new Matrix(); | 631 | Matrix interpolatedTransform = new Matrix(); |
632 | TransformUtil.IntegrateTransform(body.InterpolationWorldTransform, | 632 | TransformUtil.IntegrateTransform(body.InterpolationWorldTransform, |
633 | body.InterpolationLinearVelocity, body.InterpolationAngularVelocity, _localTime, ref interpolatedTransform); | 633 | body.InterpolationLinearVelocity, body.InterpolationAngularVelocity, _localTime, ref interpolatedTransform); |
634 | body.MotionState.SetWorldTransform(interpolatedTransform); | 634 | body.MotionState.SetWorldTransform(interpolatedTransform); |
635 | } | 635 | } |
636 | } | 636 | } |
637 | } | 637 | } |
638 | } | 638 | } |
639 | 639 | ||
640 | if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0) | 640 | if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0) |
641 | { | 641 | { |
642 | for (int i = 0; i < _vehicles.Count; i++) | 642 | for (int i = 0; i < _vehicles.Count; i++) |
643 | { | 643 | { |
644 | for (int v = 0; v < _vehicles[i].getNumWheels(); v++) | 644 | for (int v = 0; v < _vehicles[i].getNumWheels(); v++) |
645 | { | 645 | { |
646 | Vector3 wheelColor = new Vector3(0, 255, 255); | 646 | Vector3 wheelColor = new Vector3(0, 255, 255); |
647 | if (_vehicles[i].getWheelInfo(v).RaycastInfo.IsInContact) | 647 | if (_vehicles[i].getWheelInfo(v).RaycastInfo.IsInContact) |
648 | { | 648 | { |
649 | wheelColor = new Vector3(0, 0, 255); | 649 | wheelColor = new Vector3(0, 0, 255); |
650 | } | 650 | } |
651 | else | 651 | else |
652 | { | 652 | { |
653 | wheelColor = new Vector3(255, 0, 255); | 653 | wheelColor = new Vector3(255, 0, 255); |
654 | } | 654 | } |
655 | 655 | ||
656 | //synchronize the wheels with the (interpolated) chassis worldtransform | 656 | //synchronize the wheels with the (interpolated) chassis worldtransform |
657 | _vehicles[i].updateWheelTransform(v, true); | 657 | _vehicles[i].updateWheelTransform(v, true); |
658 | 658 | ||
659 | Vector3 wheelPosWS = _vehicles[i].getWheelInfo(v).WorldTransform.Translation; | 659 | Vector3 wheelPosWS = _vehicles[i].getWheelInfo(v).WorldTransform.Translation; |
660 | 660 | ||
661 | Vector3 axle = new Vector3( | 661 | Vector3 axle = new Vector3( |
662 | MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 0, _vehicles[i].getRightAxis()), | 662 | MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 0, _vehicles[i].getRightAxis()), |
663 | MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 1, _vehicles[i].getRightAxis()), | 663 | MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 1, _vehicles[i].getRightAxis()), |
664 | MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 2, _vehicles[i].getRightAxis())); | 664 | MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 2, _vehicles[i].getRightAxis())); |
665 | 665 | ||
666 | 666 | ||
667 | //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS | 667 | //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS |
668 | //debug wheels (cylinders) | 668 | //debug wheels (cylinders) |
669 | _debugDrawer.DrawLine(wheelPosWS, wheelPosWS + axle, wheelColor); | 669 | _debugDrawer.DrawLine(wheelPosWS, wheelPosWS + axle, wheelColor); |
670 | _debugDrawer.DrawLine(wheelPosWS, _vehicles[i].getWheelInfo(v).RaycastInfo.ContactPointWS, wheelColor); | 670 | _debugDrawer.DrawLine(wheelPosWS, _vehicles[i].getWheelInfo(v).RaycastInfo.ContactPointWS, wheelColor); |
671 | } | 671 | } |
672 | } | 672 | } |
673 | } | 673 | } |
674 | } | 674 | } |
675 | 675 | ||
676 | protected void SaveKinematicState(float timeStep) | 676 | protected void SaveKinematicState(float timeStep) |
677 | { | 677 | { |
678 | for (int i = 0; i < CollisionObjects.Count; i++) | 678 | for (int i = 0; i < CollisionObjects.Count; i++) |
679 | { | 679 | { |
680 | CollisionObject colObj = CollisionObjects[i]; | 680 | CollisionObject colObj = CollisionObjects[i]; |
681 | RigidBody body = RigidBody.Upcast(colObj); | 681 | RigidBody body = RigidBody.Upcast(colObj); |
682 | if (body != null) | 682 | if (body != null) |
683 | { | 683 | { |
684 | if (body.ActivationState != ActivationState.IslandSleeping) | 684 | if (body.ActivationState != ActivationState.IslandSleeping) |
685 | { | 685 | { |
686 | if (body.IsKinematicObject) | 686 | if (body.IsKinematicObject) |
687 | { | 687 | { |
688 | //to calculate velocities next frame | 688 | //to calculate velocities next frame |
689 | body.SaveKinematicState(timeStep); | 689 | body.SaveKinematicState(timeStep); |
690 | } | 690 | } |
691 | } | 691 | } |
692 | } | 692 | } |
693 | } | 693 | } |
694 | } | 694 | } |
695 | 695 | ||
696 | internal class InplaceSolverIslandCallback : SimulationIslandManager.IIslandCallback | 696 | internal class InplaceSolverIslandCallback : SimulationIslandManager.IIslandCallback |
697 | { | 697 | { |
698 | private ContactSolverInfo _solverInfo; | 698 | private ContactSolverInfo _solverInfo; |
699 | private IConstraintSolver _solver; | 699 | private IConstraintSolver _solver; |
700 | private IDebugDraw _debugDrawer; | 700 | private IDebugDraw _debugDrawer; |
701 | private List<TypedConstraint> _sortedConstraints; | 701 | private List<TypedConstraint> _sortedConstraints; |
702 | 702 | ||
703 | public InplaceSolverIslandCallback( | 703 | public InplaceSolverIslandCallback( |
704 | ContactSolverInfo solverInfo, | 704 | ContactSolverInfo solverInfo, |
705 | IConstraintSolver solver, | 705 | IConstraintSolver solver, |
706 | List<TypedConstraint> sortedConstraints, | 706 | List<TypedConstraint> sortedConstraints, |
707 | IDebugDraw debugDrawer) | 707 | IDebugDraw debugDrawer) |
708 | { | 708 | { |
709 | _solverInfo = solverInfo; | 709 | _solverInfo = solverInfo; |
710 | _solver = solver; | 710 | _solver = solver; |
711 | _sortedConstraints = sortedConstraints; | 711 | _sortedConstraints = sortedConstraints; |
712 | _debugDrawer = debugDrawer; | 712 | _debugDrawer = debugDrawer; |
713 | } | 713 | } |
714 | 714 | ||
715 | public ContactSolverInfo SolverInfo { get { return _solverInfo; } set { _solverInfo = value; } } | 715 | public ContactSolverInfo SolverInfo { get { return _solverInfo; } set { _solverInfo = value; } } |
716 | public IConstraintSolver Solver { get { return _solver; } set { _solver = value; } } | 716 | public IConstraintSolver Solver { get { return _solver; } set { _solver = value; } } |
717 | public List<TypedConstraint> Constraints { get { return _sortedConstraints; } set { _sortedConstraints = value; } } | 717 | public List<TypedConstraint> Constraints { get { return _sortedConstraints; } set { _sortedConstraints = value; } } |
718 | public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } } | 718 | public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } } |
719 | 719 | ||
720 | public void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID) | 720 | public void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID) |
721 | { | 721 | { |
722 | //also add all non-contact constraints/joints for this island | 722 | //also add all non-contact constraints/joints for this island |
723 | List<TypedConstraint> startConstraint = new List<TypedConstraint>(); | 723 | List<TypedConstraint> startConstraint = new List<TypedConstraint>(); |
724 | int numCurConstraints = 0; | 724 | int numCurConstraints = 0; |
725 | int startIndex = 0; | 725 | int startIndex = 0; |
726 | int i; | 726 | int i; |
727 | 727 | ||
728 | //find the first constraint for this island | 728 | //find the first constraint for this island |
729 | for (i = 0; i < _sortedConstraints.Count; i++) | 729 | for (i = 0; i < _sortedConstraints.Count; i++) |
730 | { | 730 | { |
731 | if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID) | 731 | if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID) |
732 | { | 732 | { |
733 | //startConstraint = &m_sortedConstraints[i]; | 733 | //startConstraint = &m_sortedConstraints[i]; |
734 | startIndex = i; | 734 | startIndex = i; |
735 | break; | 735 | break; |
736 | } | 736 | } |
737 | } | 737 | } |
738 | //count the number of constraints in this island | 738 | //count the number of constraints in this island |
739 | for (; i < _sortedConstraints.Count; i++) | 739 | for (; i < _sortedConstraints.Count; i++) |
740 | { | 740 | { |
741 | if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID) | 741 | if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID) |
742 | { | 742 | { |
743 | numCurConstraints++; | 743 | numCurConstraints++; |
744 | } | 744 | } |
745 | } | 745 | } |
746 | 746 | ||
747 | for (i = startIndex; i < startIndex + numCurConstraints; i++) | 747 | for (i = startIndex; i < startIndex + numCurConstraints; i++) |
748 | { | 748 | { |
749 | startConstraint.Add(_sortedConstraints[i]); | 749 | startConstraint.Add(_sortedConstraints[i]); |
750 | } | 750 | } |
751 | 751 | ||
752 | _solver.SolveGroup(bodies, manifolds, numManifolds, startConstraint, _solverInfo, _debugDrawer); | 752 | _solver.SolveGroup(bodies, manifolds, numManifolds, startConstraint, _solverInfo, _debugDrawer); |
753 | } | 753 | } |
754 | } | 754 | } |
755 | } | 755 | } |
756 | 756 | ||
757 | internal class DebugDrawCallback : ITriangleIndexCallback, ITriangleCallback | 757 | internal class DebugDrawCallback : ITriangleIndexCallback, ITriangleCallback |
758 | { | 758 | { |
759 | private IDebugDraw _debugDrawer; | 759 | private IDebugDraw _debugDrawer; |
760 | private Vector3 _color; | 760 | private Vector3 _color; |
761 | private Matrix _worldTrans; | 761 | private Matrix _worldTrans; |
762 | 762 | ||
763 | public DebugDrawCallback(IDebugDraw debugDrawer, Matrix worldTrans, Vector3 color) | 763 | public DebugDrawCallback(IDebugDraw debugDrawer, Matrix worldTrans, Vector3 color) |
764 | { | 764 | { |
765 | _debugDrawer = debugDrawer; | 765 | _debugDrawer = debugDrawer; |
766 | _worldTrans = worldTrans; | 766 | _worldTrans = worldTrans; |
767 | _color = color; | 767 | _color = color; |
768 | } | 768 | } |
769 | 769 | ||
770 | public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) | 770 | public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) |
771 | { | 771 | { |
772 | ProcessTriangle(triangle, partId, triangleIndex); | 772 | ProcessTriangle(triangle, partId, triangleIndex); |
773 | } | 773 | } |
774 | 774 | ||
775 | #region ITriangleCallback Members | 775 | #region ITriangleCallback Members |
776 | 776 | ||
777 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) | 777 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) |
778 | { | 778 | { |
779 | Vector3 wv0, wv1, wv2; | 779 | Vector3 wv0, wv1, wv2; |
780 | wv0 = Vector3.TransformNormal(triangle[0], _worldTrans); | 780 | wv0 = Vector3.TransformNormal(triangle[0], _worldTrans); |
781 | wv1 = Vector3.TransformNormal(triangle[1], _worldTrans); | 781 | wv1 = Vector3.TransformNormal(triangle[1], _worldTrans); |
782 | wv2 = Vector3.TransformNormal(triangle[2], _worldTrans); | 782 | wv2 = Vector3.TransformNormal(triangle[2], _worldTrans); |
783 | _debugDrawer.DrawLine(wv0, wv1, _color); | 783 | _debugDrawer.DrawLine(wv0, wv1, _color); |
784 | _debugDrawer.DrawLine(wv1, wv2, _color); | 784 | _debugDrawer.DrawLine(wv1, wv2, _color); |
785 | _debugDrawer.DrawLine(wv2, wv0, _color); | 785 | _debugDrawer.DrawLine(wv2, wv0, _color); |
786 | } | 786 | } |
787 | 787 | ||
788 | #endregion | 788 | #endregion |
789 | } | 789 | } |
790 | } \ No newline at end of file | 790 | } \ No newline at end of file |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs index 647c8ec..34ae608 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs | |||
@@ -1,59 +1,59 @@ | |||
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.Dynamics | 27 | namespace XnaDevRu.BulletX.Dynamics |
28 | { | 28 | { |
29 | public abstract class DynamicsWorld : CollisionWorld | 29 | public abstract class DynamicsWorld : CollisionWorld |
30 | { | 30 | { |
31 | public DynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) | 31 | public DynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) |
32 | : base(dispatcher, pairCache) { } | 32 | : base(dispatcher, pairCache) { } |
33 | 33 | ||
34 | //once a rigidbody is added to the dynamics world, it will get this gravity assigned | 34 | //once a rigidbody is added to the dynamics world, it will get this gravity assigned |
35 | //existing rigidbodies in the world get gravity assigned too, during this method | 35 | //existing rigidbodies in the world get gravity assigned too, during this method |
36 | public abstract Vector3 Gravity { set; } | 36 | public abstract Vector3 Gravity { set; } |
37 | public abstract IConstraintSolver ConstraintSolver { set; } | 37 | public abstract IConstraintSolver ConstraintSolver { set; } |
38 | public virtual int ConstraintsCount { get { return 0; } } | 38 | public virtual int ConstraintsCount { get { return 0; } } |
39 | public abstract IDebugDraw DebugDrawer { get; set; } | 39 | public abstract IDebugDraw DebugDrawer { get; set; } |
40 | 40 | ||
41 | //stepSimulation proceeds the simulation over timeStep units | 41 | //stepSimulation proceeds the simulation over timeStep units |
42 | public abstract void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep); | 42 | public abstract void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep); |
43 | 43 | ||
44 | public void StepSimulation(float timeStep) | 44 | public void StepSimulation(float timeStep) |
45 | { | 45 | { |
46 | StepSimulation(timeStep, 1, 1f / 60f); | 46 | StepSimulation(timeStep, 1, 1f / 60f); |
47 | } | 47 | } |
48 | 48 | ||
49 | public abstract void UpdateAabbs(); | 49 | public abstract void UpdateAabbs(); |
50 | 50 | ||
51 | public virtual void AddConstraint(TypedConstraint constraint) { } | 51 | public virtual void AddConstraint(TypedConstraint constraint) { } |
52 | public virtual void RemoveConstraint(TypedConstraint constraint) { } | 52 | public virtual void RemoveConstraint(TypedConstraint constraint) { } |
53 | 53 | ||
54 | public abstract void AddRigidBody(RigidBody body); | 54 | public abstract void AddRigidBody(RigidBody body); |
55 | public abstract void RemoveRigidBody(RigidBody body); | 55 | public abstract void RemoveRigidBody(RigidBody body); |
56 | 56 | ||
57 | public virtual TypedConstraint GetConstraint(int index) { return null; } | 57 | public virtual TypedConstraint GetConstraint(int index) { return null; } |
58 | } | 58 | } |
59 | } | 59 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs index ec14daf..12a7485 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs | |||
@@ -1,447 +1,447 @@ | |||
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.Dynamics | 27 | namespace XnaDevRu.BulletX.Dynamics |
28 | { | 28 | { |
29 | public class RigidBody : CollisionObject | 29 | public class RigidBody : CollisionObject |
30 | { | 30 | { |
31 | private static float _linearAirDamping = 1; | 31 | private static float _linearAirDamping = 1; |
32 | //'temporarily' global variables | 32 | //'temporarily' global variables |
33 | private static float _rigidBodyDeactivationTime = 2; | 33 | private static float _rigidBodyDeactivationTime = 2; |
34 | private static bool _disableDeactivation = false; | 34 | private static bool _disableDeactivation = false; |
35 | 35 | ||
36 | private static float _linearSleepingThreshold = 0.8f; | 36 | private static float _linearSleepingThreshold = 0.8f; |
37 | private static float _angularSleepingThreshold = 1.0f; | 37 | private static float _angularSleepingThreshold = 1.0f; |
38 | private static int _uniqueId = 0; | 38 | private static int _uniqueId = 0; |
39 | 39 | ||
40 | private Matrix _invInertiaTensorWorld; | 40 | private Matrix _invInertiaTensorWorld; |
41 | private Vector3 _linearVelocity; | 41 | private Vector3 _linearVelocity; |
42 | private Vector3 _angularVelocity; | 42 | private Vector3 _angularVelocity; |
43 | private float _inverseMass; | 43 | private float _inverseMass; |
44 | private float _angularFactor; | 44 | private float _angularFactor; |
45 | 45 | ||
46 | private Vector3 _gravity; | 46 | private Vector3 _gravity; |
47 | private Vector3 _invInertiaLocal; | 47 | private Vector3 _invInertiaLocal; |
48 | private Vector3 _totalForce; | 48 | private Vector3 _totalForce; |
49 | private Vector3 _totalTorque; | 49 | private Vector3 _totalTorque; |
50 | 50 | ||
51 | private float _linearDamping; | 51 | private float _linearDamping; |
52 | private float _angularDamping; | 52 | private float _angularDamping; |
53 | 53 | ||
54 | //m_optionalMotionState allows to automatic synchronize the world transform for active objects | 54 | //m_optionalMotionState allows to automatic synchronize the world transform for active objects |
55 | private MotionState _optionalMotionState; | 55 | private MotionState _optionalMotionState; |
56 | 56 | ||
57 | //for experimental overriding of friction/contact solver func | 57 | //for experimental overriding of friction/contact solver func |
58 | private ContactSolverType _contactSolverType; | 58 | private ContactSolverType _contactSolverType; |
59 | private ContactSolverType _frictionSolverType; | 59 | private ContactSolverType _frictionSolverType; |
60 | 60 | ||
61 | private int _debugBodyId; | 61 | private int _debugBodyId; |
62 | 62 | ||
63 | //Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate | 63 | //Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate |
64 | //doesn't work very well yet (value 0 disabled this damping) | 64 | //doesn't work very well yet (value 0 disabled this damping) |
65 | //note there this influences deactivation thresholds! | 65 | //note there this influences deactivation thresholds! |
66 | private float _clippedAngvelThresholdSqr = 0.01f; | 66 | private float _clippedAngvelThresholdSqr = 0.01f; |
67 | private float _clippedLinearThresholdSqr = 0.01f; | 67 | private float _clippedLinearThresholdSqr = 0.01f; |
68 | 68 | ||
69 | private float _jitterVelocityDampingFactor = 0.7f; | 69 | private float _jitterVelocityDampingFactor = 0.7f; |
70 | 70 | ||
71 | public RigidBody(float mass, MotionState motionState, CollisionShape collisionShape, Vector3 localInertia, float linearDamping, float angularDamping, float friction, float restitution) | 71 | public RigidBody(float mass, MotionState motionState, CollisionShape collisionShape, Vector3 localInertia, float linearDamping, float angularDamping, float friction, float restitution) |
72 | { | 72 | { |
73 | _optionalMotionState = motionState; | 73 | _optionalMotionState = motionState; |
74 | _angularFactor = 1; | 74 | _angularFactor = 1; |
75 | _angularDamping = 0.5f; | 75 | _angularDamping = 0.5f; |
76 | 76 | ||
77 | if (motionState != null) | 77 | if (motionState != null) |
78 | { | 78 | { |
79 | motionState.GetWorldTransform(out _worldTransform); | 79 | motionState.GetWorldTransform(out _worldTransform); |
80 | } | 80 | } |
81 | else | 81 | else |
82 | { | 82 | { |
83 | WorldTransform = Matrix.Identity; | 83 | WorldTransform = Matrix.Identity; |
84 | } | 84 | } |
85 | 85 | ||
86 | InterpolationWorldTransform = WorldTransform; | 86 | InterpolationWorldTransform = WorldTransform; |
87 | InterpolationLinearVelocity = new Vector3(); | 87 | InterpolationLinearVelocity = new Vector3(); |
88 | InterpolationAngularVelocity = new Vector3(); | 88 | InterpolationAngularVelocity = new Vector3(); |
89 | 89 | ||
90 | //moved to btCollisionObject | 90 | //moved to btCollisionObject |
91 | Friction = friction; | 91 | Friction = friction; |
92 | Restitution = restitution; | 92 | Restitution = restitution; |
93 | 93 | ||
94 | CollisionShape = collisionShape; | 94 | CollisionShape = collisionShape; |
95 | _debugBodyId = UniqueID++; | 95 | _debugBodyId = UniqueID++; |
96 | 96 | ||
97 | //m_internalOwner is to allow upcasting from collision object to rigid body | 97 | //m_internalOwner is to allow upcasting from collision object to rigid body |
98 | Owner = this; | 98 | Owner = this; |
99 | 99 | ||
100 | SetMassProps(mass, localInertia); | 100 | SetMassProps(mass, localInertia); |
101 | SetDamping(linearDamping, angularDamping); | 101 | SetDamping(linearDamping, angularDamping); |
102 | UpdateInertiaTensor(); | 102 | UpdateInertiaTensor(); |
103 | } | 103 | } |
104 | 104 | ||
105 | public int DebugBodyID { get { return _debugBodyId; } set { _debugBodyId = value; } } | 105 | public int DebugBodyID { get { return _debugBodyId; } set { _debugBodyId = value; } } |
106 | 106 | ||
107 | public ContactSolverType ContactSolverType { get { return _contactSolverType; } set { _contactSolverType = value; } } | 107 | public ContactSolverType ContactSolverType { get { return _contactSolverType; } set { _contactSolverType = value; } } |
108 | public ContactSolverType FrictionSolverType { get { return _frictionSolverType; } set { _frictionSolverType = value; } } | 108 | public ContactSolverType FrictionSolverType { get { return _frictionSolverType; } set { _frictionSolverType = value; } } |
109 | 109 | ||
110 | public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } } | 110 | public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } } |
111 | 111 | ||
112 | //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase? | 112 | //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase? |
113 | public bool IsInWorld { get { return Broadphase != null; } } | 113 | public bool IsInWorld { get { return Broadphase != null; } } |
114 | 114 | ||
115 | public Vector3 Gravity | 115 | public Vector3 Gravity |
116 | { | 116 | { |
117 | get { return _gravity; } | 117 | get { return _gravity; } |
118 | set | 118 | set |
119 | { | 119 | { |
120 | if (_inverseMass != 0.0f) | 120 | if (_inverseMass != 0.0f) |
121 | { | 121 | { |
122 | _gravity = value * (1.0f / _inverseMass); | 122 | _gravity = value * (1.0f / _inverseMass); |
123 | } | 123 | } |
124 | } | 124 | } |
125 | } | 125 | } |
126 | public Matrix InvInertiaTensorWorld { get { return _invInertiaTensorWorld; } } | 126 | public Matrix InvInertiaTensorWorld { get { return _invInertiaTensorWorld; } } |
127 | public float InverseMass { get { return _inverseMass; } } | 127 | public float InverseMass { get { return _inverseMass; } } |
128 | public Vector3 InvInertiaDiagLocal { get { return _invInertiaLocal; } set { _invInertiaLocal = value; } } | 128 | public Vector3 InvInertiaDiagLocal { get { return _invInertiaLocal; } set { _invInertiaLocal = value; } } |
129 | public Vector3 CenterOfMassPosition { get { return WorldTransform.Translation; } } | 129 | public Vector3 CenterOfMassPosition { get { return WorldTransform.Translation; } } |
130 | public Quaternion Orientation { get { return Quaternion.CreateFromRotationMatrix(WorldTransform); } } | 130 | public Quaternion Orientation { get { return Quaternion.CreateFromRotationMatrix(WorldTransform); } } |
131 | public Matrix CenterOfMassTransform | 131 | public Matrix CenterOfMassTransform |
132 | { | 132 | { |
133 | get { return WorldTransform; } | 133 | get { return WorldTransform; } |
134 | set | 134 | set |
135 | { | 135 | { |
136 | InterpolationWorldTransform = value; | 136 | InterpolationWorldTransform = value; |
137 | InterpolationLinearVelocity = LinearVelocity; | 137 | InterpolationLinearVelocity = LinearVelocity; |
138 | InterpolationAngularVelocity = AngularVelocity; | 138 | InterpolationAngularVelocity = AngularVelocity; |
139 | WorldTransform = value; | 139 | WorldTransform = value; |
140 | UpdateInertiaTensor(); | 140 | UpdateInertiaTensor(); |
141 | } | 141 | } |
142 | } | 142 | } |
143 | 143 | ||
144 | public Vector3 LinearVelocity | 144 | public Vector3 LinearVelocity |
145 | { | 145 | { |
146 | get { return _linearVelocity; } | 146 | get { return _linearVelocity; } |
147 | set | 147 | set |
148 | { | 148 | { |
149 | if (CollisionFlags == CollisionOptions.StaticObject) | 149 | if (CollisionFlags == CollisionOptions.StaticObject) |
150 | throw new BulletException("Static objects can't have linear velocity!"); | 150 | throw new BulletException("Static objects can't have linear velocity!"); |
151 | _linearVelocity = value; | 151 | _linearVelocity = value; |
152 | } | 152 | } |
153 | } | 153 | } |
154 | 154 | ||
155 | public Vector3 AngularVelocity | 155 | public Vector3 AngularVelocity |
156 | { | 156 | { |
157 | get { return _angularVelocity; } | 157 | get { return _angularVelocity; } |
158 | set | 158 | set |
159 | { | 159 | { |
160 | if (CollisionFlags == CollisionOptions.StaticObject) | 160 | if (CollisionFlags == CollisionOptions.StaticObject) |
161 | throw new BulletException("Static objects can't have angular velocity!"); | 161 | throw new BulletException("Static objects can't have angular velocity!"); |
162 | _angularVelocity = value; | 162 | _angularVelocity = value; |
163 | } | 163 | } |
164 | } | 164 | } |
165 | 165 | ||
166 | //MotionState allows to automatic synchronize the world transform for active objects | 166 | //MotionState allows to automatic synchronize the world transform for active objects |
167 | public MotionState MotionState | 167 | public MotionState MotionState |
168 | { | 168 | { |
169 | get { return _optionalMotionState; } | 169 | get { return _optionalMotionState; } |
170 | set | 170 | set |
171 | { | 171 | { |
172 | _optionalMotionState = value; | 172 | _optionalMotionState = value; |
173 | if (_optionalMotionState != null) | 173 | if (_optionalMotionState != null) |
174 | value.GetWorldTransform(out _worldTransform); | 174 | value.GetWorldTransform(out _worldTransform); |
175 | } | 175 | } |
176 | } | 176 | } |
177 | 177 | ||
178 | public static float LinearAirDamping { get { return _linearAirDamping; } set { _linearAirDamping = value; } } | 178 | public static float LinearAirDamping { get { return _linearAirDamping; } set { _linearAirDamping = value; } } |
179 | public static float RigidBodyDeactivationTime { get { return _rigidBodyDeactivationTime; } set { _rigidBodyDeactivationTime = value; } } | 179 | public static float RigidBodyDeactivationTime { get { return _rigidBodyDeactivationTime; } set { _rigidBodyDeactivationTime = value; } } |
180 | public static bool DisableDeactivation { get { return _disableDeactivation; } set { _disableDeactivation = value; } } | 180 | public static bool DisableDeactivation { get { return _disableDeactivation; } set { _disableDeactivation = value; } } |
181 | public static float LinearSleepingThreshold { get { return _linearSleepingThreshold; } set { _linearSleepingThreshold = value; } } | 181 | public static float LinearSleepingThreshold { get { return _linearSleepingThreshold; } set { _linearSleepingThreshold = value; } } |
182 | public static float AngularSleepingThreshold { get { return _angularSleepingThreshold; } set { _angularSleepingThreshold = value; } } | 182 | public static float AngularSleepingThreshold { get { return _angularSleepingThreshold; } set { _angularSleepingThreshold = value; } } |
183 | public static int UniqueID { get { return _uniqueId; } set { _uniqueId = value; } } | 183 | public static int UniqueID { get { return _uniqueId; } set { _uniqueId = value; } } |
184 | 184 | ||
185 | public void ProceedToTransform(Matrix newTrans) | 185 | public void ProceedToTransform(Matrix newTrans) |
186 | { | 186 | { |
187 | CenterOfMassTransform = newTrans; | 187 | CenterOfMassTransform = newTrans; |
188 | } | 188 | } |
189 | 189 | ||
190 | //to keep collision detection and dynamics separate we don't store a rigidbody pointer | 190 | //to keep collision detection and dynamics separate we don't store a rigidbody pointer |
191 | //but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast | 191 | //but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast |
192 | public static RigidBody Upcast(CollisionObject colObj) | 192 | public static RigidBody Upcast(CollisionObject colObj) |
193 | { | 193 | { |
194 | return colObj.Owner as RigidBody; | 194 | return colObj.Owner as RigidBody; |
195 | } | 195 | } |
196 | 196 | ||
197 | // continuous collision detection needs prediction | 197 | // continuous collision detection needs prediction |
198 | public void PredictIntegratedTransform(float step, ref Matrix predictedTransform) | 198 | public void PredictIntegratedTransform(float step, ref Matrix predictedTransform) |
199 | { | 199 | { |
200 | if ((_angularVelocity.LengthSquared() < _clippedAngvelThresholdSqr) && | 200 | if ((_angularVelocity.LengthSquared() < _clippedAngvelThresholdSqr) && |
201 | (_linearVelocity.LengthSquared() < _clippedLinearThresholdSqr)) | 201 | (_linearVelocity.LengthSquared() < _clippedLinearThresholdSqr)) |
202 | { | 202 | { |
203 | _angularVelocity *= _jitterVelocityDampingFactor; | 203 | _angularVelocity *= _jitterVelocityDampingFactor; |
204 | _linearVelocity *= _jitterVelocityDampingFactor; | 204 | _linearVelocity *= _jitterVelocityDampingFactor; |
205 | } | 205 | } |
206 | 206 | ||
207 | TransformUtil.IntegrateTransform(WorldTransform, _linearVelocity, _angularVelocity, step, ref predictedTransform); | 207 | TransformUtil.IntegrateTransform(WorldTransform, _linearVelocity, _angularVelocity, step, ref predictedTransform); |
208 | } | 208 | } |
209 | 209 | ||
210 | public void SaveKinematicState(float step) | 210 | public void SaveKinematicState(float step) |
211 | { | 211 | { |
212 | //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities | 212 | //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities |
213 | if (step != 0) | 213 | if (step != 0) |
214 | { | 214 | { |
215 | //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform | 215 | //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform |
216 | if (MotionState != null) | 216 | if (MotionState != null) |
217 | MotionState.GetWorldTransform(out _worldTransform); | 217 | MotionState.GetWorldTransform(out _worldTransform); |
218 | 218 | ||
219 | TransformUtil.CalculateVelocity(InterpolationWorldTransform, WorldTransform, step, ref _linearVelocity, ref _angularVelocity); | 219 | TransformUtil.CalculateVelocity(InterpolationWorldTransform, WorldTransform, step, ref _linearVelocity, ref _angularVelocity); |
220 | InterpolationLinearVelocity = _linearVelocity; | 220 | InterpolationLinearVelocity = _linearVelocity; |
221 | InterpolationAngularVelocity = _angularVelocity; | 221 | InterpolationAngularVelocity = _angularVelocity; |
222 | InterpolationWorldTransform = WorldTransform; | 222 | InterpolationWorldTransform = WorldTransform; |
223 | } | 223 | } |
224 | } | 224 | } |
225 | 225 | ||
226 | public void ApplyForces(float step) | 226 | public void ApplyForces(float step) |
227 | { | 227 | { |
228 | if (IsStaticOrKinematicObject) | 228 | if (IsStaticOrKinematicObject) |
229 | return; | 229 | return; |
230 | 230 | ||
231 | ApplyCentralForce(_gravity); | 231 | ApplyCentralForce(_gravity); |
232 | 232 | ||
233 | _linearVelocity *= (1 - step * LinearAirDamping * _linearDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * LinearAirDamping * _linearDamping) ? 1.0f : (1 - step * LinearAirDamping * _linearDamping)); | 233 | _linearVelocity *= (1 - step * LinearAirDamping * _linearDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * LinearAirDamping * _linearDamping) ? 1.0f : (1 - step * LinearAirDamping * _linearDamping)); |
234 | _angularVelocity *= (1 - step * _angularDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * _angularDamping) ? 1.0f : (1 - step * _angularDamping)); | 234 | _angularVelocity *= (1 - step * _angularDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * _angularDamping) ? 1.0f : (1 - step * _angularDamping)); |
235 | 235 | ||
236 | float speed = _linearVelocity.Length(); | 236 | float speed = _linearVelocity.Length(); |
237 | if (speed < _linearDamping) | 237 | if (speed < _linearDamping) |
238 | { | 238 | { |
239 | float dampVel = 0.005f; | 239 | float dampVel = 0.005f; |
240 | if (speed > dampVel) | 240 | if (speed > dampVel) |
241 | { | 241 | { |
242 | Vector3 dir = _linearVelocity; | 242 | Vector3 dir = _linearVelocity; |
243 | dir.Normalize(); | 243 | dir.Normalize(); |
244 | _linearVelocity -= dir * dampVel; | 244 | _linearVelocity -= dir * dampVel; |
245 | } | 245 | } |
246 | else | 246 | else |
247 | { | 247 | { |
248 | _linearVelocity = new Vector3(); | 248 | _linearVelocity = new Vector3(); |
249 | } | 249 | } |
250 | } | 250 | } |
251 | 251 | ||
252 | float angSpeed = _angularVelocity.Length(); | 252 | float angSpeed = _angularVelocity.Length(); |
253 | if (angSpeed < _angularDamping) | 253 | if (angSpeed < _angularDamping) |
254 | { | 254 | { |
255 | float angDampVel = 0.005f; | 255 | float angDampVel = 0.005f; |
256 | if (angSpeed > angDampVel) | 256 | if (angSpeed > angDampVel) |
257 | { | 257 | { |
258 | Vector3 dir = _angularVelocity; | 258 | Vector3 dir = _angularVelocity; |
259 | dir.Normalize(); | 259 | dir.Normalize(); |
260 | _angularVelocity -= dir * angDampVel; | 260 | _angularVelocity -= dir * angDampVel; |
261 | } | 261 | } |
262 | else | 262 | else |
263 | { | 263 | { |
264 | _angularVelocity = new Vector3(); | 264 | _angularVelocity = new Vector3(); |
265 | } | 265 | } |
266 | } | 266 | } |
267 | } | 267 | } |
268 | 268 | ||
269 | public void SetDamping(float linDamping, float angDamping) | 269 | public void SetDamping(float linDamping, float angDamping) |
270 | { | 270 | { |
271 | _linearDamping = linDamping < 0.0f ? 0.0f : (1.0f < linDamping ? 1.0f : linDamping); | 271 | _linearDamping = linDamping < 0.0f ? 0.0f : (1.0f < linDamping ? 1.0f : linDamping); |
272 | _angularDamping = angDamping < 0.0f ? 0.0f : (1.0f < angDamping ? 1.0f : angDamping); | 272 | _angularDamping = angDamping < 0.0f ? 0.0f : (1.0f < angDamping ? 1.0f : angDamping); |
273 | } | 273 | } |
274 | 274 | ||
275 | public void SetMassProps(float mass, Vector3 inertia) | 275 | public void SetMassProps(float mass, Vector3 inertia) |
276 | { | 276 | { |
277 | if (mass == 0) | 277 | if (mass == 0) |
278 | { | 278 | { |
279 | CollisionFlags |= CollisionOptions.StaticObject; | 279 | CollisionFlags |= CollisionOptions.StaticObject; |
280 | _inverseMass = 0; | 280 | _inverseMass = 0; |
281 | } | 281 | } |
282 | else | 282 | else |
283 | { | 283 | { |
284 | CollisionFlags &= (~CollisionOptions.StaticObject); | 284 | CollisionFlags &= (~CollisionOptions.StaticObject); |
285 | _inverseMass = 1.0f / mass; | 285 | _inverseMass = 1.0f / mass; |
286 | } | 286 | } |
287 | 287 | ||
288 | _invInertiaLocal = new Vector3(inertia.X != 0.0f ? 1.0f / inertia.X : 0.0f, | 288 | _invInertiaLocal = new Vector3(inertia.X != 0.0f ? 1.0f / inertia.X : 0.0f, |
289 | inertia.Y != 0.0f ? 1.0f / inertia.Y : 0.0f, | 289 | inertia.Y != 0.0f ? 1.0f / inertia.Y : 0.0f, |
290 | inertia.Z != 0.0f ? 1.0f / inertia.Z : 0.0f); | 290 | inertia.Z != 0.0f ? 1.0f / inertia.Z : 0.0f); |
291 | } | 291 | } |
292 | 292 | ||
293 | public void IntegrateVelocities(float step) | 293 | public void IntegrateVelocities(float step) |
294 | { | 294 | { |
295 | if (IsStaticOrKinematicObject) | 295 | if (IsStaticOrKinematicObject) |
296 | return; | 296 | return; |
297 | 297 | ||
298 | _linearVelocity += _totalForce * (_inverseMass * step); | 298 | _linearVelocity += _totalForce * (_inverseMass * step); |
299 | _angularVelocity += Vector3.TransformNormal(_totalTorque, _invInertiaTensorWorld) * step; | 299 | _angularVelocity += Vector3.TransformNormal(_totalTorque, _invInertiaTensorWorld) * step; |
300 | 300 | ||
301 | //float MAX_ANGVEL = MonoXnaCompactMaths.MathHelper.PiOver2; | 301 | //float MAX_ANGVEL = MonoXnaCompactMaths.MathHelper.PiOver2; |
302 | float MAX_ANGVEL = (float)Math.PI / 2.0f; | 302 | float MAX_ANGVEL = (float)Math.PI / 2.0f; |
303 | /// clamp angular velocity. collision calculations will fail on higher angular velocities | 303 | /// clamp angular velocity. collision calculations will fail on higher angular velocities |
304 | float angvel = _angularVelocity.Length(); | 304 | float angvel = _angularVelocity.Length(); |
305 | if (angvel * step > MAX_ANGVEL) | 305 | if (angvel * step > MAX_ANGVEL) |
306 | { | 306 | { |
307 | _angularVelocity *= (MAX_ANGVEL / step) / angvel; | 307 | _angularVelocity *= (MAX_ANGVEL / step) / angvel; |
308 | } | 308 | } |
309 | 309 | ||
310 | ClearForces(); | 310 | ClearForces(); |
311 | } | 311 | } |
312 | 312 | ||
313 | public void ApplyCentralForce(Vector3 force) | 313 | public void ApplyCentralForce(Vector3 force) |
314 | { | 314 | { |
315 | _totalForce += force; | 315 | _totalForce += force; |
316 | } | 316 | } |
317 | 317 | ||
318 | public void ApplyTorque(Vector3 torque) | 318 | public void ApplyTorque(Vector3 torque) |
319 | { | 319 | { |
320 | _totalTorque += torque; | 320 | _totalTorque += torque; |
321 | } | 321 | } |
322 | 322 | ||
323 | public void ApplyForce(Vector3 force, Vector3 rel_pos) | 323 | public void ApplyForce(Vector3 force, Vector3 rel_pos) |
324 | { | 324 | { |
325 | ApplyCentralForce(force); | 325 | ApplyCentralForce(force); |
326 | ApplyTorque(Vector3.Cross(rel_pos, force)); | 326 | ApplyTorque(Vector3.Cross(rel_pos, force)); |
327 | } | 327 | } |
328 | 328 | ||
329 | public void ApplyCentralImpulse(Vector3 impulse) | 329 | public void ApplyCentralImpulse(Vector3 impulse) |
330 | { | 330 | { |
331 | _linearVelocity += impulse * _inverseMass; | 331 | _linearVelocity += impulse * _inverseMass; |
332 | } | 332 | } |
333 | 333 | ||
334 | public void ApplyTorqueImpulse(Vector3 torque) | 334 | public void ApplyTorqueImpulse(Vector3 torque) |
335 | { | 335 | { |
336 | _angularVelocity += Vector3.TransformNormal(torque, _invInertiaTensorWorld); | 336 | _angularVelocity += Vector3.TransformNormal(torque, _invInertiaTensorWorld); |
337 | } | 337 | } |
338 | 338 | ||
339 | public void ApplyImpulse(Vector3 impulse, Vector3 rel_pos) | 339 | public void ApplyImpulse(Vector3 impulse, Vector3 rel_pos) |
340 | { | 340 | { |
341 | if (_inverseMass != 0) | 341 | if (_inverseMass != 0) |
342 | { | 342 | { |
343 | ApplyCentralImpulse(impulse); | 343 | ApplyCentralImpulse(impulse); |
344 | if (_angularFactor != 0) | 344 | if (_angularFactor != 0) |
345 | ApplyTorqueImpulse(Vector3.Cross(rel_pos, impulse) * _angularFactor); | 345 | ApplyTorqueImpulse(Vector3.Cross(rel_pos, impulse) * _angularFactor); |
346 | } | 346 | } |
347 | } | 347 | } |
348 | 348 | ||
349 | public void InternalApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude) | 349 | public void InternalApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude) |
350 | { | 350 | { |
351 | if (_inverseMass != 0) | 351 | if (_inverseMass != 0) |
352 | { | 352 | { |
353 | _linearVelocity += linearComponent * impulseMagnitude; | 353 | _linearVelocity += linearComponent * impulseMagnitude; |
354 | if (_angularFactor != 0) | 354 | if (_angularFactor != 0) |
355 | _angularVelocity += angularComponent * impulseMagnitude * _angularFactor; | 355 | _angularVelocity += angularComponent * impulseMagnitude * _angularFactor; |
356 | } | 356 | } |
357 | } | 357 | } |
358 | 358 | ||
359 | public void ClearForces() | 359 | public void ClearForces() |
360 | { | 360 | { |
361 | _totalForce = new Vector3(); | 361 | _totalForce = new Vector3(); |
362 | _totalTorque = new Vector3(); | 362 | _totalTorque = new Vector3(); |
363 | } | 363 | } |
364 | 364 | ||
365 | public void UpdateInertiaTensor() | 365 | public void UpdateInertiaTensor() |
366 | { | 366 | { |
367 | Matrix temp = WorldTransform; | 367 | Matrix temp = WorldTransform; |
368 | temp.Translation = Vector3.Zero; | 368 | temp.Translation = Vector3.Zero; |
369 | _invInertiaTensorWorld = MatrixOperations.Multiply(MatrixOperations.Scaled(WorldTransform, _invInertiaLocal), Matrix.Transpose(temp)); | 369 | _invInertiaTensorWorld = MatrixOperations.Multiply(MatrixOperations.Scaled(WorldTransform, _invInertiaLocal), Matrix.Transpose(temp)); |
370 | } | 370 | } |
371 | 371 | ||
372 | public Vector3 GetVelocityInLocalPoint(Vector3 relPos) | 372 | public Vector3 GetVelocityInLocalPoint(Vector3 relPos) |
373 | { | 373 | { |
374 | //we also calculate lin/ang velocity for kinematic objects | 374 | //we also calculate lin/ang velocity for kinematic objects |
375 | return _linearVelocity + Vector3.Cross(_angularVelocity, relPos); | 375 | return _linearVelocity + Vector3.Cross(_angularVelocity, relPos); |
376 | 376 | ||
377 | //for kinematic objects, we could also use use: | 377 | //for kinematic objects, we could also use use: |
378 | // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; | 378 | // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; |
379 | } | 379 | } |
380 | 380 | ||
381 | public void Translate(Vector3 v) | 381 | public void Translate(Vector3 v) |
382 | { | 382 | { |
383 | Matrix m = WorldTransform; | 383 | Matrix m = WorldTransform; |
384 | m.Translation += v; | 384 | m.Translation += v; |
385 | WorldTransform = m; | 385 | WorldTransform = m; |
386 | } | 386 | } |
387 | 387 | ||
388 | public void GetAabb(out Vector3 aabbMin, out Vector3 aabbMax) | 388 | public void GetAabb(out Vector3 aabbMin, out Vector3 aabbMax) |
389 | { | 389 | { |
390 | CollisionShape.GetAabb(WorldTransform, out aabbMin, out aabbMax); | 390 | CollisionShape.GetAabb(WorldTransform, out aabbMin, out aabbMax); |
391 | } | 391 | } |
392 | 392 | ||
393 | public float ComputeImpulseDenominator(Vector3 pos, Vector3 normal) | 393 | public float ComputeImpulseDenominator(Vector3 pos, Vector3 normal) |
394 | { | 394 | { |
395 | Vector3 r0 = pos - CenterOfMassPosition; | 395 | Vector3 r0 = pos - CenterOfMassPosition; |
396 | Vector3 c0 = Vector3.Cross(r0, normal); | 396 | Vector3 c0 = Vector3.Cross(r0, normal); |
397 | Vector3 vec = Vector3.Cross(Vector3.TransformNormal(c0, InvInertiaTensorWorld), r0); | 397 | Vector3 vec = Vector3.Cross(Vector3.TransformNormal(c0, InvInertiaTensorWorld), r0); |
398 | 398 | ||
399 | return _inverseMass + Vector3.Dot(normal, vec); | 399 | return _inverseMass + Vector3.Dot(normal, vec); |
400 | 400 | ||
401 | } | 401 | } |
402 | 402 | ||
403 | public float ComputeAngularImpulseDenominator(Vector3 axis) | 403 | public float ComputeAngularImpulseDenominator(Vector3 axis) |
404 | { | 404 | { |
405 | Vector3 vec = Vector3.TransformNormal(axis, InvInertiaTensorWorld); | 405 | Vector3 vec = Vector3.TransformNormal(axis, InvInertiaTensorWorld); |
406 | return Vector3.Dot(axis, vec); | 406 | return Vector3.Dot(axis, vec); |
407 | } | 407 | } |
408 | 408 | ||
409 | public void UpdateDeactivation(float timeStep) | 409 | public void UpdateDeactivation(float timeStep) |
410 | { | 410 | { |
411 | if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.DisableDeactivation)) | 411 | if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.DisableDeactivation)) |
412 | return; | 412 | return; |
413 | 413 | ||
414 | if ((LinearVelocity.LengthSquared() < LinearSleepingThreshold * LinearSleepingThreshold) && | 414 | if ((LinearVelocity.LengthSquared() < LinearSleepingThreshold * LinearSleepingThreshold) && |
415 | (AngularVelocity.LengthSquared() < AngularSleepingThreshold * AngularSleepingThreshold)) | 415 | (AngularVelocity.LengthSquared() < AngularSleepingThreshold * AngularSleepingThreshold)) |
416 | { | 416 | { |
417 | DeactivationTime += timeStep; | 417 | DeactivationTime += timeStep; |
418 | } | 418 | } |
419 | else | 419 | else |
420 | { | 420 | { |
421 | DeactivationTime = 0; | 421 | DeactivationTime = 0; |
422 | ActivationState = ActivationState.Nothing; | 422 | ActivationState = ActivationState.Nothing; |
423 | } | 423 | } |
424 | 424 | ||
425 | } | 425 | } |
426 | 426 | ||
427 | public bool WantsSleeping() | 427 | public bool WantsSleeping() |
428 | { | 428 | { |
429 | 429 | ||
430 | if (ActivationState == ActivationState.DisableDeactivation) | 430 | if (ActivationState == ActivationState.DisableDeactivation) |
431 | return false; | 431 | return false; |
432 | 432 | ||
433 | //disable deactivation | 433 | //disable deactivation |
434 | if (DisableDeactivation || (RigidBodyDeactivationTime == 0)) | 434 | if (DisableDeactivation || (RigidBodyDeactivationTime == 0)) |
435 | return false; | 435 | return false; |
436 | 436 | ||
437 | if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.WantsDeactivation)) | 437 | if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.WantsDeactivation)) |
438 | return true; | 438 | return true; |
439 | 439 | ||
440 | if (DeactivationTime > RigidBodyDeactivationTime) | 440 | if (DeactivationTime > RigidBodyDeactivationTime) |
441 | { | 441 | { |
442 | return true; | 442 | return true; |
443 | } | 443 | } |
444 | return false; | 444 | return false; |
445 | } | 445 | } |
446 | } | 446 | } |
447 | } | 447 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs index aa91a03..3430a73 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs | |||
@@ -1,211 +1,211 @@ | |||
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.Dynamics | 27 | namespace XnaDevRu.BulletX.Dynamics |
28 | { | 28 | { |
29 | public class SimpleDynamicsWorld : DynamicsWorld | 29 | public class SimpleDynamicsWorld : DynamicsWorld |
30 | { | 30 | { |
31 | private IConstraintSolver _constraintSolver; | 31 | private IConstraintSolver _constraintSolver; |
32 | private bool _ownsConstraintSolver; | 32 | private bool _ownsConstraintSolver; |
33 | private Vector3 _gravity; | 33 | private Vector3 _gravity; |
34 | private IDebugDraw _debugDrawer; | 34 | private IDebugDraw _debugDrawer; |
35 | 35 | ||
36 | /// <summary> | 36 | /// <summary> |
37 | /// this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver | 37 | /// this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver |
38 | /// </summary> | 38 | /// </summary> |
39 | /// <param name="dispatcher"></param> | 39 | /// <param name="dispatcher"></param> |
40 | /// <param name="pairCache"></param> | 40 | /// <param name="pairCache"></param> |
41 | /// <param name="constraintSolver"></param> | 41 | /// <param name="constraintSolver"></param> |
42 | public SimpleDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver) | 42 | public SimpleDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver) |
43 | : base(dispatcher, pairCache) | 43 | : base(dispatcher, pairCache) |
44 | { | 44 | { |
45 | _constraintSolver = constraintSolver; | 45 | _constraintSolver = constraintSolver; |
46 | _ownsConstraintSolver = false; | 46 | _ownsConstraintSolver = false; |
47 | _gravity = new Vector3(0, 0, -10); | 47 | _gravity = new Vector3(0, 0, -10); |
48 | } | 48 | } |
49 | 49 | ||
50 | public override Vector3 Gravity | 50 | public override Vector3 Gravity |
51 | { | 51 | { |
52 | set | 52 | set |
53 | { | 53 | { |
54 | _gravity = value; | 54 | _gravity = value; |
55 | for (int i = 0; i < CollisionObjects.Count; i++) | 55 | for (int i = 0; i < CollisionObjects.Count; i++) |
56 | { | 56 | { |
57 | CollisionObject colObj = CollisionObjects[i]; | 57 | CollisionObject colObj = CollisionObjects[i]; |
58 | RigidBody body = RigidBody.Upcast(colObj); | 58 | RigidBody body = RigidBody.Upcast(colObj); |
59 | if (body != null) | 59 | if (body != null) |
60 | { | 60 | { |
61 | body.Gravity = value; | 61 | body.Gravity = value; |
62 | } | 62 | } |
63 | } | 63 | } |
64 | } | 64 | } |
65 | } | 65 | } |
66 | 66 | ||
67 | public override IConstraintSolver ConstraintSolver | 67 | public override IConstraintSolver ConstraintSolver |
68 | { | 68 | { |
69 | set | 69 | set |
70 | { | 70 | { |
71 | _ownsConstraintSolver = false; | 71 | _ownsConstraintSolver = false; |
72 | _constraintSolver = value; | 72 | _constraintSolver = value; |
73 | } | 73 | } |
74 | } | 74 | } |
75 | 75 | ||
76 | public override IDebugDraw DebugDrawer | 76 | public override IDebugDraw DebugDrawer |
77 | { | 77 | { |
78 | get | 78 | get |
79 | { | 79 | { |
80 | return _debugDrawer; | 80 | return _debugDrawer; |
81 | } | 81 | } |
82 | set | 82 | set |
83 | { | 83 | { |
84 | _debugDrawer = value; | 84 | _debugDrawer = value; |
85 | } | 85 | } |
86 | } | 86 | } |
87 | 87 | ||
88 | public override void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep) | 88 | public override void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep) |
89 | { | 89 | { |
90 | //apply gravity, predict motion | 90 | //apply gravity, predict motion |
91 | PredictUnconstraintMotion(timeStep); | 91 | PredictUnconstraintMotion(timeStep); |
92 | 92 | ||
93 | DispatcherInfo dispatchInfo = new DispatcherInfo(); | 93 | DispatcherInfo dispatchInfo = new DispatcherInfo(); |
94 | dispatchInfo.TimeStep = timeStep; | 94 | dispatchInfo.TimeStep = timeStep; |
95 | dispatchInfo.StepCount = 0; | 95 | dispatchInfo.StepCount = 0; |
96 | dispatchInfo.DebugDraw = DebugDrawer; | 96 | dispatchInfo.DebugDraw = DebugDrawer; |
97 | //perform collision detection | 97 | //perform collision detection |
98 | PerformDiscreteCollisionDetection(); | 98 | PerformDiscreteCollisionDetection(); |
99 | 99 | ||
100 | //solve contact constraints | 100 | //solve contact constraints |
101 | int numManifolds = Dispatcher.ManifoldCount; | 101 | int numManifolds = Dispatcher.ManifoldCount; |
102 | if (numManifolds != 0) | 102 | if (numManifolds != 0) |
103 | { | 103 | { |
104 | 104 | ||
105 | List<PersistentManifold> manifolds = (Dispatcher as CollisionDispatcher).Manifolds; | 105 | List<PersistentManifold> manifolds = (Dispatcher as CollisionDispatcher).Manifolds; |
106 | //int numManifolds = m_dispatcher1.GetNumManifolds(); | 106 | //int numManifolds = m_dispatcher1.GetNumManifolds(); |
107 | ContactSolverInfo infoGlobal = new ContactSolverInfo(); | 107 | ContactSolverInfo infoGlobal = new ContactSolverInfo(); |
108 | infoGlobal.TimeStep = timeStep; | 108 | infoGlobal.TimeStep = timeStep; |
109 | 109 | ||
110 | _constraintSolver.SolveGroup(new List<CollisionObject>(), manifolds, manifolds.Count, new List<TypedConstraint>(), infoGlobal, _debugDrawer); | 110 | _constraintSolver.SolveGroup(new List<CollisionObject>(), manifolds, manifolds.Count, new List<TypedConstraint>(), infoGlobal, _debugDrawer); |
111 | } | 111 | } |
112 | //integrate transforms | 112 | //integrate transforms |
113 | IntegrateTransforms(timeStep); | 113 | IntegrateTransforms(timeStep); |
114 | 114 | ||
115 | UpdateAabbs(); | 115 | UpdateAabbs(); |
116 | 116 | ||
117 | SynchronizeMotionStates(); | 117 | SynchronizeMotionStates(); |
118 | } | 118 | } |
119 | 119 | ||
120 | public override void UpdateAabbs() | 120 | public override void UpdateAabbs() |
121 | { | 121 | { |
122 | for (int i = 0; i < CollisionObjects.Count; i++) | 122 | for (int i = 0; i < CollisionObjects.Count; i++) |
123 | { | 123 | { |
124 | CollisionObject colObj = CollisionObjects[i]; | 124 | CollisionObject colObj = CollisionObjects[i]; |
125 | RigidBody body = RigidBody.Upcast(colObj); | 125 | RigidBody body = RigidBody.Upcast(colObj); |
126 | if (body != null) | 126 | if (body != null) |
127 | { | 127 | { |
128 | if (body.IsActive && (!body.IsStaticObject)) | 128 | if (body.IsActive && (!body.IsStaticObject)) |
129 | { | 129 | { |
130 | Vector3 minAabb, maxAabb; | 130 | Vector3 minAabb, maxAabb; |
131 | colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb); | 131 | colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb); |
132 | IBroadphase bp = Broadphase; | 132 | IBroadphase bp = Broadphase; |
133 | bp.SetAabb(body.Broadphase, minAabb, maxAabb); | 133 | bp.SetAabb(body.Broadphase, minAabb, maxAabb); |
134 | } | 134 | } |
135 | } | 135 | } |
136 | } | 136 | } |
137 | } | 137 | } |
138 | 138 | ||
139 | public override void AddRigidBody(RigidBody body) | 139 | public override void AddRigidBody(RigidBody body) |
140 | { | 140 | { |
141 | body.Gravity = _gravity; | 141 | body.Gravity = _gravity; |
142 | 142 | ||
143 | if (body.CollisionShape != null) | 143 | if (body.CollisionShape != null) |
144 | { | 144 | { |
145 | AddCollisionObject(body); | 145 | AddCollisionObject(body); |
146 | } | 146 | } |
147 | } | 147 | } |
148 | 148 | ||
149 | public override void RemoveRigidBody(RigidBody body) | 149 | public override void RemoveRigidBody(RigidBody body) |
150 | { | 150 | { |
151 | RemoveCollisionObject(body); | 151 | RemoveCollisionObject(body); |
152 | } | 152 | } |
153 | 153 | ||
154 | public void SynchronizeMotionStates() | 154 | public void SynchronizeMotionStates() |
155 | { | 155 | { |
156 | for (int i = 0; i < CollisionObjects.Count; i++) | 156 | for (int i = 0; i < CollisionObjects.Count; i++) |
157 | { | 157 | { |
158 | CollisionObject colObj = CollisionObjects[i]; | 158 | CollisionObject colObj = CollisionObjects[i]; |
159 | RigidBody body = RigidBody.Upcast(colObj); | 159 | RigidBody body = RigidBody.Upcast(colObj); |
160 | if (body != null && body.MotionState != null) | 160 | if (body != null && body.MotionState != null) |
161 | { | 161 | { |
162 | if (body.ActivationState != ActivationState.IslandSleeping) | 162 | if (body.ActivationState != ActivationState.IslandSleeping) |
163 | { | 163 | { |
164 | body.MotionState.SetWorldTransform(body.WorldTransform); | 164 | body.MotionState.SetWorldTransform(body.WorldTransform); |
165 | } | 165 | } |
166 | } | 166 | } |
167 | } | 167 | } |
168 | } | 168 | } |
169 | 169 | ||
170 | protected void PredictUnconstraintMotion(float timeStep) | 170 | protected void PredictUnconstraintMotion(float timeStep) |
171 | { | 171 | { |
172 | for (int i = 0; i < CollisionObjects.Count; i++) | 172 | for (int i = 0; i < CollisionObjects.Count; i++) |
173 | { | 173 | { |
174 | CollisionObject colObj = CollisionObjects[i]; | 174 | CollisionObject colObj = CollisionObjects[i]; |
175 | RigidBody body = RigidBody.Upcast(colObj); | 175 | RigidBody body = RigidBody.Upcast(colObj); |
176 | if (body != null) | 176 | if (body != null) |
177 | { | 177 | { |
178 | if (!body.IsStaticObject) | 178 | if (!body.IsStaticObject) |
179 | { | 179 | { |
180 | if (body.IsActive) | 180 | if (body.IsActive) |
181 | { | 181 | { |
182 | body.ApplyForces(timeStep); | 182 | body.ApplyForces(timeStep); |
183 | body.IntegrateVelocities(timeStep); | 183 | body.IntegrateVelocities(timeStep); |
184 | Matrix temp = body.InterpolationWorldTransform; | 184 | Matrix temp = body.InterpolationWorldTransform; |
185 | body.PredictIntegratedTransform(timeStep, ref temp); | 185 | body.PredictIntegratedTransform(timeStep, ref temp); |
186 | body.InterpolationWorldTransform = temp; | 186 | body.InterpolationWorldTransform = temp; |
187 | } | 187 | } |
188 | } | 188 | } |
189 | } | 189 | } |
190 | } | 190 | } |
191 | } | 191 | } |
192 | 192 | ||
193 | protected void IntegrateTransforms(float timeStep) | 193 | protected void IntegrateTransforms(float timeStep) |
194 | { | 194 | { |
195 | Matrix predictedTrans = Matrix.Identity; | 195 | Matrix predictedTrans = Matrix.Identity; |
196 | for (int i = 0; i < CollisionObjects.Count; i++) | 196 | for (int i = 0; i < CollisionObjects.Count; i++) |
197 | { | 197 | { |
198 | CollisionObject colObj = CollisionObjects[i]; | 198 | CollisionObject colObj = CollisionObjects[i]; |
199 | RigidBody body = RigidBody.Upcast(colObj); | 199 | RigidBody body = RigidBody.Upcast(colObj); |
200 | if (body != null) | 200 | if (body != null) |
201 | { | 201 | { |
202 | if (body.IsActive && (!body.IsStaticObject)) | 202 | if (body.IsActive && (!body.IsStaticObject)) |
203 | { | 203 | { |
204 | body.PredictIntegratedTransform(timeStep, ref predictedTrans); | 204 | body.PredictIntegratedTransform(timeStep, ref predictedTrans); |
205 | body.ProceedToTransform(predictedTrans); | 205 | body.ProceedToTransform(predictedTrans); |
206 | } | 206 | } |
207 | } | 207 | } |
208 | } | 208 | } |
209 | } | 209 | } |
210 | } | 210 | } |
211 | } | 211 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs index 90db9ee..4e0e64b 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs | |||
@@ -1,94 +1,94 @@ | |||
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.Dynamics | 27 | namespace XnaDevRu.BulletX.Dynamics |
28 | { | 28 | { |
29 | public class RaycastVehicle : TypedConstraint | 29 | public class RaycastVehicle : TypedConstraint |
30 | { | 30 | { |
31 | public override void BuildJacobian() | 31 | public override void BuildJacobian() |
32 | { | 32 | { |
33 | throw new Exception("The method or operation is not implemented."); | 33 | throw new Exception("The method or operation is not implemented."); |
34 | } | 34 | } |
35 | 35 | ||
36 | public override void SolveConstraint(float timeStep) | 36 | public override void SolveConstraint(float timeStep) |
37 | { | 37 | { |
38 | throw new Exception("The method or operation is not implemented."); | 38 | throw new Exception("The method or operation is not implemented."); |
39 | } | 39 | } |
40 | 40 | ||
41 | public int getNumWheels() | 41 | public int getNumWheels() |
42 | { | 42 | { |
43 | throw new Exception("The method or operation is not implemented."); | 43 | throw new Exception("The method or operation is not implemented."); |
44 | } | 44 | } |
45 | 45 | ||
46 | public WheelInfo getWheelInfo(int v) | 46 | public WheelInfo getWheelInfo(int v) |
47 | { | 47 | { |
48 | throw new Exception("The method or operation is not implemented."); | 48 | throw new Exception("The method or operation is not implemented."); |
49 | } | 49 | } |
50 | 50 | ||
51 | public void updateWheelTransform(int v, bool p) | 51 | public void updateWheelTransform(int v, bool p) |
52 | { | 52 | { |
53 | throw new Exception("The method or operation is not implemented."); | 53 | throw new Exception("The method or operation is not implemented."); |
54 | } | 54 | } |
55 | 55 | ||
56 | public int getRightAxis() | 56 | public int getRightAxis() |
57 | { | 57 | { |
58 | throw new Exception("The method or operation is not implemented."); | 58 | throw new Exception("The method or operation is not implemented."); |
59 | } | 59 | } |
60 | 60 | ||
61 | public void updateVehicle(float timeStep) | 61 | public void updateVehicle(float timeStep) |
62 | { | 62 | { |
63 | throw new Exception("The method or operation is not implemented."); | 63 | throw new Exception("The method or operation is not implemented."); |
64 | } | 64 | } |
65 | } | 65 | } |
66 | 66 | ||
67 | public class DefaultVehicleRaycaster : IVehicleRaycaster | 67 | public class DefaultVehicleRaycaster : IVehicleRaycaster |
68 | { | 68 | { |
69 | DynamicsWorld _dynamicsWorld; | 69 | DynamicsWorld _dynamicsWorld; |
70 | 70 | ||
71 | public DefaultVehicleRaycaster(DynamicsWorld world) | 71 | public DefaultVehicleRaycaster(DynamicsWorld world) |
72 | { | 72 | { |
73 | _dynamicsWorld = world; | 73 | _dynamicsWorld = world; |
74 | } | 74 | } |
75 | 75 | ||
76 | public object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result) | 76 | public object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result) |
77 | { | 77 | { |
78 | CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(from, to); | 78 | CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(from, to); |
79 | _dynamicsWorld.RayTest(from, to, rayCallback); | 79 | _dynamicsWorld.RayTest(from, to, rayCallback); |
80 | 80 | ||
81 | result = new VehicleRaycasterResult(); | 81 | result = new VehicleRaycasterResult(); |
82 | 82 | ||
83 | if (!rayCallback.HasHit) return 0; | 83 | if (!rayCallback.HasHit) return 0; |
84 | RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject); | 84 | RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject); |
85 | if (body == null) return 0; | 85 | if (body == null) return 0; |
86 | 86 | ||
87 | result.HitPointInWorld = rayCallback.HitPointWorld; | 87 | result.HitPointInWorld = rayCallback.HitPointWorld; |
88 | result.HitNormalInWorld = rayCallback.HitNormalWorld; | 88 | result.HitNormalInWorld = rayCallback.HitNormalWorld; |
89 | result.HitNormalInWorld.Normalize(); | 89 | result.HitNormalInWorld.Normalize(); |
90 | result.DistFraction = rayCallback.ClosestHitFraction; | 90 | result.DistFraction = rayCallback.ClosestHitFraction; |
91 | return body; | 91 | return body; |
92 | } | 92 | } |
93 | } | 93 | } |
94 | } \ No newline at end of file | 94 | } \ No newline at end of file |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs index 86829e3..14c13c6 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs | |||
@@ -1,49 +1,49 @@ | |||
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.Dynamics | 27 | namespace XnaDevRu.BulletX.Dynamics |
28 | { | 28 | { |
29 | public interface IVehicleRaycaster | 29 | public interface IVehicleRaycaster |
30 | { | 30 | { |
31 | object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result); | 31 | object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result); |
32 | } | 32 | } |
33 | 33 | ||
34 | public class VehicleRaycasterResult | 34 | public class VehicleRaycasterResult |
35 | { | 35 | { |
36 | private Single _distFraction; | 36 | private Single _distFraction; |
37 | private Vector3 _hitNormalInWorld; | 37 | private Vector3 _hitNormalInWorld; |
38 | private Vector3 _hitPointInWorld; | 38 | private Vector3 _hitPointInWorld; |
39 | 39 | ||
40 | public VehicleRaycasterResult() | 40 | public VehicleRaycasterResult() |
41 | { | 41 | { |
42 | _distFraction = -1; | 42 | _distFraction = -1; |
43 | } | 43 | } |
44 | 44 | ||
45 | public float DistFraction { get { return _distFraction; } set { _distFraction = value; } } | 45 | public float DistFraction { get { return _distFraction; } set { _distFraction = value; } } |
46 | public Vector3 HitNormalInWorld { get { return _hitNormalInWorld; } set { _hitNormalInWorld = value; } } | 46 | public Vector3 HitNormalInWorld { get { return _hitNormalInWorld; } set { _hitNormalInWorld = value; } } |
47 | public Vector3 HitPointInWorld { get { return _hitPointInWorld; } set { _hitPointInWorld = value; } } | 47 | public Vector3 HitPointInWorld { get { return _hitPointInWorld; } set { _hitPointInWorld = value; } } |
48 | } | 48 | } |
49 | } | 49 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs index d4a94f3..516e455 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs | |||
@@ -1,529 +1,529 @@ | |||
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.Dynamics | 27 | namespace XnaDevRu.BulletX.Dynamics |
28 | { | 28 | { |
29 | public struct WheelInfoConstructionInfo | 29 | public struct WheelInfoConstructionInfo |
30 | { | 30 | { |
31 | private Vector3 _chassicConnectionCS; | 31 | private Vector3 _chassicConnectionCS; |
32 | private Vector3 _wheelDirectionCS; | 32 | private Vector3 _wheelDirectionCS; |
33 | private Vector3 _wheelAxisCS; | 33 | private Vector3 _wheelAxisCS; |
34 | 34 | ||
35 | private Single _suspensionRestLength; | 35 | private Single _suspensionRestLength; |
36 | private Single _maxSuspensionTravelCm; | 36 | private Single _maxSuspensionTravelCm; |
37 | private Single _wheelRadius; | 37 | private Single _wheelRadius; |
38 | private Single _suspensionStiffness; | 38 | private Single _suspensionStiffness; |
39 | private Single _wheelsDampingCompression; | 39 | private Single _wheelsDampingCompression; |
40 | private Single _wheelsDampingRelaxation; | 40 | private Single _wheelsDampingRelaxation; |
41 | private Single _frictionSlip; | 41 | private Single _frictionSlip; |
42 | 42 | ||
43 | private Boolean _isFrontWheel; | 43 | private Boolean _isFrontWheel; |
44 | 44 | ||
45 | #region Basic Properties | 45 | #region Basic Properties |
46 | public Vector3 ChassicConnectionCS | 46 | public Vector3 ChassicConnectionCS |
47 | { | 47 | { |
48 | get { return _chassicConnectionCS; } | 48 | get { return _chassicConnectionCS; } |
49 | set { _chassicConnectionCS = value; } | 49 | set { _chassicConnectionCS = value; } |
50 | } | 50 | } |
51 | 51 | ||
52 | public Vector3 WheelDirectionCS | 52 | public Vector3 WheelDirectionCS |
53 | { | 53 | { |
54 | get { return _wheelDirectionCS; } | 54 | get { return _wheelDirectionCS; } |
55 | set { _wheelDirectionCS = value; } | 55 | set { _wheelDirectionCS = value; } |
56 | } | 56 | } |
57 | 57 | ||
58 | public Vector3 WheelAxleCS | 58 | public Vector3 WheelAxleCS |
59 | { | 59 | { |
60 | get { return _wheelAxisCS; } | 60 | get { return _wheelAxisCS; } |
61 | set { _wheelAxisCS = value; } | 61 | set { _wheelAxisCS = value; } |
62 | } | 62 | } |
63 | 63 | ||
64 | 64 | ||
65 | public Single SuspensionRestLength | 65 | public Single SuspensionRestLength |
66 | { | 66 | { |
67 | get { return _suspensionRestLength; } | 67 | get { return _suspensionRestLength; } |
68 | set { _suspensionRestLength = value; } | 68 | set { _suspensionRestLength = value; } |
69 | } | 69 | } |
70 | 70 | ||
71 | public Single MaxSuspensionTravelCm | 71 | public Single MaxSuspensionTravelCm |
72 | { | 72 | { |
73 | get { return _maxSuspensionTravelCm; } | 73 | get { return _maxSuspensionTravelCm; } |
74 | set { _maxSuspensionTravelCm = value; } | 74 | set { _maxSuspensionTravelCm = value; } |
75 | } | 75 | } |
76 | 76 | ||
77 | public Single WheelRadius | 77 | public Single WheelRadius |
78 | { | 78 | { |
79 | get { return _wheelRadius; } | 79 | get { return _wheelRadius; } |
80 | set { _wheelRadius = value; } | 80 | set { _wheelRadius = value; } |
81 | } | 81 | } |
82 | 82 | ||
83 | 83 | ||
84 | public Single SuspensionStiffness | 84 | public Single SuspensionStiffness |
85 | { | 85 | { |
86 | get { return _suspensionStiffness; } | 86 | get { return _suspensionStiffness; } |
87 | set { _suspensionStiffness = value; } | 87 | set { _suspensionStiffness = value; } |
88 | } | 88 | } |
89 | 89 | ||
90 | public Single WheelsDampingCompression | 90 | public Single WheelsDampingCompression |
91 | { | 91 | { |
92 | get { return _wheelsDampingCompression; } | 92 | get { return _wheelsDampingCompression; } |
93 | set { _wheelsDampingCompression = value; } | 93 | set { _wheelsDampingCompression = value; } |
94 | } | 94 | } |
95 | 95 | ||
96 | public Single WheelsDampingRelaxation | 96 | public Single WheelsDampingRelaxation |
97 | { | 97 | { |
98 | get { return _wheelsDampingRelaxation; } | 98 | get { return _wheelsDampingRelaxation; } |
99 | set { _wheelsDampingRelaxation = value; } | 99 | set { _wheelsDampingRelaxation = value; } |
100 | } | 100 | } |
101 | 101 | ||
102 | public Single FrictionSlip | 102 | public Single FrictionSlip |
103 | { | 103 | { |
104 | get { return _frictionSlip; } | 104 | get { return _frictionSlip; } |
105 | set { _frictionSlip = value; } | 105 | set { _frictionSlip = value; } |
106 | } | 106 | } |
107 | 107 | ||
108 | 108 | ||
109 | public Boolean IsFrontWheel | 109 | public Boolean IsFrontWheel |
110 | { | 110 | { |
111 | get { return _isFrontWheel; } | 111 | get { return _isFrontWheel; } |
112 | set { _isFrontWheel = value; } | 112 | set { _isFrontWheel = value; } |
113 | } | 113 | } |
114 | #endregion | 114 | #endregion |
115 | } | 115 | } |
116 | 116 | ||
117 | public struct RaycastInfo | 117 | public struct RaycastInfo |
118 | { | 118 | { |
119 | private Vector3 _contractNormalWS; | 119 | private Vector3 _contractNormalWS; |
120 | private Vector3 _contractPointWS; | 120 | private Vector3 _contractPointWS; |
121 | 121 | ||
122 | private Vector3 _hardPointWS; | 122 | private Vector3 _hardPointWS; |
123 | private Vector3 _wheelDirectionWS; | 123 | private Vector3 _wheelDirectionWS; |
124 | private Vector3 _wheelAxleWS; | 124 | private Vector3 _wheelAxleWS; |
125 | 125 | ||
126 | private Single _suspensionLength; | 126 | private Single _suspensionLength; |
127 | private Boolean _isInContract; | 127 | private Boolean _isInContract; |
128 | 128 | ||
129 | #region Basic Properties | 129 | #region Basic Properties |
130 | public Single SuspensionLength | 130 | public Single SuspensionLength |
131 | { | 131 | { |
132 | get { return _suspensionLength; } | 132 | get { return _suspensionLength; } |
133 | set { _suspensionLength = value; } | 133 | set { _suspensionLength = value; } |
134 | } | 134 | } |
135 | 135 | ||
136 | public Boolean IsInContact | 136 | public Boolean IsInContact |
137 | { | 137 | { |
138 | get { return _isInContract; } | 138 | get { return _isInContract; } |
139 | set { _isInContract = value; } | 139 | set { _isInContract = value; } |
140 | } | 140 | } |
141 | 141 | ||
142 | public Vector3 ContactNormalWS | 142 | public Vector3 ContactNormalWS |
143 | { | 143 | { |
144 | get { return _contractNormalWS; } | 144 | get { return _contractNormalWS; } |
145 | set { _contractNormalWS = value; } | 145 | set { _contractNormalWS = value; } |
146 | } | 146 | } |
147 | 147 | ||
148 | public Vector3 ContactPointWS | 148 | public Vector3 ContactPointWS |
149 | { | 149 | { |
150 | get { return _contractPointWS; } | 150 | get { return _contractPointWS; } |
151 | set { _contractPointWS = value; } | 151 | set { _contractPointWS = value; } |
152 | } | 152 | } |
153 | 153 | ||
154 | public Vector3 HardPointWS | 154 | public Vector3 HardPointWS |
155 | { | 155 | { |
156 | get { return _hardPointWS; } | 156 | get { return _hardPointWS; } |
157 | set { _hardPointWS = value; } | 157 | set { _hardPointWS = value; } |
158 | } | 158 | } |
159 | 159 | ||
160 | public Vector3 WheelDirectionWS | 160 | public Vector3 WheelDirectionWS |
161 | { | 161 | { |
162 | get { return _wheelDirectionWS; } | 162 | get { return _wheelDirectionWS; } |
163 | set { _wheelDirectionWS = value; } | 163 | set { _wheelDirectionWS = value; } |
164 | } | 164 | } |
165 | 165 | ||
166 | public Vector3 WheelAxleWS | 166 | public Vector3 WheelAxleWS |
167 | { | 167 | { |
168 | get { return _wheelAxleWS; } | 168 | get { return _wheelAxleWS; } |
169 | set { _wheelAxleWS = value; } | 169 | set { _wheelAxleWS = value; } |
170 | } | 170 | } |
171 | #endregion | 171 | #endregion |
172 | } | 172 | } |
173 | 173 | ||
174 | public struct WheelInfo | 174 | public struct WheelInfo |
175 | { | 175 | { |
176 | private RaycastInfo _raycastInfo; | 176 | private RaycastInfo _raycastInfo; |
177 | 177 | ||
178 | private Matrix _worldTransform; | 178 | private Matrix _worldTransform; |
179 | 179 | ||
180 | private Vector3 _chassicConnectionPointCS; | 180 | private Vector3 _chassicConnectionPointCS; |
181 | private Vector3 _wheelDirectionCS; | 181 | private Vector3 _wheelDirectionCS; |
182 | private Vector3 _wheelAxleCS; | 182 | private Vector3 _wheelAxleCS; |
183 | 183 | ||
184 | private Single _suspensionRestLength; | 184 | private Single _suspensionRestLength; |
185 | private Single _maxSuspensionTravelCm; | 185 | private Single _maxSuspensionTravelCm; |
186 | 186 | ||
187 | private Single _wheelsRadius; | 187 | private Single _wheelsRadius; |
188 | private Single _rollInfluence; | 188 | private Single _rollInfluence; |
189 | private Single _suspensionStiffness; | 189 | private Single _suspensionStiffness; |
190 | private Single _wheelsDampingCompression; | 190 | private Single _wheelsDampingCompression; |
191 | private Single _wheelsDampingRelaxation; | 191 | private Single _wheelsDampingRelaxation; |
192 | private Single _frictionSlip; | 192 | private Single _frictionSlip; |
193 | private Single _steering; | 193 | private Single _steering; |
194 | private Single _rotation; | 194 | private Single _rotation; |
195 | private Single _deltaRotation; | 195 | private Single _deltaRotation; |
196 | 196 | ||
197 | private Single _engineForce; | 197 | private Single _engineForce; |
198 | private Single _brake; | 198 | private Single _brake; |
199 | private Boolean _isFrontWheel; | 199 | private Boolean _isFrontWheel; |
200 | 200 | ||
201 | 201 | ||
202 | private Single _clippedInvContactDotSuspension; | 202 | private Single _clippedInvContactDotSuspension; |
203 | private Single _skidInfo; | 203 | private Single _skidInfo; |
204 | private Single _wheelsSuspensionForce; | 204 | private Single _wheelsSuspensionForce; |
205 | private Single _suspensionRelativeVelocity; | 205 | private Single _suspensionRelativeVelocity; |
206 | //can be used to store pointer to sync transforms... | 206 | //can be used to store pointer to sync transforms... |
207 | private object _clientInfo; | 207 | private object _clientInfo; |
208 | 208 | ||
209 | #region Constructor | 209 | #region Constructor |
210 | public WheelInfo(WheelInfoConstructionInfo constructionInfo) | 210 | public WheelInfo(WheelInfoConstructionInfo constructionInfo) |
211 | { | 211 | { |
212 | _suspensionRestLength = constructionInfo.SuspensionRestLength; | 212 | _suspensionRestLength = constructionInfo.SuspensionRestLength; |
213 | _maxSuspensionTravelCm = constructionInfo.MaxSuspensionTravelCm; | 213 | _maxSuspensionTravelCm = constructionInfo.MaxSuspensionTravelCm; |
214 | 214 | ||
215 | _wheelsRadius = constructionInfo.WheelRadius; | 215 | _wheelsRadius = constructionInfo.WheelRadius; |
216 | _wheelsDampingCompression = constructionInfo.WheelsDampingCompression; | 216 | _wheelsDampingCompression = constructionInfo.WheelsDampingCompression; |
217 | _wheelsDampingRelaxation = constructionInfo.WheelsDampingRelaxation; | 217 | _wheelsDampingRelaxation = constructionInfo.WheelsDampingRelaxation; |
218 | _wheelDirectionCS = constructionInfo.WheelDirectionCS; | 218 | _wheelDirectionCS = constructionInfo.WheelDirectionCS; |
219 | 219 | ||
220 | _suspensionStiffness = constructionInfo.SuspensionStiffness; | 220 | _suspensionStiffness = constructionInfo.SuspensionStiffness; |
221 | _chassicConnectionPointCS = constructionInfo.ChassicConnectionCS; | 221 | _chassicConnectionPointCS = constructionInfo.ChassicConnectionCS; |
222 | 222 | ||
223 | _wheelAxleCS = constructionInfo.WheelAxleCS; | 223 | _wheelAxleCS = constructionInfo.WheelAxleCS; |
224 | _frictionSlip = constructionInfo.FrictionSlip; | 224 | _frictionSlip = constructionInfo.FrictionSlip; |
225 | 225 | ||
226 | _clippedInvContactDotSuspension = 0; | 226 | _clippedInvContactDotSuspension = 0; |
227 | _suspensionRelativeVelocity = 0; | 227 | _suspensionRelativeVelocity = 0; |
228 | _wheelsSuspensionForce = 0; | 228 | _wheelsSuspensionForce = 0; |
229 | _skidInfo = 0; | 229 | _skidInfo = 0; |
230 | 230 | ||
231 | _steering = 0; | 231 | _steering = 0; |
232 | _engineForce = 0; | 232 | _engineForce = 0; |
233 | _rotation = 0; | 233 | _rotation = 0; |
234 | _rotation = 0; | 234 | _rotation = 0; |
235 | _deltaRotation = 0; | 235 | _deltaRotation = 0; |
236 | _brake = 0; | 236 | _brake = 0; |
237 | _rollInfluence = 0.1f; | 237 | _rollInfluence = 0.1f; |
238 | _brake = 0; | 238 | _brake = 0; |
239 | _rollInfluence = 0.1f; | 239 | _rollInfluence = 0.1f; |
240 | 240 | ||
241 | _isFrontWheel = constructionInfo.IsFrontWheel; | 241 | _isFrontWheel = constructionInfo.IsFrontWheel; |
242 | 242 | ||
243 | _raycastInfo = default(RaycastInfo); | 243 | _raycastInfo = default(RaycastInfo); |
244 | _worldTransform = default(Matrix); | 244 | _worldTransform = default(Matrix); |
245 | _clientInfo = null; | 245 | _clientInfo = null; |
246 | } | 246 | } |
247 | #endregion | 247 | #endregion |
248 | 248 | ||
249 | #region BasicProperties | 249 | #region BasicProperties |
250 | public object ClientInfo { get { return _clientInfo; } set { _clientInfo = value; } } | 250 | public object ClientInfo { get { return _clientInfo; } set { _clientInfo = value; } } |
251 | 251 | ||
252 | public RaycastInfo RaycastInfo | 252 | public RaycastInfo RaycastInfo |
253 | { | 253 | { |
254 | get { return _raycastInfo; } | 254 | get { return _raycastInfo; } |
255 | set { _raycastInfo = value; } | 255 | set { _raycastInfo = value; } |
256 | } | 256 | } |
257 | 257 | ||
258 | public Matrix WorldTransform | 258 | public Matrix WorldTransform |
259 | { | 259 | { |
260 | get { return _worldTransform; } | 260 | get { return _worldTransform; } |
261 | set { _worldTransform = value; } | 261 | set { _worldTransform = value; } |
262 | } | 262 | } |
263 | 263 | ||
264 | public Vector3 ChassicConnectionPointCS | 264 | public Vector3 ChassicConnectionPointCS |
265 | { | 265 | { |
266 | get { return _chassicConnectionPointCS; } | 266 | get { return _chassicConnectionPointCS; } |
267 | set { _chassicConnectionPointCS = value; } | 267 | set { _chassicConnectionPointCS = value; } |
268 | } | 268 | } |
269 | public Vector3 WheelDirectionCS | 269 | public Vector3 WheelDirectionCS |
270 | { | 270 | { |
271 | get { return _wheelDirectionCS; } | 271 | get { return _wheelDirectionCS; } |
272 | set { _wheelDirectionCS = value; } | 272 | set { _wheelDirectionCS = value; } |
273 | } | 273 | } |
274 | public Vector3 WheelAxleCS | 274 | public Vector3 WheelAxleCS |
275 | { | 275 | { |
276 | get { return _wheelAxleCS; } | 276 | get { return _wheelAxleCS; } |
277 | set { _wheelAxleCS = value; } | 277 | set { _wheelAxleCS = value; } |
278 | } | 278 | } |
279 | 279 | ||
280 | public Single SuspensionRestLength | 280 | public Single SuspensionRestLength |
281 | { | 281 | { |
282 | get { return _suspensionRestLength; } | 282 | get { return _suspensionRestLength; } |
283 | set { _suspensionRestLength = value; } | 283 | set { _suspensionRestLength = value; } |
284 | } | 284 | } |
285 | 285 | ||
286 | 286 | ||
287 | public Single MaxSuspensionTravelCm | 287 | public Single MaxSuspensionTravelCm |
288 | { | 288 | { |
289 | get { return _maxSuspensionTravelCm; } | 289 | get { return _maxSuspensionTravelCm; } |
290 | set { _maxSuspensionTravelCm = value; } | 290 | set { _maxSuspensionTravelCm = value; } |
291 | } | 291 | } |
292 | 292 | ||
293 | public Single WheelsRadius | 293 | public Single WheelsRadius |
294 | { | 294 | { |
295 | get { return _wheelsRadius; } | 295 | get { return _wheelsRadius; } |
296 | set { _wheelsRadius = value; } | 296 | set { _wheelsRadius = value; } |
297 | } | 297 | } |
298 | 298 | ||
299 | public Single SuspensionStiffness | 299 | public Single SuspensionStiffness |
300 | { | 300 | { |
301 | get { return _suspensionStiffness; } | 301 | get { return _suspensionStiffness; } |
302 | set { _suspensionStiffness = value; } | 302 | set { _suspensionStiffness = value; } |
303 | } | 303 | } |
304 | 304 | ||
305 | public Single WheelsDampingCompression | 305 | public Single WheelsDampingCompression |
306 | { | 306 | { |
307 | get { return _wheelsDampingCompression; } | 307 | get { return _wheelsDampingCompression; } |
308 | set { _wheelsDampingCompression = value; } | 308 | set { _wheelsDampingCompression = value; } |
309 | } | 309 | } |
310 | 310 | ||
311 | public Single WheelsDampingRelaxation | 311 | public Single WheelsDampingRelaxation |
312 | { | 312 | { |
313 | get { return _wheelsDampingRelaxation; } | 313 | get { return _wheelsDampingRelaxation; } |
314 | set { _wheelsDampingRelaxation = value; } | 314 | set { _wheelsDampingRelaxation = value; } |
315 | } | 315 | } |
316 | 316 | ||
317 | public Single FrictionSlip | 317 | public Single FrictionSlip |
318 | { | 318 | { |
319 | get { return _frictionSlip; } | 319 | get { return _frictionSlip; } |
320 | set { _frictionSlip = value; } | 320 | set { _frictionSlip = value; } |
321 | } | 321 | } |
322 | 322 | ||
323 | public Single Steering | 323 | public Single Steering |
324 | { | 324 | { |
325 | get { return _steering; } | 325 | get { return _steering; } |
326 | set { _steering = value; } | 326 | set { _steering = value; } |
327 | } | 327 | } |
328 | 328 | ||
329 | public Single Rotation | 329 | public Single Rotation |
330 | { | 330 | { |
331 | get { return _rotation; } | 331 | get { return _rotation; } |
332 | set { _rotation = value; } | 332 | set { _rotation = value; } |
333 | } | 333 | } |
334 | 334 | ||
335 | public Single DeltaRotation | 335 | public Single DeltaRotation |
336 | { | 336 | { |
337 | get { return _deltaRotation; } | 337 | get { return _deltaRotation; } |
338 | set { _deltaRotation = value; } | 338 | set { _deltaRotation = value; } |
339 | } | 339 | } |
340 | 340 | ||
341 | public Single RollInfluence | 341 | public Single RollInfluence |
342 | { | 342 | { |
343 | get { return _rollInfluence; } | 343 | get { return _rollInfluence; } |
344 | set { _rollInfluence = value; } | 344 | set { _rollInfluence = value; } |
345 | } | 345 | } |
346 | 346 | ||
347 | public Single EngineForce | 347 | public Single EngineForce |
348 | { | 348 | { |
349 | get { return _engineForce; } | 349 | get { return _engineForce; } |
350 | set { _engineForce = value; } | 350 | set { _engineForce = value; } |
351 | } | 351 | } |
352 | 352 | ||
353 | public Single Brake | 353 | public Single Brake |
354 | { | 354 | { |
355 | get { return _brake; } | 355 | get { return _brake; } |
356 | set { _brake = value; } | 356 | set { _brake = value; } |
357 | } | 357 | } |
358 | 358 | ||
359 | public Boolean IsFrontWheel | 359 | public Boolean IsFrontWheel |
360 | { | 360 | { |
361 | get { return _isFrontWheel; } | 361 | get { return _isFrontWheel; } |
362 | set { _isFrontWheel = value; } | 362 | set { _isFrontWheel = value; } |
363 | } | 363 | } |
364 | 364 | ||
365 | public Single ClippedInvContactDotSuspension | 365 | public Single ClippedInvContactDotSuspension |
366 | { | 366 | { |
367 | get { return _clippedInvContactDotSuspension; } | 367 | get { return _clippedInvContactDotSuspension; } |
368 | set { _clippedInvContactDotSuspension = value; } | 368 | set { _clippedInvContactDotSuspension = value; } |
369 | } | 369 | } |
370 | 370 | ||
371 | public Single SuspensionRelativeVelocity | 371 | public Single SuspensionRelativeVelocity |
372 | { | 372 | { |
373 | get { return _suspensionRelativeVelocity; } | 373 | get { return _suspensionRelativeVelocity; } |
374 | set { _suspensionRelativeVelocity = value; } | 374 | set { _suspensionRelativeVelocity = value; } |
375 | } | 375 | } |
376 | 376 | ||
377 | public Single WheelsSuspensionForce | 377 | public Single WheelsSuspensionForce |
378 | { | 378 | { |
379 | get { return _wheelsSuspensionForce; } | 379 | get { return _wheelsSuspensionForce; } |
380 | set { _wheelsSuspensionForce = value; } | 380 | set { _wheelsSuspensionForce = value; } |
381 | } | 381 | } |
382 | 382 | ||
383 | public Single SkidInfo | 383 | public Single SkidInfo |
384 | { | 384 | { |
385 | get { return _skidInfo; } | 385 | get { return _skidInfo; } |
386 | set { _skidInfo = value; } | 386 | set { _skidInfo = value; } |
387 | } | 387 | } |
388 | #endregion | 388 | #endregion |
389 | 389 | ||
390 | /// <summary> | 390 | /// <summary> |
391 | /// | 391 | /// |
392 | /// </summary> | 392 | /// </summary> |
393 | /// <param name="chassis"></param> | 393 | /// <param name="chassis"></param> |
394 | /// <param name="paramRaycastInfo">Not used!</param> | 394 | /// <param name="paramRaycastInfo">Not used!</param> |
395 | public void UpdateWheel(RigidBody chassis, RaycastInfo paramRaycastInfo) | 395 | public void UpdateWheel(RigidBody chassis, RaycastInfo paramRaycastInfo) |
396 | { | 396 | { |
397 | if (_raycastInfo.IsInContact) | 397 | if (_raycastInfo.IsInContact) |
398 | { | 398 | { |
399 | float project = Vector3.Dot(_raycastInfo.ContactNormalWS, _raycastInfo.WheelDirectionWS); | 399 | float project = Vector3.Dot(_raycastInfo.ContactNormalWS, _raycastInfo.WheelDirectionWS); |
400 | 400 | ||
401 | Vector3 chassisVelocityAtContactPoint = new Vector3(); | 401 | Vector3 chassisVelocityAtContactPoint = new Vector3(); |
402 | Vector3 relpos = _raycastInfo.ContactPointWS - chassis.CenterOfMassPosition; | 402 | Vector3 relpos = _raycastInfo.ContactPointWS - chassis.CenterOfMassPosition; |
403 | chassisVelocityAtContactPoint = chassis.GetVelocityInLocalPoint(relpos); | 403 | chassisVelocityAtContactPoint = chassis.GetVelocityInLocalPoint(relpos); |
404 | float projVel = Vector3.Dot(_raycastInfo.ContactNormalWS, chassisVelocityAtContactPoint); | 404 | float projVel = Vector3.Dot(_raycastInfo.ContactNormalWS, chassisVelocityAtContactPoint); |
405 | 405 | ||
406 | if (project >= -0.1f) | 406 | if (project >= -0.1f) |
407 | { | 407 | { |
408 | _suspensionRelativeVelocity = 0; | 408 | _suspensionRelativeVelocity = 0; |
409 | _clippedInvContactDotSuspension = 1.0f / 0.1f; | 409 | _clippedInvContactDotSuspension = 1.0f / 0.1f; |
410 | } | 410 | } |
411 | else | 411 | else |
412 | { | 412 | { |
413 | float inv = -1 / project; | 413 | float inv = -1 / project; |
414 | _suspensionRelativeVelocity = projVel * inv; | 414 | _suspensionRelativeVelocity = projVel * inv; |
415 | _clippedInvContactDotSuspension = inv; | 415 | _clippedInvContactDotSuspension = inv; |
416 | } | 416 | } |
417 | } | 417 | } |
418 | else | 418 | else |
419 | { | 419 | { |
420 | _raycastInfo.SuspensionLength = _suspensionRestLength; | 420 | _raycastInfo.SuspensionLength = _suspensionRestLength; |
421 | _suspensionRelativeVelocity = 0.0f; | 421 | _suspensionRelativeVelocity = 0.0f; |
422 | _raycastInfo.ContactNormalWS = -_raycastInfo.WheelDirectionWS; | 422 | _raycastInfo.ContactNormalWS = -_raycastInfo.WheelDirectionWS; |
423 | _clippedInvContactDotSuspension = 1.0f; | 423 | _clippedInvContactDotSuspension = 1.0f; |
424 | } | 424 | } |
425 | } | 425 | } |
426 | 426 | ||
427 | // if (m_raycastInfo.m_isInContact) | 427 | // if (m_raycastInfo.m_isInContact) |
428 | 428 | ||
429 | //{ | 429 | //{ |
430 | // btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS ); | 430 | // btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS ); |
431 | // btVector3 chassis_velocity_at_contactPoint; | 431 | // btVector3 chassis_velocity_at_contactPoint; |
432 | // btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition(); | 432 | // btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition(); |
433 | // chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos ); | 433 | // chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos ); |
434 | // btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); | 434 | // btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); |
435 | // if ( project >= -0.1f) | 435 | // if ( project >= -0.1f) |
436 | // { | 436 | // { |
437 | // m_suspensionRelativeVelocity = 0.0f; | 437 | // m_suspensionRelativeVelocity = 0.0f; |
438 | // m_clippedInvContactDotSuspension = 1.0f / 0.1f; | 438 | // m_clippedInvContactDotSuspension = 1.0f / 0.1f; |
439 | // } | 439 | // } |
440 | // else | 440 | // else |
441 | // { | 441 | // { |
442 | // btScalar inv = -1.f / project; | 442 | // btScalar inv = -1.f / project; |
443 | // m_suspensionRelativeVelocity = projVel * inv; | 443 | // m_suspensionRelativeVelocity = projVel * inv; |
444 | // m_clippedInvContactDotSuspension = inv; | 444 | // m_clippedInvContactDotSuspension = inv; |
445 | // } | 445 | // } |
446 | 446 | ||
447 | //} | 447 | //} |
448 | 448 | ||
449 | //else // Not in contact : position wheel in a nice (rest length) position | 449 | //else // Not in contact : position wheel in a nice (rest length) position |
450 | //{ | 450 | //{ |
451 | // m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength(); | 451 | // m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength(); |
452 | // m_suspensionRelativeVelocity = 0.0f; | 452 | // m_suspensionRelativeVelocity = 0.0f; |
453 | // m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; | 453 | // m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; |
454 | // m_clippedInvContactDotSuspension = 1.0f; | 454 | // m_clippedInvContactDotSuspension = 1.0f; |
455 | //} | 455 | //} |
456 | }; | 456 | }; |
457 | 457 | ||
458 | //btScalar m_clippedInvContactDotSuspension; | 458 | //btScalar m_clippedInvContactDotSuspension; |
459 | //btScalar m_suspensionRelativeVelocity; | 459 | //btScalar m_suspensionRelativeVelocity; |
460 | //btScalar m_wheelsSuspensionForce; | 460 | //btScalar m_wheelsSuspensionForce; |
461 | //btScalar m_skidInfo; | 461 | //btScalar m_skidInfo; |
462 | 462 | ||
463 | //void* m_clientInfo;//can be used to store pointer to sync transforms... | 463 | //void* m_clientInfo;//can be used to store pointer to sync transforms... |
464 | 464 | ||
465 | //btWheelInfo(btWheelInfoConstructionInfo& ci) | 465 | //btWheelInfo(btWheelInfoConstructionInfo& ci) |
466 | 466 | ||
467 | //{ | 467 | //{ |
468 | 468 | ||
469 | // m_suspensionRestLength1 = ci.m_suspensionRestLength; | 469 | // m_suspensionRestLength1 = ci.m_suspensionRestLength; |
470 | // m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm; | 470 | // m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm; |
471 | 471 | ||
472 | // m_wheelsRadius = ci.m_wheelRadius; | 472 | // m_wheelsRadius = ci.m_wheelRadius; |
473 | // m_suspensionStiffness = ci.m_suspensionStiffness; | 473 | // m_suspensionStiffness = ci.m_suspensionStiffness; |
474 | // m_wheelsDampingCompression = ci.m_wheelsDampingCompression; | 474 | // m_wheelsDampingCompression = ci.m_wheelsDampingCompression; |
475 | // m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation; | 475 | // m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation; |
476 | // m_chassisConnectionPointCS = ci.m_chassisConnectionCS; | 476 | // m_chassisConnectionPointCS = ci.m_chassisConnectionCS; |
477 | // m_wheelDirectionCS = ci.m_wheelDirectionCS; | 477 | // m_wheelDirectionCS = ci.m_wheelDirectionCS; |
478 | // m_wheelAxleCS = ci.m_wheelAxleCS; | 478 | // m_wheelAxleCS = ci.m_wheelAxleCS; |
479 | // m_frictionSlip = ci.m_frictionSlip; | 479 | // m_frictionSlip = ci.m_frictionSlip; |
480 | // m_steering = 0.f; | 480 | // m_steering = 0.f; |
481 | // m_engineForce = 0.f; | 481 | // m_engineForce = 0.f; |
482 | // m_rotation = 0.f; | 482 | // m_rotation = 0.f; |
483 | // m_deltaRotation = 0.f; | 483 | // m_deltaRotation = 0.f; |
484 | // m_brake = 0.f; | 484 | // m_brake = 0.f; |
485 | // m_rollInfluence = 0.1f; | 485 | // m_rollInfluence = 0.1f; |
486 | // m_bIsFrontWheel = ci.m_bIsFrontWheel; | 486 | // m_bIsFrontWheel = ci.m_bIsFrontWheel; |
487 | 487 | ||
488 | //} | 488 | //} |
489 | 489 | ||
490 | //void updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo); | 490 | //void updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo); |
491 | 491 | ||
492 | //btScalar m_clippedInvContactDotSuspension; | 492 | //btScalar m_clippedInvContactDotSuspension; |
493 | //btScalar m_suspensionRelativeVelocity; | 493 | //btScalar m_suspensionRelativeVelocity; |
494 | ////calculated by suspension | 494 | ////calculated by suspension |
495 | //btScalar m_wheelsSuspensionForce; | 495 | //btScalar m_wheelsSuspensionForce; |
496 | //btScalar m_skidInfo; | 496 | //btScalar m_skidInfo; |
497 | 497 | ||
498 | //}; | 498 | //}; |
499 | 499 | ||
500 | //struct RaycastInfo | 500 | //struct RaycastInfo |
501 | //{ | 501 | //{ |
502 | // //set by raycaster | 502 | // //set by raycaster |
503 | // btVector3 m_contactNormalWS;//contactnormal | 503 | // btVector3 m_contactNormalWS;//contactnormal |
504 | // btVector3 m_contactPointWS;//raycast hitpoint | 504 | // btVector3 m_contactPointWS;//raycast hitpoint |
505 | // btScalar m_suspensionLength; | 505 | // btScalar m_suspensionLength; |
506 | // btVector3 m_hardPointWS;//raycast starting point | 506 | // btVector3 m_hardPointWS;//raycast starting point |
507 | // btVector3 m_wheelDirectionWS; //direction in worldspace | 507 | // btVector3 m_wheelDirectionWS; //direction in worldspace |
508 | // btVector3 m_wheelAxleWS; // axle in worldspace | 508 | // btVector3 m_wheelAxleWS; // axle in worldspace |
509 | // bool m_isInContact; | 509 | // bool m_isInContact; |
510 | // void* m_groundObject; //could be general void* ptr | 510 | // void* m_groundObject; //could be general void* ptr |
511 | //}; | 511 | //}; |
512 | 512 | ||
513 | //struct btWheelInfoConstructionInfo | 513 | //struct btWheelInfoConstructionInfo |
514 | //{ | 514 | //{ |
515 | // btVector3 m_chassisConnectionCS; | 515 | // btVector3 m_chassisConnectionCS; |
516 | // btVector3 m_wheelDirectionCS; | 516 | // btVector3 m_wheelDirectionCS; |
517 | // btVector3 m_wheelAxleCS; | 517 | // btVector3 m_wheelAxleCS; |
518 | // btScalar m_suspensionRestLength; | 518 | // btScalar m_suspensionRestLength; |
519 | // btScalar m_maxSuspensionTravelCm; | 519 | // btScalar m_maxSuspensionTravelCm; |
520 | // btScalar m_wheelRadius; | 520 | // btScalar m_wheelRadius; |
521 | 521 | ||
522 | // float m_suspensionStiffness; | 522 | // float m_suspensionStiffness; |
523 | // float m_wheelsDampingCompression; | 523 | // float m_wheelsDampingCompression; |
524 | // float m_wheelsDampingRelaxation; | 524 | // float m_wheelsDampingRelaxation; |
525 | // float m_frictionSlip; | 525 | // float m_frictionSlip; |
526 | // bool m_bIsFrontWheel; | 526 | // bool m_bIsFrontWheel; |
527 | 527 | ||
528 | //}; | 528 | //}; |
529 | } | 529 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs index abd4885..3ae3ad6 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs | |||
@@ -1,53 +1,53 @@ | |||
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 System.Runtime.Serialization; | 25 | using System.Runtime.Serialization; |
26 | 26 | ||
27 | namespace XnaDevRu.BulletX | 27 | namespace XnaDevRu.BulletX |
28 | { | 28 | { |
29 | public class BulletException : Exception | 29 | public class BulletException : Exception |
30 | { | 30 | { |
31 | internal BulletException() | 31 | internal BulletException() |
32 | : base("Bullet Physics Library has thrown an exception.") | 32 | : base("Bullet Physics Library has thrown an exception.") |
33 | { | 33 | { |
34 | } | 34 | } |
35 | 35 | ||
36 | internal BulletException(string message) | 36 | internal BulletException(string message) |
37 | : base(message) | 37 | : base(message) |
38 | { | 38 | { |
39 | } | 39 | } |
40 | 40 | ||
41 | internal BulletException(string message, Exception innerException) | 41 | internal BulletException(string message, Exception innerException) |
42 | : base(message, innerException) | 42 | : base(message, innerException) |
43 | { | 43 | { |
44 | } | 44 | } |
45 | 45 | ||
46 | #if !XBOX | 46 | #if !XBOX |
47 | internal BulletException(SerializationInfo info, StreamingContext context) | 47 | internal BulletException(SerializationInfo info, StreamingContext context) |
48 | : base(info, context) | 48 | : base(info, context) |
49 | { | 49 | { |
50 | } | 50 | } |
51 | #endif | 51 | #endif |
52 | } | 52 | } |
53 | } | 53 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs b/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs index ef5b5dc..5af979c 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs | |||
@@ -1,60 +1,60 @@ | |||
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 | public enum DebugDrawModes | 29 | public enum DebugDrawModes |
30 | { | 30 | { |
31 | NoDebug = 0, | 31 | NoDebug = 0, |
32 | DrawWireframe = 1, | 32 | DrawWireframe = 1, |
33 | DrawAabb = 2, | 33 | DrawAabb = 2, |
34 | DrawFeaturesText = 4, | 34 | DrawFeaturesText = 4, |
35 | DrawContactPoints = 8, | 35 | DrawContactPoints = 8, |
36 | NoDeactivation = 16, | 36 | NoDeactivation = 16, |
37 | NoHelpText = 32, | 37 | NoHelpText = 32, |
38 | DrawText = 64, | 38 | DrawText = 64, |
39 | ProfileTimings = 128, | 39 | ProfileTimings = 128, |
40 | EnableSatComparison = 256, | 40 | EnableSatComparison = 256, |
41 | DisableBulletLcp = 512, | 41 | DisableBulletLcp = 512, |
42 | EnableCcd = 1024, | 42 | EnableCcd = 1024, |
43 | MaxDebugDrawMode | 43 | MaxDebugDrawMode |
44 | } | 44 | } |
45 | 45 | ||
46 | public interface IDebugDraw | 46 | public interface IDebugDraw |
47 | { | 47 | { |
48 | void DrawLine(Vector3 from, Vector3 to, Vector3 color); | 48 | void DrawLine(Vector3 from, Vector3 to, Vector3 color); |
49 | 49 | ||
50 | void DrawContactPoint( | 50 | void DrawContactPoint( |
51 | Vector3 pointOnB, | 51 | Vector3 pointOnB, |
52 | Vector3 normalOnB, | 52 | Vector3 normalOnB, |
53 | float distance, | 53 | float distance, |
54 | int lifeTime, | 54 | int lifeTime, |
55 | Vector3 color | 55 | Vector3 color |
56 | ); | 56 | ); |
57 | 57 | ||
58 | DebugDrawModes DebugMode { get; set; } | 58 | DebugDrawModes DebugMode { get; set; } |
59 | } | 59 | } |
60 | } \ No newline at end of file | 60 | } \ No newline at end of file |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs index 2a45a20..73dc675 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs | |||
@@ -1,66 +1,66 @@ | |||
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 | // DefaultMotionState provides a common implementation to synchronize world transforms with offsets | 29 | // DefaultMotionState provides a common implementation to synchronize world transforms with offsets |
30 | public class DefaultMotionState : MotionState | 30 | public class DefaultMotionState : MotionState |
31 | { | 31 | { |
32 | private Matrix _graphicsWorldTransform; | 32 | private Matrix _graphicsWorldTransform; |
33 | private Matrix _centerOfMassOffset; | 33 | private Matrix _centerOfMassOffset; |
34 | private Matrix _startWorldTransform; | 34 | private Matrix _startWorldTransform; |
35 | private object _userData; | 35 | private object _userData; |
36 | 36 | ||
37 | public DefaultMotionState() | 37 | public DefaultMotionState() |
38 | : this(Matrix.Identity, Matrix.Identity) { } | 38 | : this(Matrix.Identity, Matrix.Identity) { } |
39 | 39 | ||
40 | public DefaultMotionState(Matrix startTransform, Matrix centerOfMassOffset) | 40 | public DefaultMotionState(Matrix startTransform, Matrix centerOfMassOffset) |
41 | { | 41 | { |
42 | _graphicsWorldTransform = startTransform; | 42 | _graphicsWorldTransform = startTransform; |
43 | _centerOfMassOffset = centerOfMassOffset; | 43 | _centerOfMassOffset = centerOfMassOffset; |
44 | _startWorldTransform = startTransform; | 44 | _startWorldTransform = startTransform; |
45 | } | 45 | } |
46 | 46 | ||
47 | public Matrix GraphicsWorldTransform { get { return _graphicsWorldTransform; } set { _graphicsWorldTransform = value; } } | 47 | public Matrix GraphicsWorldTransform { get { return _graphicsWorldTransform; } set { _graphicsWorldTransform = value; } } |
48 | public Matrix CenterOfMassOffset { get { return _centerOfMassOffset; } set { _centerOfMassOffset = value; } } | 48 | public Matrix CenterOfMassOffset { get { return _centerOfMassOffset; } set { _centerOfMassOffset = value; } } |
49 | public Matrix StartWorldTransform { get { return _startWorldTransform; } set { _startWorldTransform = value; } } | 49 | public Matrix StartWorldTransform { get { return _startWorldTransform; } set { _startWorldTransform = value; } } |
50 | public object UserData { get { return _userData; } set { _userData = value; } } | 50 | public object UserData { get { return _userData; } set { _userData = value; } } |
51 | 51 | ||
52 | // synchronizes world transform from user to physics | 52 | // synchronizes world transform from user to physics |
53 | public override void GetWorldTransform(out Matrix centerOfMassWorldTrans) | 53 | public override void GetWorldTransform(out Matrix centerOfMassWorldTrans) |
54 | { | 54 | { |
55 | centerOfMassWorldTrans = MathHelper.InvertMatrix(_centerOfMassOffset) * _graphicsWorldTransform; | 55 | centerOfMassWorldTrans = MathHelper.InvertMatrix(_centerOfMassOffset) * _graphicsWorldTransform; |
56 | } | 56 | } |
57 | 57 | ||
58 | // synchronizes world transform from physics to user | 58 | // synchronizes world transform from physics to user |
59 | // Bullet only calls the update of worldtransform for active objects | 59 | // Bullet only calls the update of worldtransform for active objects |
60 | public override void SetWorldTransform(Matrix centerOfMassWorldTrans) | 60 | public override void SetWorldTransform(Matrix centerOfMassWorldTrans) |
61 | { | 61 | { |
62 | _graphicsWorldTransform = MatrixOperations.Multiply(centerOfMassWorldTrans, _centerOfMassOffset); | 62 | _graphicsWorldTransform = MatrixOperations.Multiply(centerOfMassWorldTrans, _centerOfMassOffset); |
63 | _graphicsWorldTransform.Translation = centerOfMassWorldTrans.Translation; | 63 | _graphicsWorldTransform.Translation = centerOfMassWorldTrans.Translation; |
64 | } | 64 | } |
65 | } | 65 | } |
66 | } | 66 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs index 2e3f258..15c7bb8 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs | |||
@@ -1,581 +1,581 @@ | |||
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 | 25 | ||
26 | using MonoXnaCompactMaths; | 26 | using MonoXnaCompactMaths; |
27 | 27 | ||
28 | namespace XnaDevRu.BulletX | 28 | namespace XnaDevRu.BulletX |
29 | { | 29 | { |
30 | public static class MathHelper | 30 | public static class MathHelper |
31 | { | 31 | { |
32 | internal const float Sqrt12 = 0.7071067811865475244008443621048490f; | 32 | internal const float Sqrt12 = 0.7071067811865475244008443621048490f; |
33 | internal const float Infinity = 3.402823466e+38f; | 33 | internal const float Infinity = 3.402823466e+38f; |
34 | internal const float Epsilon = 1.192092896e-07f; | 34 | internal const float Epsilon = 1.192092896e-07f; |
35 | 35 | ||
36 | public static Vector3 MatrixToVector(Matrix m, Vector3 v) | 36 | public static Vector3 MatrixToVector(Matrix m, Vector3 v) |
37 | { | 37 | { |
38 | return new Vector3( | 38 | return new Vector3( |
39 | Vector3.Dot(new Vector3(m.M11, m.M12, m.M13), v) + m.Translation.X, | 39 | Vector3.Dot(new Vector3(m.M11, m.M12, m.M13), v) + m.Translation.X, |
40 | Vector3.Dot(new Vector3(m.M21, m.M22, m.M23), v) + m.Translation.Y, | 40 | Vector3.Dot(new Vector3(m.M21, m.M22, m.M23), v) + m.Translation.Y, |
41 | Vector3.Dot(new Vector3(m.M31, m.M32, m.M33), v) + m.Translation.Z | 41 | Vector3.Dot(new Vector3(m.M31, m.M32, m.M33), v) + m.Translation.Z |
42 | ); | 42 | ); |
43 | } | 43 | } |
44 | 44 | ||
45 | internal static int ClosestAxis(Vector4 v) | 45 | internal static int ClosestAxis(Vector4 v) |
46 | { | 46 | { |
47 | return MaxAxis(Absolute(v)); | 47 | return MaxAxis(Absolute(v)); |
48 | } | 48 | } |
49 | 49 | ||
50 | internal static Vector4 Absolute(Vector4 v) | 50 | internal static Vector4 Absolute(Vector4 v) |
51 | { | 51 | { |
52 | return new Vector4(Math.Abs(v.X), Math.Abs(v.Y), Math.Abs(v.Z), Math.Abs(v.W)); | 52 | return new Vector4(Math.Abs(v.X), Math.Abs(v.Y), Math.Abs(v.Z), Math.Abs(v.W)); |
53 | } | 53 | } |
54 | 54 | ||
55 | internal static int MaxAxis(Vector4 v) | 55 | internal static int MaxAxis(Vector4 v) |
56 | { | 56 | { |
57 | int maxIndex = -1; | 57 | int maxIndex = -1; |
58 | float maxVal = float.MinValue; | 58 | float maxVal = float.MinValue; |
59 | if (v.X > maxVal) | 59 | if (v.X > maxVal) |
60 | { | 60 | { |
61 | maxIndex = 0; | 61 | maxIndex = 0; |
62 | maxVal = v.X; | 62 | maxVal = v.X; |
63 | } | 63 | } |
64 | if (v.Y > maxVal) | 64 | if (v.Y > maxVal) |
65 | { | 65 | { |
66 | maxIndex = 1; | 66 | maxIndex = 1; |
67 | maxVal = v.Y; | 67 | maxVal = v.Y; |
68 | } | 68 | } |
69 | if (v.Z > maxVal) | 69 | if (v.Z > maxVal) |
70 | { | 70 | { |
71 | maxIndex = 2; | 71 | maxIndex = 2; |
72 | maxVal = v.Z; | 72 | maxVal = v.Z; |
73 | } | 73 | } |
74 | if (v.W > maxVal) | 74 | if (v.W > maxVal) |
75 | { | 75 | { |
76 | maxIndex = 3; | 76 | maxIndex = 3; |
77 | maxVal = v.W; | 77 | maxVal = v.W; |
78 | } | 78 | } |
79 | 79 | ||
80 | return maxIndex; | 80 | return maxIndex; |
81 | } | 81 | } |
82 | 82 | ||
83 | internal static int MaxAxis(Vector3 v) | 83 | internal static int MaxAxis(Vector3 v) |
84 | { | 84 | { |
85 | return v.X < v.Y ? (v.Y < v.Z ? 2 : 1) : (v.X < v.Z ? 2 : 0); | 85 | return v.X < v.Y ? (v.Y < v.Z ? 2 : 1) : (v.X < v.Z ? 2 : 0); |
86 | } | 86 | } |
87 | 87 | ||
88 | // conservative test for overlap between two aabbs | 88 | // conservative test for overlap between two aabbs |
89 | internal static bool TestAabbAgainstAabb2(Vector3 aabbMinA, Vector3 aabbMaxA, Vector3 aabbMinB, Vector3 aabbMaxB) | 89 | internal static bool TestAabbAgainstAabb2(Vector3 aabbMinA, Vector3 aabbMaxA, Vector3 aabbMinB, Vector3 aabbMaxB) |
90 | { | 90 | { |
91 | bool overlap = true; | 91 | bool overlap = true; |
92 | overlap = (aabbMinA.X > aabbMaxB.X || aabbMaxA.X < aabbMinB.X) ? false : overlap; | 92 | overlap = (aabbMinA.X > aabbMaxB.X || aabbMaxA.X < aabbMinB.X) ? false : overlap; |
93 | overlap = (aabbMinA.Z > aabbMaxB.Z || aabbMaxA.Z < aabbMinB.Z) ? false : overlap; | 93 | overlap = (aabbMinA.Z > aabbMaxB.Z || aabbMaxA.Z < aabbMinB.Z) ? false : overlap; |
94 | overlap = (aabbMinA.Y > aabbMaxB.Y || aabbMaxA.Y < aabbMinB.Y) ? false : overlap; | 94 | overlap = (aabbMinA.Y > aabbMaxB.Y || aabbMaxA.Y < aabbMinB.Y) ? false : overlap; |
95 | return overlap; | 95 | return overlap; |
96 | } | 96 | } |
97 | 97 | ||
98 | internal static bool TestTriangleAgainstAabb2(Vector3[] vertices, Vector3 aabbMin, Vector3 aabbMax) | 98 | internal static bool TestTriangleAgainstAabb2(Vector3[] vertices, Vector3 aabbMin, Vector3 aabbMax) |
99 | { | 99 | { |
100 | Vector3 p1 = vertices[0]; | 100 | Vector3 p1 = vertices[0]; |
101 | Vector3 p2 = vertices[1]; | 101 | Vector3 p2 = vertices[1]; |
102 | Vector3 p3 = vertices[2]; | 102 | Vector3 p3 = vertices[2]; |
103 | 103 | ||
104 | if (Math.Min(Math.Min(p1.X, p2.X), p3.X) > aabbMax.X) return false; | 104 | if (Math.Min(Math.Min(p1.X, p2.X), p3.X) > aabbMax.X) return false; |
105 | if (Math.Max(Math.Max(p1.X, p2.X), p3.X) < aabbMin.X) return false; | 105 | if (Math.Max(Math.Max(p1.X, p2.X), p3.X) < aabbMin.X) return false; |
106 | 106 | ||
107 | if (Math.Min(Math.Min(p1.Z, p2.Z), p3.Z) > aabbMax.Z) return false; | 107 | if (Math.Min(Math.Min(p1.Z, p2.Z), p3.Z) > aabbMax.Z) return false; |
108 | if (Math.Max(Math.Max(p1.Z, p2.Z), p3.Z) < aabbMin.Z) return false; | 108 | if (Math.Max(Math.Max(p1.Z, p2.Z), p3.Z) < aabbMin.Z) return false; |
109 | 109 | ||
110 | if (Math.Min(Math.Min(p1.Y, p2.Y), p3.Y) > aabbMax.Y) return false; | 110 | if (Math.Min(Math.Min(p1.Y, p2.Y), p3.Y) > aabbMax.Y) return false; |
111 | if (Math.Max(Math.Max(p1.Y, p2.Y), p3.Y) < aabbMin.Y) return false; | 111 | if (Math.Max(Math.Max(p1.Y, p2.Y), p3.Y) < aabbMin.Y) return false; |
112 | return true; | 112 | return true; |
113 | } | 113 | } |
114 | 114 | ||
115 | internal static void SetInterpolate3(Vector3 vA, Vector3 vB, float rt, ref Vector3 interpolated) | 115 | internal static void SetInterpolate3(Vector3 vA, Vector3 vB, float rt, ref Vector3 interpolated) |
116 | { | 116 | { |
117 | float s = 1.0f - rt; | 117 | float s = 1.0f - rt; |
118 | interpolated.X = s * vA.X + rt * vB.X; | 118 | interpolated.X = s * vA.X + rt * vB.X; |
119 | interpolated.Y = s * vA.Y + rt * vB.Y; | 119 | interpolated.Y = s * vA.Y + rt * vB.Y; |
120 | interpolated.Z = s * vA.Z + rt * vB.Z; | 120 | interpolated.Z = s * vA.Z + rt * vB.Z; |
121 | } | 121 | } |
122 | 122 | ||
123 | internal static void PlaneSpace1(Vector3 n, ref Vector3 p, ref Vector3 q) | 123 | internal static void PlaneSpace1(Vector3 n, ref Vector3 p, ref Vector3 q) |
124 | { | 124 | { |
125 | if (Math.Abs(n.Z) > Sqrt12) | 125 | if (Math.Abs(n.Z) > Sqrt12) |
126 | { | 126 | { |
127 | // choose p in y-z plane | 127 | // choose p in y-z plane |
128 | float a = n.Y * n.Y + n.Z * n.Z; | 128 | float a = n.Y * n.Y + n.Z * n.Z; |
129 | float k = 1f / (float)Math.Sqrt(a); | 129 | float k = 1f / (float)Math.Sqrt(a); |
130 | p.X = 0; | 130 | p.X = 0; |
131 | p.Y = -n.Z * k; | 131 | p.Y = -n.Z * k; |
132 | p.Z = n.Y * k; | 132 | p.Z = n.Y * k; |
133 | // set q = n x p | 133 | // set q = n x p |
134 | q.X = a * k; | 134 | q.X = a * k; |
135 | q.Y = -n.X * p.Z; | 135 | q.Y = -n.X * p.Z; |
136 | q.Z = n.X * p.Y; | 136 | q.Z = n.X * p.Y; |
137 | } | 137 | } |
138 | else | 138 | else |
139 | { | 139 | { |
140 | // choose p in x-y plane | 140 | // choose p in x-y plane |
141 | float a = n.X * n.X + n.Y * n.Y; | 141 | float a = n.X * n.X + n.Y * n.Y; |
142 | float k = 1f / (float)Math.Sqrt(a); | 142 | float k = 1f / (float)Math.Sqrt(a); |
143 | p.X = -n.Y * k; | 143 | p.X = -n.Y * k; |
144 | p.Y = n.X * k; | 144 | p.Y = n.X * k; |
145 | p.Z = 0; | 145 | p.Z = 0; |
146 | // set q = n x p | 146 | // set q = n x p |
147 | q.X = -n.Z * p.Y; | 147 | q.X = -n.Z * p.Y; |
148 | q.Y = n.Z * p.X; | 148 | q.Y = n.Z * p.X; |
149 | q.Z = a * k; | 149 | q.Z = a * k; |
150 | } | 150 | } |
151 | } | 151 | } |
152 | 152 | ||
153 | internal static bool RayAabb(Vector3 rayFrom, | 153 | internal static bool RayAabb(Vector3 rayFrom, |
154 | Vector3 rayTo, | 154 | Vector3 rayTo, |
155 | Vector3 aabbMin, | 155 | Vector3 aabbMin, |
156 | Vector3 aabbMax, | 156 | Vector3 aabbMax, |
157 | float param, Vector3 normal) | 157 | float param, Vector3 normal) |
158 | { | 158 | { |
159 | Vector3 aabbHalfExtent = (aabbMax - aabbMin) * 0.5f; | 159 | Vector3 aabbHalfExtent = (aabbMax - aabbMin) * 0.5f; |
160 | Vector3 aabbCenter = (aabbMax + aabbMin) * 0.5f; | 160 | Vector3 aabbCenter = (aabbMax + aabbMin) * 0.5f; |
161 | Vector3 source = rayFrom - aabbCenter; | 161 | Vector3 source = rayFrom - aabbCenter; |
162 | Vector3 target = rayTo - aabbCenter; | 162 | Vector3 target = rayTo - aabbCenter; |
163 | int sourceOutcode = Outcode(source, aabbHalfExtent); | 163 | int sourceOutcode = Outcode(source, aabbHalfExtent); |
164 | int targetOutcode = Outcode(target, aabbHalfExtent); | 164 | int targetOutcode = Outcode(target, aabbHalfExtent); |
165 | if ((sourceOutcode & targetOutcode) == 0x0) | 165 | if ((sourceOutcode & targetOutcode) == 0x0) |
166 | { | 166 | { |
167 | float lambda_enter = 0; | 167 | float lambda_enter = 0; |
168 | float lambda_exit = param; | 168 | float lambda_exit = param; |
169 | Vector3 r = target - source; | 169 | Vector3 r = target - source; |
170 | float normSign = 1; | 170 | float normSign = 1; |
171 | Vector3 hitNormal = new Vector3(); | 171 | Vector3 hitNormal = new Vector3(); |
172 | int bit = 1; | 172 | int bit = 1; |
173 | 173 | ||
174 | for (int j = 0; j < 2; j++) | 174 | for (int j = 0; j < 2; j++) |
175 | { | 175 | { |
176 | { | 176 | { |
177 | if ((sourceOutcode & bit) != 0) | 177 | if ((sourceOutcode & bit) != 0) |
178 | { | 178 | { |
179 | float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X; | 179 | float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X; |
180 | if (lambda_enter <= lambda) | 180 | if (lambda_enter <= lambda) |
181 | { | 181 | { |
182 | lambda_enter = lambda; | 182 | lambda_enter = lambda; |
183 | hitNormal = new Vector3(); | 183 | hitNormal = new Vector3(); |
184 | hitNormal.X = normSign; | 184 | hitNormal.X = normSign; |
185 | } | 185 | } |
186 | } | 186 | } |
187 | else if ((targetOutcode & bit) != 0) | 187 | else if ((targetOutcode & bit) != 0) |
188 | { | 188 | { |
189 | float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X; | 189 | float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X; |
190 | SetMin(ref lambda_exit, lambda); | 190 | SetMin(ref lambda_exit, lambda); |
191 | } | 191 | } |
192 | bit <<= 1; | 192 | bit <<= 1; |
193 | } | 193 | } |
194 | { | 194 | { |
195 | if ((sourceOutcode & bit) != 0) | 195 | if ((sourceOutcode & bit) != 0) |
196 | { | 196 | { |
197 | float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y; | 197 | float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y; |
198 | if (lambda_enter <= lambda) | 198 | if (lambda_enter <= lambda) |
199 | { | 199 | { |
200 | lambda_enter = lambda; | 200 | lambda_enter = lambda; |
201 | hitNormal = new Vector3(); | 201 | hitNormal = new Vector3(); |
202 | hitNormal.Y = normSign; | 202 | hitNormal.Y = normSign; |
203 | } | 203 | } |
204 | } | 204 | } |
205 | else if ((targetOutcode & bit) != 0) | 205 | else if ((targetOutcode & bit) != 0) |
206 | { | 206 | { |
207 | float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y; | 207 | float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y; |
208 | SetMin(ref lambda_exit, lambda); | 208 | SetMin(ref lambda_exit, lambda); |
209 | } | 209 | } |
210 | bit <<= 1; | 210 | bit <<= 1; |
211 | } | 211 | } |
212 | { | 212 | { |
213 | if ((sourceOutcode & bit) != 0) | 213 | if ((sourceOutcode & bit) != 0) |
214 | { | 214 | { |
215 | float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z; | 215 | float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z; |
216 | if (lambda_enter <= lambda) | 216 | if (lambda_enter <= lambda) |
217 | { | 217 | { |
218 | lambda_enter = lambda; | 218 | lambda_enter = lambda; |
219 | hitNormal = new Vector3(); | 219 | hitNormal = new Vector3(); |
220 | hitNormal.Z = normSign; | 220 | hitNormal.Z = normSign; |
221 | } | 221 | } |
222 | } | 222 | } |
223 | else if ((targetOutcode & bit) != 0) | 223 | else if ((targetOutcode & bit) != 0) |
224 | { | 224 | { |
225 | float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z; | 225 | float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z; |
226 | SetMin(ref lambda_exit, lambda); | 226 | SetMin(ref lambda_exit, lambda); |
227 | } | 227 | } |
228 | bit <<= 1; | 228 | bit <<= 1; |
229 | } | 229 | } |
230 | normSign = -1; | 230 | normSign = -1; |
231 | } | 231 | } |
232 | if (lambda_enter <= lambda_exit) | 232 | if (lambda_enter <= lambda_exit) |
233 | { | 233 | { |
234 | param = lambda_enter; | 234 | param = lambda_enter; |
235 | normal = hitNormal; | 235 | normal = hitNormal; |
236 | return true; | 236 | return true; |
237 | } | 237 | } |
238 | } | 238 | } |
239 | return false; | 239 | return false; |
240 | } | 240 | } |
241 | 241 | ||
242 | internal static void SetMin(ref float a, float b) | 242 | internal static void SetMin(ref float a, float b) |
243 | { | 243 | { |
244 | if (a > b) | 244 | if (a > b) |
245 | a = b; | 245 | a = b; |
246 | } | 246 | } |
247 | 247 | ||
248 | internal static void SetMax(ref float a, float b) | 248 | internal static void SetMax(ref float a, float b) |
249 | { | 249 | { |
250 | if (a < b) | 250 | if (a < b) |
251 | a = b; | 251 | a = b; |
252 | } | 252 | } |
253 | 253 | ||
254 | internal static void SetMax(ref Vector3 self, Vector3 other) | 254 | internal static void SetMax(ref Vector3 self, Vector3 other) |
255 | { | 255 | { |
256 | if (other.X > self.X) | 256 | if (other.X > self.X) |
257 | self.X = other.X; | 257 | self.X = other.X; |
258 | 258 | ||
259 | if (other.Y > self.Y) | 259 | if (other.Y > self.Y) |
260 | self.Y = other.Y; | 260 | self.Y = other.Y; |
261 | 261 | ||
262 | if (other.Z > self.Z) | 262 | if (other.Z > self.Z) |
263 | self.Z = other.Z; | 263 | self.Z = other.Z; |
264 | } | 264 | } |
265 | 265 | ||
266 | internal static Vector3 SetMax(Vector3 self, Vector3 other) | 266 | internal static Vector3 SetMax(Vector3 self, Vector3 other) |
267 | { | 267 | { |
268 | if (other.X > self.X) | 268 | if (other.X > self.X) |
269 | self.X = other.X; | 269 | self.X = other.X; |
270 | 270 | ||
271 | if (other.Y > self.Y) | 271 | if (other.Y > self.Y) |
272 | self.Y = other.Y; | 272 | self.Y = other.Y; |
273 | 273 | ||
274 | if (other.Z > self.Z) | 274 | if (other.Z > self.Z) |
275 | self.Z = other.Z; | 275 | self.Z = other.Z; |
276 | 276 | ||
277 | return self; | 277 | return self; |
278 | } | 278 | } |
279 | 279 | ||
280 | internal static void SetMin(ref Vector3 self, Vector3 other) | 280 | internal static void SetMin(ref Vector3 self, Vector3 other) |
281 | { | 281 | { |
282 | if (other.X < self.X) | 282 | if (other.X < self.X) |
283 | self.X = other.X; | 283 | self.X = other.X; |
284 | 284 | ||
285 | if (other.Y < self.Y) | 285 | if (other.Y < self.Y) |
286 | self.Y = other.Y; | 286 | self.Y = other.Y; |
287 | 287 | ||
288 | if (other.Z < self.Z) | 288 | if (other.Z < self.Z) |
289 | self.Z = other.Z; | 289 | self.Z = other.Z; |
290 | } | 290 | } |
291 | 291 | ||
292 | internal static Vector3 SetMin(Vector3 self, Vector3 other) | 292 | internal static Vector3 SetMin(Vector3 self, Vector3 other) |
293 | { | 293 | { |
294 | if (other.X < self.X) | 294 | if (other.X < self.X) |
295 | self.X = other.X; | 295 | self.X = other.X; |
296 | 296 | ||
297 | if (other.Y < self.Y) | 297 | if (other.Y < self.Y) |
298 | self.Y = other.Y; | 298 | self.Y = other.Y; |
299 | 299 | ||
300 | if (other.Z < self.Z) | 300 | if (other.Z < self.Z) |
301 | self.Z = other.Z; | 301 | self.Z = other.Z; |
302 | 302 | ||
303 | return self; | 303 | return self; |
304 | } | 304 | } |
305 | 305 | ||
306 | internal static int Outcode(Vector3 p, Vector3 halfExtent) | 306 | internal static int Outcode(Vector3 p, Vector3 halfExtent) |
307 | { | 307 | { |
308 | return (p.X < -halfExtent.X ? 0x01 : 0x0) | | 308 | return (p.X < -halfExtent.X ? 0x01 : 0x0) | |
309 | (p.X > halfExtent.X ? 0x08 : 0x0) | | 309 | (p.X > halfExtent.X ? 0x08 : 0x0) | |
310 | (p.Y < -halfExtent.Y ? 0x02 : 0x0) | | 310 | (p.Y < -halfExtent.Y ? 0x02 : 0x0) | |
311 | (p.Y > halfExtent.Y ? 0x10 : 0x0) | | 311 | (p.Y > halfExtent.Y ? 0x10 : 0x0) | |
312 | (p.Z < -halfExtent.Z ? 0x4 : 0x0) | | 312 | (p.Z < -halfExtent.Z ? 0x4 : 0x0) | |
313 | (p.Z > halfExtent.Z ? 0x20 : 0x0); | 313 | (p.Z > halfExtent.Z ? 0x20 : 0x0); |
314 | } | 314 | } |
315 | 315 | ||
316 | internal static Matrix Absolute(Matrix m) | 316 | internal static Matrix Absolute(Matrix m) |
317 | { | 317 | { |
318 | return new Matrix(Math.Abs(m.M11), Math.Abs(m.M12), Math.Abs(m.M13), Math.Abs(m.M14), | 318 | return new Matrix(Math.Abs(m.M11), Math.Abs(m.M12), Math.Abs(m.M13), Math.Abs(m.M14), |
319 | Math.Abs(m.M21), Math.Abs(m.M22), Math.Abs(m.M23), Math.Abs(m.M24), | 319 | Math.Abs(m.M21), Math.Abs(m.M22), Math.Abs(m.M23), Math.Abs(m.M24), |
320 | Math.Abs(m.M31), Math.Abs(m.M32), Math.Abs(m.M33), Math.Abs(m.M34), | 320 | Math.Abs(m.M31), Math.Abs(m.M32), Math.Abs(m.M33), Math.Abs(m.M34), |
321 | Math.Abs(m.M41), Math.Abs(m.M42), Math.Abs(m.M43), Math.Abs(m.M44)); | 321 | Math.Abs(m.M41), Math.Abs(m.M42), Math.Abs(m.M43), Math.Abs(m.M44)); |
322 | } | 322 | } |
323 | 323 | ||
324 | internal static void SetValueByIndex(ref Vector3 v, int i, float value) | 324 | internal static void SetValueByIndex(ref Vector3 v, int i, float value) |
325 | { | 325 | { |
326 | if (i == 0) | 326 | if (i == 0) |
327 | v.X = value; | 327 | v.X = value; |
328 | else if (i == 1) | 328 | else if (i == 1) |
329 | v.Y = value; | 329 | v.Y = value; |
330 | else | 330 | else |
331 | v.Z = value; | 331 | v.Z = value; |
332 | } | 332 | } |
333 | 333 | ||
334 | internal static float GetValueByIndex(Vector3 v, int i) | 334 | internal static float GetValueByIndex(Vector3 v, int i) |
335 | { | 335 | { |
336 | if (i == 0) | 336 | if (i == 0) |
337 | return v.X; | 337 | return v.X; |
338 | else if (i == 1) | 338 | else if (i == 1) |
339 | return v.Y; | 339 | return v.Y; |
340 | else | 340 | else |
341 | return v.Z; | 341 | return v.Z; |
342 | } | 342 | } |
343 | 343 | ||
344 | internal static Vector3 InvXForm(Matrix m, Vector3 v) | 344 | internal static Vector3 InvXForm(Matrix m, Vector3 v) |
345 | { | 345 | { |
346 | v -= m.Translation; | 346 | v -= m.Translation; |
347 | m.Translation = new Vector3(); | 347 | m.Translation = new Vector3(); |
348 | return MathHelper.Transform(v, Matrix.Transpose(m)); | 348 | return MathHelper.Transform(v, Matrix.Transpose(m)); |
349 | } | 349 | } |
350 | 350 | ||
351 | internal static Matrix InverseTimes(Matrix m, Matrix t) | 351 | internal static Matrix InverseTimes(Matrix m, Matrix t) |
352 | { | 352 | { |
353 | Vector3 v = t.Translation - m.Translation; | 353 | Vector3 v = t.Translation - m.Translation; |
354 | 354 | ||
355 | Matrix mat = TransposeTimes(m, t); | 355 | Matrix mat = TransposeTimes(m, t); |
356 | mat.Translation = Vector3.Transform(v, m); | 356 | mat.Translation = Vector3.Transform(v, m); |
357 | return mat; | 357 | return mat; |
358 | } | 358 | } |
359 | 359 | ||
360 | internal static Matrix TransposeTimes(Matrix mA, Matrix mB) | 360 | internal static Matrix TransposeTimes(Matrix mA, Matrix mB) |
361 | { | 361 | { |
362 | return new Matrix( | 362 | return new Matrix( |
363 | mA.M11 * mB.M11 + mA.M21 * mB.M21 + mA.M31 * mB.M31, | 363 | mA.M11 * mB.M11 + mA.M21 * mB.M21 + mA.M31 * mB.M31, |
364 | mA.M11 * mB.M12 + mA.M21 * mB.M22 + mA.M31 * mB.M32, | 364 | mA.M11 * mB.M12 + mA.M21 * mB.M22 + mA.M31 * mB.M32, |
365 | mA.M11 * mB.M13 + mA.M21 * mB.M23 + mA.M31 * mB.M33, | 365 | mA.M11 * mB.M13 + mA.M21 * mB.M23 + mA.M31 * mB.M33, |
366 | 0, | 366 | 0, |
367 | mA.M12 * mB.M11 + mA.M22 * mB.M21 + mA.M32 * mB.M31, | 367 | mA.M12 * mB.M11 + mA.M22 * mB.M21 + mA.M32 * mB.M31, |
368 | mA.M12 * mB.M12 + mA.M22 * mB.M22 + mA.M32 * mB.M32, | 368 | mA.M12 * mB.M12 + mA.M22 * mB.M22 + mA.M32 * mB.M32, |
369 | mA.M12 * mB.M13 + mA.M22 * mB.M23 + mA.M32 * mB.M33, | 369 | mA.M12 * mB.M13 + mA.M22 * mB.M23 + mA.M32 * mB.M33, |
370 | 0, | 370 | 0, |
371 | mA.M13 * mB.M11 + mA.M23 * mB.M21 + mA.M33 * mB.M31, | 371 | mA.M13 * mB.M11 + mA.M23 * mB.M21 + mA.M33 * mB.M31, |
372 | mA.M13 * mB.M12 + mA.M23 * mB.M22 + mA.M33 * mB.M32, | 372 | mA.M13 * mB.M12 + mA.M23 * mB.M22 + mA.M33 * mB.M32, |
373 | mA.M13 * mB.M13 + mA.M23 * mB.M23 + mA.M33 * mB.M33, | 373 | mA.M13 * mB.M13 + mA.M23 * mB.M23 + mA.M33 * mB.M33, |
374 | 0, 0, 0, 0, 1); | 374 | 0, 0, 0, 0, 1); |
375 | } | 375 | } |
376 | 376 | ||
377 | internal static Vector3 GetColumn(Matrix m, int column) | 377 | internal static Vector3 GetColumn(Matrix m, int column) |
378 | { | 378 | { |
379 | switch (column) | 379 | switch (column) |
380 | { | 380 | { |
381 | case 1: | 381 | case 1: |
382 | return new Vector3(m.M11, m.M21, m.M31); | 382 | return new Vector3(m.M11, m.M21, m.M31); |
383 | case 2: | 383 | case 2: |
384 | return new Vector3(m.M12, m.M22, m.M32); | 384 | return new Vector3(m.M12, m.M22, m.M32); |
385 | case 3: | 385 | case 3: |
386 | return new Vector3(m.M13, m.M23, m.M33); | 386 | return new Vector3(m.M13, m.M23, m.M33); |
387 | default: | 387 | default: |
388 | throw new ArgumentOutOfRangeException("column"); | 388 | throw new ArgumentOutOfRangeException("column"); |
389 | } | 389 | } |
390 | } | 390 | } |
391 | 391 | ||
392 | internal static Vector3 GetRow(Matrix m, int row) | 392 | internal static Vector3 GetRow(Matrix m, int row) |
393 | { | 393 | { |
394 | switch (row) | 394 | switch (row) |
395 | { | 395 | { |
396 | case 1: | 396 | case 1: |
397 | return new Vector3(m.M11, m.M12, m.M13); | 397 | return new Vector3(m.M11, m.M12, m.M13); |
398 | case 2: | 398 | case 2: |
399 | return new Vector3(m.M21, m.M22, m.M23); | 399 | return new Vector3(m.M21, m.M22, m.M23); |
400 | case 3: | 400 | case 3: |
401 | return new Vector3(m.M31, m.M32, m.M33); | 401 | return new Vector3(m.M31, m.M32, m.M33); |
402 | default: | 402 | default: |
403 | throw new ArgumentOutOfRangeException("row"); | 403 | throw new ArgumentOutOfRangeException("row"); |
404 | } | 404 | } |
405 | } | 405 | } |
406 | 406 | ||
407 | internal static Quaternion GetRotation(Matrix m) | 407 | internal static Quaternion GetRotation(Matrix m) |
408 | { | 408 | { |
409 | float trace = m.M11 + m.M22 + m.M33; | 409 | float trace = m.M11 + m.M22 + m.M33; |
410 | Quaternion q = new Quaternion(); | 410 | Quaternion q = new Quaternion(); |
411 | 411 | ||
412 | if (trace > 0) | 412 | if (trace > 0) |
413 | { | 413 | { |
414 | float s = (float)Math.Sqrt(trace + 1.0f); | 414 | float s = (float)Math.Sqrt(trace + 1.0f); |
415 | q.W = s * 0.5f; | 415 | q.W = s * 0.5f; |
416 | s = 0.5f / s; | 416 | s = 0.5f / s; |
417 | 417 | ||
418 | q.X = (m.M32 - m.M23) * s; | 418 | q.X = (m.M32 - m.M23) * s; |
419 | q.Y = (m.M13 - m.M31) * s; | 419 | q.Y = (m.M13 - m.M31) * s; |
420 | q.Z = (m.M21 - m.M12) * s; | 420 | q.Z = (m.M21 - m.M12) * s; |
421 | } | 421 | } |
422 | else | 422 | else |
423 | { | 423 | { |
424 | int i = m.M11 < m.M22 ? | 424 | int i = m.M11 < m.M22 ? |
425 | (m.M22 < m.M33 ? 2 : 1) : | 425 | (m.M22 < m.M33 ? 2 : 1) : |
426 | (m.M11 < m.M33 ? 2 : 0); | 426 | (m.M11 < m.M33 ? 2 : 0); |
427 | int j = (i + 1) % 3; | 427 | int j = (i + 1) % 3; |
428 | int k = (i + 2) % 3; | 428 | int k = (i + 2) % 3; |
429 | 429 | ||
430 | float s = (float)Math.Sqrt(GetElement(m, i, i) - GetElement(m, j, j) - GetElement(m, k, k) + 1.0f); | 430 | float s = (float)Math.Sqrt(GetElement(m, i, i) - GetElement(m, j, j) - GetElement(m, k, k) + 1.0f); |
431 | SetElement(ref q, i, s * 0.5f); | 431 | SetElement(ref q, i, s * 0.5f); |
432 | s = 0.5f / s; | 432 | s = 0.5f / s; |
433 | 433 | ||
434 | q.W = (GetElement(m, k, j) - GetElement(m, j, k)) * s; | 434 | q.W = (GetElement(m, k, j) - GetElement(m, j, k)) * s; |
435 | SetElement(ref q, j, (GetElement(m, j, i) + GetElement(m, i, j)) * s); | 435 | SetElement(ref q, j, (GetElement(m, j, i) + GetElement(m, i, j)) * s); |
436 | SetElement(ref q, k, (GetElement(m, k, i) + GetElement(m, i, k)) * s); | 436 | SetElement(ref q, k, (GetElement(m, k, i) + GetElement(m, i, k)) * s); |
437 | } | 437 | } |
438 | return q; | 438 | return q; |
439 | } | 439 | } |
440 | 440 | ||
441 | internal static float SetElement(ref Quaternion q, int index, float value) | 441 | internal static float SetElement(ref Quaternion q, int index, float value) |
442 | { | 442 | { |
443 | switch (index) | 443 | switch (index) |
444 | { | 444 | { |
445 | case 0: | 445 | case 0: |
446 | q.X = value; break; | 446 | q.X = value; break; |
447 | case 1: | 447 | case 1: |
448 | q.Y = value; break; | 448 | q.Y = value; break; |
449 | case 2: | 449 | case 2: |
450 | q.Z = value; break; | 450 | q.Z = value; break; |
451 | case 3: | 451 | case 3: |
452 | q.W = value; break; | 452 | q.W = value; break; |
453 | } | 453 | } |
454 | 454 | ||
455 | return 0; | 455 | return 0; |
456 | } | 456 | } |
457 | 457 | ||
458 | internal static float GetElement(Quaternion q, int index) | 458 | internal static float GetElement(Quaternion q, int index) |
459 | { | 459 | { |
460 | switch (index) | 460 | switch (index) |
461 | { | 461 | { |
462 | case 0: | 462 | case 0: |
463 | return q.X; | 463 | return q.X; |
464 | case 1: | 464 | case 1: |
465 | return q.Y; | 465 | return q.Y; |
466 | case 2: | 466 | case 2: |
467 | return q.Z; | 467 | return q.Z; |
468 | default: | 468 | default: |
469 | return q.W; | 469 | return q.W; |
470 | } | 470 | } |
471 | } | 471 | } |
472 | 472 | ||
473 | internal static float GetElement(Matrix mat, int index) | 473 | internal static float GetElement(Matrix mat, int index) |
474 | { | 474 | { |
475 | int row = index % 3; | 475 | int row = index % 3; |
476 | int col = index / 3; | 476 | int col = index / 3; |
477 | 477 | ||
478 | return GetElement(mat, row, col); | 478 | return GetElement(mat, row, col); |
479 | } | 479 | } |
480 | 480 | ||
481 | internal static float GetElement(Matrix mat, int row, int col) | 481 | internal static float GetElement(Matrix mat, int row, int col) |
482 | { | 482 | { |
483 | switch (row) | 483 | switch (row) |
484 | { | 484 | { |
485 | case 0: | 485 | case 0: |
486 | switch (col) | 486 | switch (col) |
487 | { | 487 | { |
488 | case 0: | 488 | case 0: |
489 | return mat.M11; | 489 | return mat.M11; |
490 | case 1: | 490 | case 1: |
491 | return mat.M12; | 491 | return mat.M12; |
492 | case 2: | 492 | case 2: |
493 | return mat.M13; | 493 | return mat.M13; |
494 | } break; | 494 | } break; |
495 | case 1: | 495 | case 1: |
496 | switch (col) | 496 | switch (col) |
497 | { | 497 | { |
498 | case 0: | 498 | case 0: |
499 | return mat.M21; | 499 | return mat.M21; |
500 | case 1: | 500 | case 1: |
501 | return mat.M22; | 501 | return mat.M22; |
502 | case 2: | 502 | case 2: |
503 | return mat.M23; | 503 | return mat.M23; |
504 | } break; | 504 | } break; |
505 | case 2: | 505 | case 2: |
506 | switch (col) | 506 | switch (col) |
507 | { | 507 | { |
508 | case 0: | 508 | case 0: |
509 | return mat.M31; | 509 | return mat.M31; |
510 | case 1: | 510 | case 1: |
511 | return mat.M32; | 511 | return mat.M32; |
512 | case 2: | 512 | case 2: |
513 | return mat.M33; | 513 | return mat.M33; |
514 | } break; | 514 | } break; |
515 | } | 515 | } |
516 | 516 | ||
517 | return 0; | 517 | return 0; |
518 | } | 518 | } |
519 | 519 | ||
520 | internal static float GetElement(Vector3 v, int index) | 520 | internal static float GetElement(Vector3 v, int index) |
521 | { | 521 | { |
522 | if (index == 0) | 522 | if (index == 0) |
523 | return v.X; | 523 | return v.X; |
524 | if (index == 1) | 524 | if (index == 1) |
525 | return v.Y; | 525 | return v.Y; |
526 | if (index == 2) | 526 | if (index == 2) |
527 | return v.Z; | 527 | return v.Z; |
528 | 528 | ||
529 | throw new ArgumentOutOfRangeException("index"); | 529 | throw new ArgumentOutOfRangeException("index"); |
530 | } | 530 | } |
531 | 531 | ||
532 | internal static void SetElement(ref Vector3 v, int index, float value) | 532 | internal static void SetElement(ref Vector3 v, int index, float value) |
533 | { | 533 | { |
534 | if (index == 0) | 534 | if (index == 0) |
535 | v.X = value; | 535 | v.X = value; |
536 | else if (index == 1) | 536 | else if (index == 1) |
537 | v.Y = value; | 537 | v.Y = value; |
538 | else if (index == 2) | 538 | else if (index == 2) |
539 | v.Z = value; | 539 | v.Z = value; |
540 | else | 540 | else |
541 | throw new ArgumentOutOfRangeException("index"); | 541 | throw new ArgumentOutOfRangeException("index"); |
542 | } | 542 | } |
543 | 543 | ||
544 | public static Matrix InvertMatrix(Matrix m) | 544 | public static Matrix InvertMatrix(Matrix m) |
545 | { | 545 | { |
546 | Vector3 pos = m.Translation; | 546 | Vector3 pos = m.Translation; |
547 | m.Translation = Vector3.Zero; | 547 | m.Translation = Vector3.Zero; |
548 | Matrix inv = Matrix.Transpose(m); | 548 | Matrix inv = Matrix.Transpose(m); |
549 | pos = Vector3.Transform(-pos, m); | 549 | pos = Vector3.Transform(-pos, m); |
550 | inv.Translation = pos; | 550 | inv.Translation = pos; |
551 | return inv; | 551 | return inv; |
552 | } | 552 | } |
553 | 553 | ||
554 | public static Matrix GetDisplayMatrix(Matrix m) | 554 | public static Matrix GetDisplayMatrix(Matrix m) |
555 | { | 555 | { |
556 | Matrix displayMatrix = m; | 556 | Matrix displayMatrix = m; |
557 | displayMatrix.Translation = Vector3.Zero; | 557 | displayMatrix.Translation = Vector3.Zero; |
558 | displayMatrix = Matrix.Transpose(displayMatrix); | 558 | displayMatrix = Matrix.Transpose(displayMatrix); |
559 | displayMatrix.Translation = m.Translation; | 559 | displayMatrix.Translation = m.Translation; |
560 | return displayMatrix; | 560 | return displayMatrix; |
561 | } | 561 | } |
562 | 562 | ||
563 | internal static Vector3 Transform(Vector3 position, Matrix matrix) | 563 | internal static Vector3 Transform(Vector3 position, Matrix matrix) |
564 | { | 564 | { |
565 | Vector3 vector = new Vector3(); | 565 | Vector3 vector = new Vector3(); |
566 | vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)) + matrix.M41; | 566 | vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)) + matrix.M41; |
567 | vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23)) + matrix.M42; | 567 | vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23)) + matrix.M42; |
568 | vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)) + matrix.M43; | 568 | vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)) + matrix.M43; |
569 | return vector; | 569 | return vector; |
570 | } | 570 | } |
571 | 571 | ||
572 | internal static Vector3 TransformNormal(Vector3 position, Matrix matrix) | 572 | internal static Vector3 TransformNormal(Vector3 position, Matrix matrix) |
573 | { | 573 | { |
574 | Vector3 vector = new Vector3(); | 574 | Vector3 vector = new Vector3(); |
575 | vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)); | 575 | vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)); |
576 | vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23)); | 576 | vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23)); |
577 | vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)); | 577 | vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)); |
578 | return vector; | 578 | return vector; |
579 | } | 579 | } |
580 | } | 580 | } |
581 | } | 581 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs index 04e42d9..93a99f2 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs | |||
@@ -1,116 +1,116 @@ | |||
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 | internal static class MatrixOperations | 29 | internal static class MatrixOperations |
30 | { | 30 | { |
31 | public static void SetRotation(ref Matrix m, Quaternion q) | 31 | public static void SetRotation(ref Matrix m, Quaternion q) |
32 | { | 32 | { |
33 | float d = q.LengthSquared(); | 33 | float d = q.LengthSquared(); |
34 | BulletDebug.Assert(d != 0); | 34 | BulletDebug.Assert(d != 0); |
35 | float s = 2f / d; | 35 | float s = 2f / d; |
36 | float xs = q.X * s, ys = q.Y * s, zs = q.Z * s; | 36 | float xs = q.X * s, ys = q.Y * s, zs = q.Z * s; |
37 | float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs; | 37 | float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs; |
38 | float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs; | 38 | float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs; |
39 | float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs; | 39 | float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs; |
40 | m = new Matrix(1 - (yy + zz), xy - wz, xz + wy, 0, | 40 | m = new Matrix(1 - (yy + zz), xy - wz, xz + wy, 0, |
41 | xy + wz, 1 - (xx + zz), yz - wx, 0, | 41 | xy + wz, 1 - (xx + zz), yz - wx, 0, |
42 | xz - wy, yz + wx, 1 - (xx + yy), 0, | 42 | xz - wy, yz + wx, 1 - (xx + yy), 0, |
43 | m.M41, m.M42, m.M43, 1); | 43 | m.M41, m.M42, m.M43, 1); |
44 | } | 44 | } |
45 | 45 | ||
46 | public static Quaternion GetRotation(Matrix m) | 46 | public static Quaternion GetRotation(Matrix m) |
47 | { | 47 | { |
48 | Quaternion q = new Quaternion(); | 48 | Quaternion q = new Quaternion(); |
49 | 49 | ||
50 | float trace = m.M11 + m.M22 + m.M33; | 50 | float trace = m.M11 + m.M22 + m.M33; |
51 | 51 | ||
52 | if (trace > 0) | 52 | if (trace > 0) |
53 | { | 53 | { |
54 | float s = (float)Math.Sqrt(trace + 1); | 54 | float s = (float)Math.Sqrt(trace + 1); |
55 | q.W = s * 0.5f; | 55 | q.W = s * 0.5f; |
56 | s = 0.5f / s; | 56 | s = 0.5f / s; |
57 | 57 | ||
58 | q.X = (m.M32 - m.M23) * s; | 58 | q.X = (m.M32 - m.M23) * s; |
59 | q.Y = (m.M13 - m.M31) * s; | 59 | q.Y = (m.M13 - m.M31) * s; |
60 | q.Z = (m.M21 - m.M12) * s; | 60 | q.Z = (m.M21 - m.M12) * s; |
61 | } | 61 | } |
62 | else | 62 | else |
63 | { | 63 | { |
64 | int i = m.M11 < m.M22 ? | 64 | int i = m.M11 < m.M22 ? |
65 | (m.M22 < m.M33 ? 2 : 1) : | 65 | (m.M22 < m.M33 ? 2 : 1) : |
66 | (m.M11 < m.M33 ? 2 : 0); | 66 | (m.M11 < m.M33 ? 2 : 0); |
67 | int j = (i + 1) % 3; | 67 | int j = (i + 1) % 3; |
68 | int k = (i + 2) % 3; | 68 | int k = (i + 2) % 3; |
69 | 69 | ||
70 | float s = (float)Math.Sqrt(MathHelper.GetElement(m, i, i) - MathHelper.GetElement(m, j, j) - MathHelper.GetElement(m, k, k) + 1); | 70 | float s = (float)Math.Sqrt(MathHelper.GetElement(m, i, i) - MathHelper.GetElement(m, j, j) - MathHelper.GetElement(m, k, k) + 1); |
71 | MathHelper.SetElement(ref q, i, s * 0.5f); | 71 | MathHelper.SetElement(ref q, i, s * 0.5f); |
72 | s = 0.5f / s; | 72 | s = 0.5f / s; |
73 | 73 | ||
74 | q.W = (MathHelper.GetElement(m, k, j) - MathHelper.GetElement(m, j, k)) * s; | 74 | q.W = (MathHelper.GetElement(m, k, j) - MathHelper.GetElement(m, j, k)) * s; |
75 | MathHelper.SetElement(ref q, j, (MathHelper.GetElement(m, j, i) + MathHelper.GetElement(m, i, j)) * s); | 75 | MathHelper.SetElement(ref q, j, (MathHelper.GetElement(m, j, i) + MathHelper.GetElement(m, i, j)) * s); |
76 | MathHelper.SetElement(ref q, k, (MathHelper.GetElement(m, k, i) + MathHelper.GetElement(m, i, k)) * s); | 76 | MathHelper.SetElement(ref q, k, (MathHelper.GetElement(m, k, i) + MathHelper.GetElement(m, i, k)) * s); |
77 | } | 77 | } |
78 | 78 | ||
79 | return q; | 79 | return q; |
80 | } | 80 | } |
81 | 81 | ||
82 | public static Matrix Scaled(Matrix m, Vector3 v) | 82 | public static Matrix Scaled(Matrix m, Vector3 v) |
83 | { | 83 | { |
84 | return new Matrix( m.M11 * v.X, m.M12 * v.Y, m.M13 * v.Z, 0, | 84 | return new Matrix( m.M11 * v.X, m.M12 * v.Y, m.M13 * v.Z, 0, |
85 | m.M21 * v.X, m.M22 * v.Y, m.M23 * v.Z, 0, | 85 | m.M21 * v.X, m.M22 * v.Y, m.M23 * v.Z, 0, |
86 | m.M31 * v.X, m.M32 * v.Y, m.M33 * v.Z, 0, | 86 | m.M31 * v.X, m.M32 * v.Y, m.M33 * v.Z, 0, |
87 | 0, 0, 0, 1); | 87 | 0, 0, 0, 1); |
88 | } | 88 | } |
89 | 89 | ||
90 | public static Matrix Multiply(Matrix a, Matrix b) | 90 | public static Matrix Multiply(Matrix a, Matrix b) |
91 | { | 91 | { |
92 | /*return btMatrix3x3( | 92 | /*return btMatrix3x3( |
93 | m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]), | 93 | m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]), |
94 | m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]), | 94 | m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]), |
95 | m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));*/ | 95 | m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));*/ |
96 | return new Matrix( | 96 | return new Matrix( |
97 | Dot(b, 0, MathHelper.GetRow(a, 1)), Dot(b, 1, MathHelper.GetRow(a, 1)), Dot(b, 2, MathHelper.GetRow(a, 1)), 0, | 97 | Dot(b, 0, MathHelper.GetRow(a, 1)), Dot(b, 1, MathHelper.GetRow(a, 1)), Dot(b, 2, MathHelper.GetRow(a, 1)), 0, |
98 | Dot(b, 0, MathHelper.GetRow(a, 2)), Dot(b, 1, MathHelper.GetRow(a, 2)), Dot(b, 2, MathHelper.GetRow(a, 2)), 0, | 98 | Dot(b, 0, MathHelper.GetRow(a, 2)), Dot(b, 1, MathHelper.GetRow(a, 2)), Dot(b, 2, MathHelper.GetRow(a, 2)), 0, |
99 | Dot(b, 0, MathHelper.GetRow(a, 3)), Dot(b, 1, MathHelper.GetRow(a, 3)), Dot(b, 2, MathHelper.GetRow(a, 3)), 0, | 99 | Dot(b, 0, MathHelper.GetRow(a, 3)), Dot(b, 1, MathHelper.GetRow(a, 3)), Dot(b, 2, MathHelper.GetRow(a, 3)), 0, |
100 | 0, 0, 0, 1); | 100 | 0, 0, 0, 1); |
101 | } | 101 | } |
102 | 102 | ||
103 | public static float Dot(Matrix m, int c, Vector3 v) | 103 | public static float Dot(Matrix m, int c, Vector3 v) |
104 | { | 104 | { |
105 | return MathHelper.GetElement(m, 0, c) * v.X + MathHelper.GetElement(m, 1, c) * v.Y + MathHelper.GetElement(m, 2, c) * v.Z; | 105 | return MathHelper.GetElement(m, 0, c) * v.X + MathHelper.GetElement(m, 1, c) * v.Y + MathHelper.GetElement(m, 2, c) * v.Z; |
106 | } | 106 | } |
107 | 107 | ||
108 | public static Matrix Transpose(Matrix m) | 108 | public static Matrix Transpose(Matrix m) |
109 | { | 109 | { |
110 | return new Matrix( m.M11, m.M21, m.M31, 0, | 110 | return new Matrix( m.M11, m.M21, m.M31, 0, |
111 | m.M12, m.M22, m.M32, 0, | 111 | m.M12, m.M22, m.M32, 0, |
112 | m.M13, m.M23, m.M33, 0, | 112 | m.M13, m.M23, m.M33, 0, |
113 | 0, 0, 0, 1); | 113 | 0, 0, 0, 1); |
114 | } | 114 | } |
115 | } | 115 | } |
116 | } | 116 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs index 4039177..407a285 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs | |||
@@ -1,44 +1,44 @@ | |||
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 | public abstract class MotionState | 29 | public abstract class MotionState |
30 | { | 30 | { |
31 | /*public abstract Vector3 getWorldPosition(); | 31 | /*public abstract Vector3 getWorldPosition(); |
32 | public abstract Vector3 getWorldScaling(); | 32 | public abstract Vector3 getWorldScaling(); |
33 | public abstract Quaternion getWorldOrientation(); | 33 | public abstract Quaternion getWorldOrientation(); |
34 | 34 | ||
35 | public abstract void setWorldPosition(Vector3 worldPos); | 35 | public abstract void setWorldPosition(Vector3 worldPos); |
36 | public abstract void setWorldOrientation(Quaternion orn); | 36 | public abstract void setWorldOrientation(Quaternion orn); |
37 | 37 | ||
38 | public abstract void calculateWorldTransformations();*/ | 38 | public abstract void calculateWorldTransformations();*/ |
39 | 39 | ||
40 | public abstract void GetWorldTransform(out Matrix worldTransform); | 40 | public abstract void GetWorldTransform(out Matrix worldTransform); |
41 | //Bullet only calls the update of worldtransform for active objects | 41 | //Bullet only calls the update of worldtransform for active objects |
42 | public abstract void SetWorldTransform(Matrix worldTransform); | 42 | public abstract void SetWorldTransform(Matrix worldTransform); |
43 | } | 43 | } |
44 | } | 44 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs index 35c9fed..0f59e87 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs | |||
@@ -1,88 +1,88 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX.LinearMath | 26 | namespace XnaDevRu.BulletX.LinearMath |
27 | { | 27 | { |
28 | internal abstract class QuadWord | 28 | internal abstract class QuadWord |
29 | { | 29 | { |
30 | private float x; | 30 | private float x; |
31 | private float y; | 31 | private float y; |
32 | private float z; | 32 | private float z; |
33 | private float w; | 33 | private float w; |
34 | 34 | ||
35 | public float X { get { return x; } set { x = value; } } | 35 | public float X { get { return x; } set { x = value; } } |
36 | public float Y { get { return y; } set { y = value; } } | 36 | public float Y { get { return y; } set { y = value; } } |
37 | public float Z { get { return z; } set { z = value; } } | 37 | public float Z { get { return z; } set { z = value; } } |
38 | public float W { get { return w; } set { w = value; } } | 38 | public float W { get { return w; } set { w = value; } } |
39 | 39 | ||
40 | public QuadWord() { } | 40 | public QuadWord() { } |
41 | 41 | ||
42 | public QuadWord(float x, float y, float z, float w) | 42 | public QuadWord(float x, float y, float z, float w) |
43 | { | 43 | { |
44 | X = x; | 44 | X = x; |
45 | Y = y; | 45 | Y = y; |
46 | Z = z; | 46 | Z = z; |
47 | W = w; | 47 | W = w; |
48 | } | 48 | } |
49 | 49 | ||
50 | public QuadWord(float x, float y, float z) | 50 | public QuadWord(float x, float y, float z) |
51 | { | 51 | { |
52 | X = x; | 52 | X = x; |
53 | Y = y; | 53 | Y = y; |
54 | Z = z; | 54 | Z = z; |
55 | W = 0; | 55 | W = 0; |
56 | } | 56 | } |
57 | 57 | ||
58 | public void SetMax(QuadWord other) | 58 | public void SetMax(QuadWord other) |
59 | { | 59 | { |
60 | if (other.X > X) | 60 | if (other.X > X) |
61 | X = other.X; | 61 | X = other.X; |
62 | 62 | ||
63 | if (other.Y > Y) | 63 | if (other.Y > Y) |
64 | Y = other.Y; | 64 | Y = other.Y; |
65 | 65 | ||
66 | if (other.Z > Z) | 66 | if (other.Z > Z) |
67 | Z = other.Z; | 67 | Z = other.Z; |
68 | 68 | ||
69 | if (other.W > W) | 69 | if (other.W > W) |
70 | W = other.W; | 70 | W = other.W; |
71 | } | 71 | } |
72 | 72 | ||
73 | public void SetMin(QuadWord other) | 73 | public void SetMin(QuadWord other) |
74 | { | 74 | { |
75 | if (other.X < X) | 75 | if (other.X < X) |
76 | X = other.X; | 76 | X = other.X; |
77 | 77 | ||
78 | if (other.Y < Y) | 78 | if (other.Y < Y) |
79 | Y = other.Y; | 79 | Y = other.Y; |
80 | 80 | ||
81 | if (other.Z < Z) | 81 | if (other.Z < Z) |
82 | Z = other.Z; | 82 | Z = other.Z; |
83 | 83 | ||
84 | if (other.W < W) | 84 | if (other.W < W) |
85 | W = other.W; | 85 | W = other.W; |
86 | } | 86 | } |
87 | } | 87 | } |
88 | } | 88 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs index 5425a05..0685d4a 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs | |||
@@ -1,198 +1,198 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX.LinearMath | 26 | namespace XnaDevRu.BulletX.LinearMath |
27 | { | 27 | { |
28 | internal class Quaternion : QuadWord | 28 | internal class Quaternion : QuadWord |
29 | { | 29 | { |
30 | public Quaternion() { } | 30 | public Quaternion() { } |
31 | 31 | ||
32 | public Quaternion(float x, float y, float z, float w) | 32 | public Quaternion(float x, float y, float z, float w) |
33 | : base(x, y, z, w) { } | 33 | : base(x, y, z, w) { } |
34 | 34 | ||
35 | public Quaternion(Vector3 axis, float angle) | 35 | public Quaternion(Vector3 axis, float angle) |
36 | { | 36 | { |
37 | SetRotation(axis, angle); | 37 | SetRotation(axis, angle); |
38 | } | 38 | } |
39 | 39 | ||
40 | public Quaternion(float yaw, float pitch, float roll) | 40 | public Quaternion(float yaw, float pitch, float roll) |
41 | { | 41 | { |
42 | SetEuler(yaw, pitch, roll); | 42 | SetEuler(yaw, pitch, roll); |
43 | } | 43 | } |
44 | 44 | ||
45 | public void SetRotation(Vector3 axis, float angle) | 45 | public void SetRotation(Vector3 axis, float angle) |
46 | { | 46 | { |
47 | float d = axis.Length(); | 47 | float d = axis.Length(); |
48 | if (d == 0) throw new DivideByZeroException(); | 48 | if (d == 0) throw new DivideByZeroException(); |
49 | float s = (float)Math.Sin(angle * 0.5f) / d; | 49 | float s = (float)Math.Sin(angle * 0.5f) / d; |
50 | X = axis.X * s; | 50 | X = axis.X * s; |
51 | Y = axis.Y * s; | 51 | Y = axis.Y * s; |
52 | Z = axis.Z * s; | 52 | Z = axis.Z * s; |
53 | W = (float)Math.Cos(angle * 0.5f); | 53 | W = (float)Math.Cos(angle * 0.5f); |
54 | } | 54 | } |
55 | 55 | ||
56 | public void SetEuler(float yaw, float pitch, float roll) | 56 | public void SetEuler(float yaw, float pitch, float roll) |
57 | { | 57 | { |
58 | float halfYaw = yaw * 0.5f; | 58 | float halfYaw = yaw * 0.5f; |
59 | float halfPitch = pitch * 0.5f; | 59 | float halfPitch = pitch * 0.5f; |
60 | float halfRoll = roll * 0.5f; | 60 | float halfRoll = roll * 0.5f; |
61 | float cosYaw = (float)Math.Cos(halfYaw); | 61 | float cosYaw = (float)Math.Cos(halfYaw); |
62 | float sinYaw = (float)Math.Sin(halfYaw); | 62 | float sinYaw = (float)Math.Sin(halfYaw); |
63 | float cosPitch = (float)Math.Cos(halfPitch); | 63 | float cosPitch = (float)Math.Cos(halfPitch); |
64 | float sinPitch = (float)Math.Sin(halfPitch); | 64 | float sinPitch = (float)Math.Sin(halfPitch); |
65 | float cosRoll = (float)Math.Cos(halfRoll); | 65 | float cosRoll = (float)Math.Cos(halfRoll); |
66 | float sinRoll = (float)Math.Sin(halfRoll); | 66 | float sinRoll = (float)Math.Sin(halfRoll); |
67 | X = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; | 67 | X = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; |
68 | Y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw; | 68 | Y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw; |
69 | Z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw; | 69 | Z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw; |
70 | W = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw; | 70 | W = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw; |
71 | } | 71 | } |
72 | 72 | ||
73 | public float LengthSquared() | 73 | public float LengthSquared() |
74 | { | 74 | { |
75 | return Dot(this, this); | 75 | return Dot(this, this); |
76 | } | 76 | } |
77 | 77 | ||
78 | public float Length() | 78 | public float Length() |
79 | { | 79 | { |
80 | return (float)Math.Sqrt(LengthSquared()); | 80 | return (float)Math.Sqrt(LengthSquared()); |
81 | } | 81 | } |
82 | 82 | ||
83 | public float Angle() | 83 | public float Angle() |
84 | { | 84 | { |
85 | return 2f * (float)Math.Acos(W); | 85 | return 2f * (float)Math.Acos(W); |
86 | } | 86 | } |
87 | 87 | ||
88 | public static float Angle(Quaternion a, Quaternion b) | 88 | public static float Angle(Quaternion a, Quaternion b) |
89 | { | 89 | { |
90 | float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared()); | 90 | float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared()); |
91 | if (s == 0) throw new DivideByZeroException(); | 91 | if (s == 0) throw new DivideByZeroException(); |
92 | return (float)Math.Acos(Dot(a, b) / s); | 92 | return (float)Math.Acos(Dot(a, b) / s); |
93 | } | 93 | } |
94 | 94 | ||
95 | public static Quaternion Farthest(Quaternion a, Quaternion b) | 95 | public static Quaternion Farthest(Quaternion a, Quaternion b) |
96 | { | 96 | { |
97 | Quaternion diff, sum; | 97 | Quaternion diff, sum; |
98 | diff = a - b; | 98 | diff = a - b; |
99 | sum = a + b; | 99 | sum = a + b; |
100 | if (Dot(diff, diff) > Dot(sum, sum)) | 100 | if (Dot(diff, diff) > Dot(sum, sum)) |
101 | return b; | 101 | return b; |
102 | return -b; | 102 | return -b; |
103 | } | 103 | } |
104 | 104 | ||
105 | public static Quaternion Slerp(Quaternion a, Quaternion b, float c) | 105 | public static Quaternion Slerp(Quaternion a, Quaternion b, float c) |
106 | { | 106 | { |
107 | float theta = Angle(a, b); | 107 | float theta = Angle(a, b); |
108 | if (theta != 0) | 108 | if (theta != 0) |
109 | { | 109 | { |
110 | float d = 1f / (float)Math.Sin(theta); | 110 | float d = 1f / (float)Math.Sin(theta); |
111 | float s0 = (float)Math.Sin((1f - c) * theta); | 111 | float s0 = (float)Math.Sin((1f - c) * theta); |
112 | float s1 = (float)Math.Sin(c * theta); | 112 | float s1 = (float)Math.Sin(c * theta); |
113 | return new Quaternion( | 113 | return new Quaternion( |
114 | (a.X * s0 + b.X * s1) * d, | 114 | (a.X * s0 + b.X * s1) * d, |
115 | (a.Y * s0 + b.Y * s1) * d, | 115 | (a.Y * s0 + b.Y * s1) * d, |
116 | (a.Z * s0 + b.Z * s1) * d, | 116 | (a.Z * s0 + b.Z * s1) * d, |
117 | (a.W * s0 + b.W * s1) * d); | 117 | (a.W * s0 + b.W * s1) * d); |
118 | } | 118 | } |
119 | else | 119 | else |
120 | { | 120 | { |
121 | return a; | 121 | return a; |
122 | } | 122 | } |
123 | } | 123 | } |
124 | 124 | ||
125 | public static Quaternion Inverse(Quaternion a) | 125 | public static Quaternion Inverse(Quaternion a) |
126 | { | 126 | { |
127 | return new Quaternion(a.X, a.Y, a.Z, -a.W); | 127 | return new Quaternion(a.X, a.Y, a.Z, -a.W); |
128 | } | 128 | } |
129 | 129 | ||
130 | public static Quaternion Normalize(Quaternion a) | 130 | public static Quaternion Normalize(Quaternion a) |
131 | { | 131 | { |
132 | return a / a.Length(); | 132 | return a / a.Length(); |
133 | } | 133 | } |
134 | 134 | ||
135 | public static float Dot(Quaternion a, Quaternion b) | 135 | public static float Dot(Quaternion a, Quaternion b) |
136 | { | 136 | { |
137 | return a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W; | 137 | return a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W; |
138 | } | 138 | } |
139 | 139 | ||
140 | public static Quaternion operator +(Quaternion a, Quaternion b) | 140 | public static Quaternion operator +(Quaternion a, Quaternion b) |
141 | { | 141 | { |
142 | return new Quaternion(a.X + b.X, a.Y + b.Y, a.Z + b.Z, a.W + b.W); | 142 | return new Quaternion(a.X + b.X, a.Y + b.Y, a.Z + b.Z, a.W + b.W); |
143 | } | 143 | } |
144 | 144 | ||
145 | public static Quaternion operator -(Quaternion a, Quaternion b) | 145 | public static Quaternion operator -(Quaternion a, Quaternion b) |
146 | { | 146 | { |
147 | return new Quaternion(a.X - b.X, a.Y - b.Y, a.Z - b.Z, a.W - b.W); | 147 | return new Quaternion(a.X - b.X, a.Y - b.Y, a.Z - b.Z, a.W - b.W); |
148 | } | 148 | } |
149 | 149 | ||
150 | public static Quaternion operator -(Quaternion a) | 150 | public static Quaternion operator -(Quaternion a) |
151 | { | 151 | { |
152 | return new Quaternion(-a.X, -a.Y, -a.Z, -a.W); | 152 | return new Quaternion(-a.X, -a.Y, -a.Z, -a.W); |
153 | } | 153 | } |
154 | 154 | ||
155 | public static Quaternion operator *(Quaternion a, float b) | 155 | public static Quaternion operator *(Quaternion a, float b) |
156 | { | 156 | { |
157 | return new Quaternion(a.X * b, a.Y * b, a.Z * b, a.W * b); | 157 | return new Quaternion(a.X * b, a.Y * b, a.Z * b, a.W * b); |
158 | } | 158 | } |
159 | 159 | ||
160 | public static Quaternion operator *(Quaternion a, Quaternion b) | 160 | public static Quaternion operator *(Quaternion a, Quaternion b) |
161 | { | 161 | { |
162 | return new Quaternion( | 162 | return new Quaternion( |
163 | a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y, | 163 | a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y, |
164 | a.W * b.Y + a.Y * b.W + a.Z * b.X - a.X * b.Z, | 164 | a.W * b.Y + a.Y * b.W + a.Z * b.X - a.X * b.Z, |
165 | a.W * b.Z + a.Z * b.W + a.X * b.Y - a.Y * b.X, | 165 | a.W * b.Z + a.Z * b.W + a.X * b.Y - a.Y * b.X, |
166 | a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z); | 166 | a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z); |
167 | } | 167 | } |
168 | 168 | ||
169 | public static Quaternion operator *(Quaternion a, Vector3 b) | 169 | public static Quaternion operator *(Quaternion a, Vector3 b) |
170 | { | 170 | { |
171 | return new Quaternion( | 171 | return new Quaternion( |
172 | a.W * b.X + a.Y * b.Z - a.Z * b.Y, | 172 | a.W * b.X + a.Y * b.Z - a.Z * b.Y, |
173 | a.W * b.Y + a.Z * b.X - a.X * b.Z, | 173 | a.W * b.Y + a.Z * b.X - a.X * b.Z, |
174 | a.W * b.Z + a.X * b.Y - a.Y * b.X, | 174 | a.W * b.Z + a.X * b.Y - a.Y * b.X, |
175 | -a.X * b.X - a.Y * b.Y - a.Z * b.Z); | 175 | -a.X * b.X - a.Y * b.Y - a.Z * b.Z); |
176 | } | 176 | } |
177 | 177 | ||
178 | public static Quaternion operator *(Vector3 w, Quaternion q) | 178 | public static Quaternion operator *(Vector3 w, Quaternion q) |
179 | { | 179 | { |
180 | return new Quaternion( | 180 | return new Quaternion( |
181 | w.X * q.W + w.Y * q.Z - w.Z * q.Y, | 181 | w.X * q.W + w.Y * q.Z - w.Z * q.Y, |
182 | w.Y * q.W + w.Z * q.X - w.X * q.Z, | 182 | w.Y * q.W + w.Z * q.X - w.X * q.Z, |
183 | w.Z * q.W + w.X * q.Y - w.Y * q.X, | 183 | w.Z * q.W + w.X * q.Y - w.Y * q.X, |
184 | -w.X * q.X - w.Y * q.Y - w.Z * q.Z); | 184 | -w.X * q.X - w.Y * q.Y - w.Z * q.Z); |
185 | } | 185 | } |
186 | 186 | ||
187 | public static Quaternion operator /(Quaternion a, float b) | 187 | public static Quaternion operator /(Quaternion a, float b) |
188 | { | 188 | { |
189 | if (b == 0) throw new DivideByZeroException(); | 189 | if (b == 0) throw new DivideByZeroException(); |
190 | return new Quaternion(a.X / b, a.Y / b, a.Z / b, a.W / b); | 190 | return new Quaternion(a.X / b, a.Y / b, a.Z / b, a.W / b); |
191 | } | 191 | } |
192 | 192 | ||
193 | public static explicit operator MonoXnaCompactMaths.Quaternion(Quaternion a) | 193 | public static explicit operator MonoXnaCompactMaths.Quaternion(Quaternion a) |
194 | { | 194 | { |
195 | return new MonoXnaCompactMaths.Quaternion(a.X, a.Y, a.Z, a.W); | 195 | return new MonoXnaCompactMaths.Quaternion(a.X, a.Y, a.Z, a.W); |
196 | } | 196 | } |
197 | } | 197 | } |
198 | } | 198 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs index 031faab..ccf92a0 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs | |||
@@ -1,102 +1,102 @@ | |||
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 | public static class TransformUtil | 29 | public static class TransformUtil |
30 | { | 30 | { |
31 | //const float AngularMotionTreshold = 0.5f * MonoXnaCompactMaths.MathHelper.PiOver2; | 31 | //const float AngularMotionTreshold = 0.5f * MonoXnaCompactMaths.MathHelper.PiOver2; |
32 | const float AngularMotionTreshold = 0.5f * (float)Math.PI / 2.0f; | 32 | const float AngularMotionTreshold = 0.5f * (float)Math.PI / 2.0f; |
33 | 33 | ||
34 | public static void IntegrateTransform(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, ref Matrix predictedTransform) | 34 | public static void IntegrateTransform(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, ref Matrix predictedTransform) |
35 | { | 35 | { |
36 | predictedTransform.Translation = currentTransform.Translation + linearVelocity * timeStep; | 36 | predictedTransform.Translation = currentTransform.Translation + linearVelocity * timeStep; |
37 | //exponential map | 37 | //exponential map |
38 | Vector3 axis; | 38 | Vector3 axis; |
39 | float angle = angularVelocity.Length(); | 39 | float angle = angularVelocity.Length(); |
40 | //limit the angular motion | 40 | //limit the angular motion |
41 | if (angle * timeStep > AngularMotionTreshold) | 41 | if (angle * timeStep > AngularMotionTreshold) |
42 | { | 42 | { |
43 | angle = AngularMotionTreshold / timeStep; | 43 | angle = AngularMotionTreshold / timeStep; |
44 | } | 44 | } |
45 | 45 | ||
46 | if (angle < 0.001f) | 46 | if (angle < 0.001f) |
47 | { | 47 | { |
48 | // use Taylor's expansions of sync function | 48 | // use Taylor's expansions of sync function |
49 | axis = angularVelocity * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * angle * angle); | 49 | axis = angularVelocity * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * angle * angle); |
50 | } | 50 | } |
51 | else | 51 | else |
52 | { | 52 | { |
53 | // sync(fAngle) = sin(c*fAngle)/t | 53 | // sync(fAngle) = sin(c*fAngle)/t |
54 | axis = angularVelocity * ((float)Math.Sin(0.5f * angle * timeStep) / angle); | 54 | axis = angularVelocity * ((float)Math.Sin(0.5f * angle * timeStep) / angle); |
55 | } | 55 | } |
56 | Quaternion dorn = new Quaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * timeStep * 0.5f)); | 56 | Quaternion dorn = new Quaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * timeStep * 0.5f)); |
57 | Quaternion ornA = MatrixOperations.GetRotation(currentTransform); | 57 | Quaternion ornA = MatrixOperations.GetRotation(currentTransform); |
58 | 58 | ||
59 | Quaternion predictedOrn = dorn * ornA; | 59 | Quaternion predictedOrn = dorn * ornA; |
60 | predictedOrn.Normalize(); | 60 | predictedOrn.Normalize(); |
61 | 61 | ||
62 | MatrixOperations.SetRotation(ref predictedTransform, predictedOrn); | 62 | MatrixOperations.SetRotation(ref predictedTransform, predictedOrn); |
63 | 63 | ||
64 | Matrix test = Matrix.CreateFromQuaternion(predictedOrn); | 64 | Matrix test = Matrix.CreateFromQuaternion(predictedOrn); |
65 | } | 65 | } |
66 | 66 | ||
67 | public static void CalculateVelocity(Matrix transformA, Matrix transformB, float timeStep, ref Vector3 linearVelocity, ref Vector3 angularVelocity) | 67 | public static void CalculateVelocity(Matrix transformA, Matrix transformB, float timeStep, ref Vector3 linearVelocity, ref Vector3 angularVelocity) |
68 | { | 68 | { |
69 | linearVelocity = (transformB.Translation - transformA.Translation) / timeStep; | 69 | linearVelocity = (transformB.Translation - transformA.Translation) / timeStep; |
70 | Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); | 70 | Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); |
71 | Quaternion dorn = Quaternion.CreateFromRotationMatrix(dmat); | 71 | Quaternion dorn = Quaternion.CreateFromRotationMatrix(dmat); |
72 | 72 | ||
73 | Vector3 axis; | 73 | Vector3 axis; |
74 | float angle = 2 * (float)Math.Acos(dorn.W); | 74 | float angle = 2 * (float)Math.Acos(dorn.W); |
75 | axis = new Vector3(dorn.X, dorn.Y, dorn.Z); | 75 | axis = new Vector3(dorn.X, dorn.Y, dorn.Z); |
76 | //axis[3] = 0.f; | 76 | //axis[3] = 0.f; |
77 | //check for axis length | 77 | //check for axis length |
78 | float len = axis.LengthSquared(); | 78 | float len = axis.LengthSquared(); |
79 | if (len < MathHelper.Epsilon * MathHelper.Epsilon) | 79 | if (len < MathHelper.Epsilon * MathHelper.Epsilon) |
80 | axis = new Vector3(1f, 0f, 0f); | 80 | axis = new Vector3(1f, 0f, 0f); |
81 | else | 81 | else |
82 | axis /= (float)Math.Sqrt(len); | 82 | axis /= (float)Math.Sqrt(len); |
83 | 83 | ||
84 | angularVelocity = axis * angle / timeStep; | 84 | angularVelocity = axis * angle / timeStep; |
85 | } | 85 | } |
86 | 86 | ||
87 | public static void CalculateDiffAxisAngle(Matrix transformA, Matrix transformB, out Vector3 axis, out float angle) | 87 | public static void CalculateDiffAxisAngle(Matrix transformA, Matrix transformB, out Vector3 axis, out float angle) |
88 | { | 88 | { |
89 | Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); | 89 | Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); |
90 | Quaternion dorn = MathHelper.GetRotation(dmat); | 90 | Quaternion dorn = MathHelper.GetRotation(dmat); |
91 | 91 | ||
92 | angle = 2f * (float)Math.Acos(dorn.W); | 92 | angle = 2f * (float)Math.Acos(dorn.W); |
93 | axis = new Vector3(dorn.X, dorn.Y, dorn.Z); | 93 | axis = new Vector3(dorn.X, dorn.Y, dorn.Z); |
94 | //check for axis length | 94 | //check for axis length |
95 | float len = axis.LengthSquared(); | 95 | float len = axis.LengthSquared(); |
96 | if (len < MathHelper.Epsilon * MathHelper.Epsilon) | 96 | if (len < MathHelper.Epsilon * MathHelper.Epsilon) |
97 | axis = new Vector3(1f, 0f, 0f); | 97 | axis = new Vector3(1f, 0f, 0f); |
98 | else | 98 | else |
99 | axis /= (float)Math.Sqrt(len); | 99 | axis /= (float)Math.Sqrt(len); |
100 | } | 100 | } |
101 | } | 101 | } |
102 | } | 102 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs index 79b1262..adeec25 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs | |||
@@ -1,221 +1,221 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX.LinearMath | 26 | namespace XnaDevRu.BulletX.LinearMath |
27 | { | 27 | { |
28 | internal class Vector3 : QuadWord | 28 | internal class Vector3 : QuadWord |
29 | { | 29 | { |
30 | public Vector3() { } | 30 | public Vector3() { } |
31 | 31 | ||
32 | public Vector3(float x, float y, float z) | 32 | public Vector3(float x, float y, float z) |
33 | : base(x, y, z) { } | 33 | : base(x, y, z) { } |
34 | 34 | ||
35 | public void SetInterpolate3(Vector3 a, Vector3 b, float c) | 35 | public void SetInterpolate3(Vector3 a, Vector3 b, float c) |
36 | { | 36 | { |
37 | float s = 1.0f - c; | 37 | float s = 1.0f - c; |
38 | X = s * a.X + c * b.X; | 38 | X = s * a.X + c * b.X; |
39 | Y = s * a.Y + c * b.Y; | 39 | Y = s * a.Y + c * b.Y; |
40 | Z = s * a.Z + c * b.Z; | 40 | Z = s * a.Z + c * b.Z; |
41 | } | 41 | } |
42 | 42 | ||
43 | public float LengthSquared() | 43 | public float LengthSquared() |
44 | { | 44 | { |
45 | return Dot(this, this); | 45 | return Dot(this, this); |
46 | } | 46 | } |
47 | 47 | ||
48 | public float Length() | 48 | public float Length() |
49 | { | 49 | { |
50 | return (float)Math.Sqrt(LengthSquared()); | 50 | return (float)Math.Sqrt(LengthSquared()); |
51 | } | 51 | } |
52 | 52 | ||
53 | public int MinAxis() | 53 | public int MinAxis() |
54 | { | 54 | { |
55 | return X < Y ? (X < Z ? 0 : 2) : (Y < Z ? 1 : 2); | 55 | return X < Y ? (X < Z ? 0 : 2) : (Y < Z ? 1 : 2); |
56 | } | 56 | } |
57 | 57 | ||
58 | public int MaxAxis() | 58 | public int MaxAxis() |
59 | { | 59 | { |
60 | return X < Y ? (Y < Z ? 2 : 1) : (X < Z ? 2 : 0); | 60 | return X < Y ? (Y < Z ? 2 : 1) : (X < Z ? 2 : 0); |
61 | } | 61 | } |
62 | 62 | ||
63 | public int FurthestAxis() | 63 | public int FurthestAxis() |
64 | { | 64 | { |
65 | return Absolute(this).MinAxis(); | 65 | return Absolute(this).MinAxis(); |
66 | } | 66 | } |
67 | 67 | ||
68 | public int ClosestAxis() | 68 | public int ClosestAxis() |
69 | { | 69 | { |
70 | return Absolute(this).MaxAxis(); | 70 | return Absolute(this).MaxAxis(); |
71 | } | 71 | } |
72 | 72 | ||
73 | public Vector3 Rotate(Vector3 axis, float angle) | 73 | public Vector3 Rotate(Vector3 axis, float angle) |
74 | { | 74 | { |
75 | Vector3 o = axis * Dot(axis, this); | 75 | Vector3 o = axis * Dot(axis, this); |
76 | Vector3 x = this - o; | 76 | Vector3 x = this - o; |
77 | Vector3 y = Cross(axis, this); | 77 | Vector3 y = Cross(axis, this); |
78 | 78 | ||
79 | return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle)); | 79 | return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle)); |
80 | } | 80 | } |
81 | 81 | ||
82 | public static Vector3 Lerp(Vector3 a, Vector3 b, float c) | 82 | public static Vector3 Lerp(Vector3 a, Vector3 b, float c) |
83 | { | 83 | { |
84 | return new Vector3( | 84 | return new Vector3( |
85 | a.X + (b.X - a.X) * c, | 85 | a.X + (b.X - a.X) * c, |
86 | a.Y + (b.Y - a.Y) * c, | 86 | a.Y + (b.Y - a.Y) * c, |
87 | a.Z + (b.Z - a.Z) * c); | 87 | a.Z + (b.Z - a.Z) * c); |
88 | } | 88 | } |
89 | 89 | ||
90 | public static float Angle(Vector3 a, Vector3 b) | 90 | public static float Angle(Vector3 a, Vector3 b) |
91 | { | 91 | { |
92 | float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared()); | 92 | float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared()); |
93 | if (s == 0) throw new DivideByZeroException(); | 93 | if (s == 0) throw new DivideByZeroException(); |
94 | return (float)Math.Acos(Dot(a, b) / s); | 94 | return (float)Math.Acos(Dot(a, b) / s); |
95 | } | 95 | } |
96 | 96 | ||
97 | public static Vector3 Absolute(Vector3 a) | 97 | public static Vector3 Absolute(Vector3 a) |
98 | { | 98 | { |
99 | return new Vector3( | 99 | return new Vector3( |
100 | Math.Abs(a.X), | 100 | Math.Abs(a.X), |
101 | Math.Abs(a.Y), | 101 | Math.Abs(a.Y), |
102 | Math.Abs(a.Z)); | 102 | Math.Abs(a.Z)); |
103 | } | 103 | } |
104 | 104 | ||
105 | public static Vector3 Normalize(Vector3 a) | 105 | public static Vector3 Normalize(Vector3 a) |
106 | { | 106 | { |
107 | return a / a.Length(); | 107 | return a / a.Length(); |
108 | } | 108 | } |
109 | 109 | ||
110 | public static Vector3 Cross(Vector3 a, Vector3 b) | 110 | public static Vector3 Cross(Vector3 a, Vector3 b) |
111 | { | 111 | { |
112 | return new Vector3( | 112 | return new Vector3( |
113 | a.Y * b.Z - a.Z * b.Y, | 113 | a.Y * b.Z - a.Z * b.Y, |
114 | a.Z * b.X - a.X * b.Z, | 114 | a.Z * b.X - a.X * b.Z, |
115 | a.X * b.Y - a.Y * b.X); | 115 | a.X * b.Y - a.Y * b.X); |
116 | } | 116 | } |
117 | 117 | ||
118 | public static float Dot(Vector3 a, Vector3 b) | 118 | public static float Dot(Vector3 a, Vector3 b) |
119 | { | 119 | { |
120 | return a.X * b.X + a.Y * b.Y + a.Z * b.Z; | 120 | return a.X * b.X + a.Y * b.Y + a.Z * b.Z; |
121 | } | 121 | } |
122 | 122 | ||
123 | public static float Triple(Vector3 a, Vector3 b, Vector3 c) | 123 | public static float Triple(Vector3 a, Vector3 b, Vector3 c) |
124 | { | 124 | { |
125 | return a.X * (b.Y * c.Z - b.Z * c.Y) + | 125 | return a.X * (b.Y * c.Z - b.Z * c.Y) + |
126 | a.Y * (b.Z * c.X - b.X * c.Z) + | 126 | a.Y * (b.Z * c.X - b.X * c.Z) + |
127 | a.Z * (b.X * c.Y - b.Y * c.X); | 127 | a.Z * (b.X * c.Y - b.Y * c.X); |
128 | } | 128 | } |
129 | 129 | ||
130 | public static float Distance(Vector3 a, Vector3 b) | 130 | public static float Distance(Vector3 a, Vector3 b) |
131 | { | 131 | { |
132 | return (b - a).Length(); | 132 | return (b - a).Length(); |
133 | } | 133 | } |
134 | 134 | ||
135 | public static float DistanceSquared(Vector3 a, Vector3 b) | 135 | public static float DistanceSquared(Vector3 a, Vector3 b) |
136 | { | 136 | { |
137 | return (b - a).LengthSquared(); | 137 | return (b - a).LengthSquared(); |
138 | } | 138 | } |
139 | 139 | ||
140 | public static Vector3 Rotate(Vector3 a, Vector3 axis, float angle) | 140 | public static Vector3 Rotate(Vector3 a, Vector3 axis, float angle) |
141 | { | 141 | { |
142 | Vector3 o = axis * Dot(axis, a); | 142 | Vector3 o = axis * Dot(axis, a); |
143 | Vector3 x = a - o; | 143 | Vector3 x = a - o; |
144 | Vector3 y = Cross(axis, a); | 144 | Vector3 y = Cross(axis, a); |
145 | 145 | ||
146 | return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle)); | 146 | return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle)); |
147 | } | 147 | } |
148 | 148 | ||
149 | public static Vector3 operator +(Vector3 a, Vector3 b) | 149 | public static Vector3 operator +(Vector3 a, Vector3 b) |
150 | { | 150 | { |
151 | return new Vector3(a.X + b.X, a.Y + b.Y, a.Z + b.Z); | 151 | return new Vector3(a.X + b.X, a.Y + b.Y, a.Z + b.Z); |
152 | } | 152 | } |
153 | 153 | ||
154 | public static Vector3 operator -(Vector3 a, Vector3 b) | 154 | public static Vector3 operator -(Vector3 a, Vector3 b) |
155 | { | 155 | { |
156 | return new Vector3(a.X - b.X, a.Y - b.Y, a.Z - b.Z); | 156 | return new Vector3(a.X - b.X, a.Y - b.Y, a.Z - b.Z); |
157 | } | 157 | } |
158 | 158 | ||
159 | public static Vector3 operator -(Vector3 a) | 159 | public static Vector3 operator -(Vector3 a) |
160 | { | 160 | { |
161 | return new Vector3(-a.X, -a.Y, -a.Z); | 161 | return new Vector3(-a.X, -a.Y, -a.Z); |
162 | } | 162 | } |
163 | 163 | ||
164 | public static Vector3 operator *(float b, Vector3 a) | 164 | public static Vector3 operator *(float b, Vector3 a) |
165 | { | 165 | { |
166 | return new Vector3(a.X * b, a.Y * b, a.Z * b); | 166 | return new Vector3(a.X * b, a.Y * b, a.Z * b); |
167 | } | 167 | } |
168 | 168 | ||
169 | public static Vector3 operator *(Vector3 a, float b) | 169 | public static Vector3 operator *(Vector3 a, float b) |
170 | { | 170 | { |
171 | return new Vector3(a.X * b, a.Y * b, a.Z * b); | 171 | return new Vector3(a.X * b, a.Y * b, a.Z * b); |
172 | } | 172 | } |
173 | 173 | ||
174 | public static Vector3 operator *(Vector3 a, Vector3 b) | 174 | public static Vector3 operator *(Vector3 a, Vector3 b) |
175 | { | 175 | { |
176 | return new Vector3(a.X * b.X, a.Y * b.Y, a.Z * b.Z); | 176 | return new Vector3(a.X * b.X, a.Y * b.Y, a.Z * b.Z); |
177 | } | 177 | } |
178 | 178 | ||
179 | public static Vector3 operator /(Vector3 a, float b) | 179 | public static Vector3 operator /(Vector3 a, float b) |
180 | { | 180 | { |
181 | if (b == 0) throw new DivideByZeroException(); | 181 | if (b == 0) throw new DivideByZeroException(); |
182 | return new Vector3(a.X / b, a.Y / b, a.Z / b); | 182 | return new Vector3(a.X / b, a.Y / b, a.Z / b); |
183 | } | 183 | } |
184 | 184 | ||
185 | public static Vector3 operator /(Vector3 a, Vector3 b) | 185 | public static Vector3 operator /(Vector3 a, Vector3 b) |
186 | { | 186 | { |
187 | if (b.X == 0 || b.Y == 0 || b.Z == 0) throw new DivideByZeroException(); | 187 | if (b.X == 0 || b.Y == 0 || b.Z == 0) throw new DivideByZeroException(); |
188 | return new Vector3(a.X / b.X, a.Y / b.Y, a.Z / b.Z); | 188 | return new Vector3(a.X / b.X, a.Y / b.Y, a.Z / b.Z); |
189 | } | 189 | } |
190 | 190 | ||
191 | public static bool operator ==(Vector3 a, Vector3 b) | 191 | public static bool operator ==(Vector3 a, Vector3 b) |
192 | { | 192 | { |
193 | return a.X == b.X && a.Y == b.Y && a.Z == b.Z; | 193 | return a.X == b.X && a.Y == b.Y && a.Z == b.Z; |
194 | } | 194 | } |
195 | 195 | ||
196 | public static bool operator !=(Vector3 a, Vector3 b) | 196 | public static bool operator !=(Vector3 a, Vector3 b) |
197 | { | 197 | { |
198 | return a.X != b.X || a.Y != b.Y || a.Z != b.Z; | 198 | return a.X != b.X || a.Y != b.Y || a.Z != b.Z; |
199 | } | 199 | } |
200 | 200 | ||
201 | public static explicit operator MonoXnaCompactMaths.Vector3(Vector3 a) | 201 | public static explicit operator MonoXnaCompactMaths.Vector3(Vector3 a) |
202 | { | 202 | { |
203 | return new MonoXnaCompactMaths.Vector3(a.X, a.Y, a.Z); | 203 | return new MonoXnaCompactMaths.Vector3(a.X, a.Y, a.Z); |
204 | } | 204 | } |
205 | 205 | ||
206 | public override bool Equals(object obj) | 206 | public override bool Equals(object obj) |
207 | { | 207 | { |
208 | return object.Equals(this, obj); | 208 | return object.Equals(this, obj); |
209 | } | 209 | } |
210 | 210 | ||
211 | public override int GetHashCode() | 211 | public override int GetHashCode() |
212 | { | 212 | { |
213 | return X.GetHashCode() & Y.GetHashCode() & Z.GetHashCode(); | 213 | return X.GetHashCode() & Y.GetHashCode() & Z.GetHashCode(); |
214 | } | 214 | } |
215 | 215 | ||
216 | public override string ToString() | 216 | public override string ToString() |
217 | { | 217 | { |
218 | return string.Format("{0}, {1}, {2}", X, Y, Z); | 218 | return string.Format("{0}, {1}, {2}", X, Y, Z); |
219 | } | 219 | } |
220 | } | 220 | } |
221 | } | 221 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs index a64ce97..de98ccd 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs | |||
@@ -1,110 +1,110 @@ | |||
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 | 25 | ||
26 | namespace XnaDevRu.BulletX.LinearMath | 26 | namespace XnaDevRu.BulletX.LinearMath |
27 | { | 27 | { |
28 | internal class Vector4 : Vector3 | 28 | internal class Vector4 : Vector3 |
29 | { | 29 | { |
30 | public Vector4() { } | 30 | public Vector4() { } |
31 | 31 | ||
32 | public Vector4(float x, float y, float z, float w) | 32 | public Vector4(float x, float y, float z, float w) |
33 | : base(x, y, z) { W = w; } | 33 | : base(x, y, z) { W = w; } |
34 | 34 | ||
35 | public static Vector4 Absolute4(Vector4 a) | 35 | public static Vector4 Absolute4(Vector4 a) |
36 | { | 36 | { |
37 | return new Vector4( | 37 | return new Vector4( |
38 | Math.Abs(a.X), | 38 | Math.Abs(a.X), |
39 | Math.Abs(a.Y), | 39 | Math.Abs(a.Y), |
40 | Math.Abs(a.Z), | 40 | Math.Abs(a.Z), |
41 | Math.Abs(a.W)); | 41 | Math.Abs(a.W)); |
42 | } | 42 | } |
43 | 43 | ||
44 | public int MaxAxis4() | 44 | public int MaxAxis4() |
45 | { | 45 | { |
46 | int maxIndex = -1; | 46 | int maxIndex = -1; |
47 | float maxVal = -1e30f; | 47 | float maxVal = -1e30f; |
48 | if (X > maxVal) | 48 | if (X > maxVal) |
49 | { | 49 | { |
50 | maxIndex = 0; | 50 | maxIndex = 0; |
51 | maxVal = X; | 51 | maxVal = X; |
52 | } | 52 | } |
53 | if (Y > maxVal) | 53 | if (Y > maxVal) |
54 | { | 54 | { |
55 | maxIndex = 1; | 55 | maxIndex = 1; |
56 | maxVal = Y; | 56 | maxVal = Y; |
57 | } | 57 | } |
58 | if (Z > maxVal) | 58 | if (Z > maxVal) |
59 | { | 59 | { |
60 | maxIndex = 2; | 60 | maxIndex = 2; |
61 | maxVal = Z; | 61 | maxVal = Z; |
62 | } | 62 | } |
63 | if (W > maxVal) | 63 | if (W > maxVal) |
64 | { | 64 | { |
65 | maxIndex = 3; | 65 | maxIndex = 3; |
66 | maxVal = W; | 66 | maxVal = W; |
67 | } | 67 | } |
68 | 68 | ||
69 | return maxIndex; | 69 | return maxIndex; |
70 | } | 70 | } |
71 | 71 | ||
72 | public int MinAxis4() | 72 | public int MinAxis4() |
73 | { | 73 | { |
74 | int minIndex = -1; | 74 | int minIndex = -1; |
75 | float minVal = 1e30f; | 75 | float minVal = 1e30f; |
76 | if (X < minVal) | 76 | if (X < minVal) |
77 | { | 77 | { |
78 | minIndex = 0; | 78 | minIndex = 0; |
79 | minVal = X; | 79 | minVal = X; |
80 | } | 80 | } |
81 | if (Y < minVal) | 81 | if (Y < minVal) |
82 | { | 82 | { |
83 | minIndex = 1; | 83 | minIndex = 1; |
84 | minVal = Y; | 84 | minVal = Y; |
85 | } | 85 | } |
86 | if (Z < minVal) | 86 | if (Z < minVal) |
87 | { | 87 | { |
88 | minIndex = 2; | 88 | minIndex = 2; |
89 | minVal = Z; | 89 | minVal = Z; |
90 | } | 90 | } |
91 | if (W < minVal) | 91 | if (W < minVal) |
92 | { | 92 | { |
93 | minIndex = 3; | 93 | minIndex = 3; |
94 | minVal = W; | 94 | minVal = W; |
95 | } | 95 | } |
96 | 96 | ||
97 | return minIndex; | 97 | return minIndex; |
98 | } | 98 | } |
99 | 99 | ||
100 | public int ClosestAxis4() | 100 | public int ClosestAxis4() |
101 | { | 101 | { |
102 | return Absolute4(this).MaxAxis4(); | 102 | return Absolute4(this).MaxAxis4(); |
103 | } | 103 | } |
104 | 104 | ||
105 | public static explicit operator MonoXnaCompactMaths.Vector4(Vector4 a) | 105 | public static explicit operator MonoXnaCompactMaths.Vector4(Vector4 a) |
106 | { | 106 | { |
107 | return new MonoXnaCompactMaths.Vector4(a.X, a.Y, a.Z, a.W); | 107 | return new MonoXnaCompactMaths.Vector4(a.X, a.Y, a.Z, a.W); |
108 | } | 108 | } |
109 | } | 109 | } |
110 | } | 110 | } |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj index bed8d19..06b98bc 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj | |||
@@ -1,168 +1,168 @@ | |||
1 | <Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> | 1 | <Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> |
2 | <PropertyGroup> | 2 | <PropertyGroup> |
3 | <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration> | 3 | <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration> |
4 | <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform> | 4 | <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform> |
5 | <ProductVersion>8.0.50727</ProductVersion> | 5 | <ProductVersion>8.0.50727</ProductVersion> |
6 | <SchemaVersion>2.0</SchemaVersion> | 6 | <SchemaVersion>2.0</SchemaVersion> |
7 | <ProjectGuid>{44270344-ACA7-4875-B585-81D5C06D0489}</ProjectGuid> | 7 | <ProjectGuid>{44270344-ACA7-4875-B585-81D5C06D0489}</ProjectGuid> |
8 | <OutputType>Library</OutputType> | 8 | <OutputType>Library</OutputType> |
9 | <AppDesignerFolder>Properties</AppDesignerFolder> | 9 | <AppDesignerFolder>Properties</AppDesignerFolder> |
10 | <RootNamespace>XnaDevRu.BulletX</RootNamespace> | 10 | <RootNamespace>XnaDevRu.BulletX</RootNamespace> |
11 | <AssemblyName>Modified.XnaDevRu.BulletX</AssemblyName> | 11 | <AssemblyName>Modified.XnaDevRu.BulletX</AssemblyName> |
12 | </PropertyGroup> | 12 | </PropertyGroup> |
13 | <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' "> | 13 | <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' "> |
14 | <DebugSymbols>true</DebugSymbols> | 14 | <DebugSymbols>true</DebugSymbols> |
15 | <DebugType>full</DebugType> | 15 | <DebugType>full</DebugType> |
16 | <Optimize>false</Optimize> | 16 | <Optimize>false</Optimize> |
17 | <OutputPath>bin\Debug\</OutputPath> | 17 | <OutputPath>bin\Debug\</OutputPath> |
18 | <DefineConstants>DEBUG;TRACE</DefineConstants> | 18 | <DefineConstants>DEBUG;TRACE</DefineConstants> |
19 | <ErrorReport>prompt</ErrorReport> | 19 | <ErrorReport>prompt</ErrorReport> |
20 | <WarningLevel>4</WarningLevel> | 20 | <WarningLevel>4</WarningLevel> |
21 | </PropertyGroup> | 21 | </PropertyGroup> |
22 | <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' "> | 22 | <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' "> |
23 | <DebugType>pdbonly</DebugType> | 23 | <DebugType>pdbonly</DebugType> |
24 | <Optimize>true</Optimize> | 24 | <Optimize>true</Optimize> |
25 | <OutputPath>bin\Release\</OutputPath> | 25 | <OutputPath>bin\Release\</OutputPath> |
26 | <DefineConstants>TRACE</DefineConstants> | 26 | <DefineConstants>TRACE</DefineConstants> |
27 | <ErrorReport>prompt</ErrorReport> | 27 | <ErrorReport>prompt</ErrorReport> |
28 | <WarningLevel>4</WarningLevel> | 28 | <WarningLevel>4</WarningLevel> |
29 | </PropertyGroup> | 29 | </PropertyGroup> |
30 | <ItemGroup> | 30 | <ItemGroup> |
31 | <Reference Include="System" /> | 31 | <Reference Include="System" /> |
32 | </ItemGroup> | 32 | </ItemGroup> |
33 | <ItemGroup> | 33 | <ItemGroup> |
34 | <Compile Include="BulletDebug.cs" /> | 34 | <Compile Include="BulletDebug.cs" /> |
35 | <Compile Include="Collision\BroadphaseCollision\AxisSweep3.cs" /> | 35 | <Compile Include="Collision\BroadphaseCollision\AxisSweep3.cs" /> |
36 | <Compile Include="Collision\BroadphaseCollision\BroadphaseNativeTypes.cs" /> | 36 | <Compile Include="Collision\BroadphaseCollision\BroadphaseNativeTypes.cs" /> |
37 | <Compile Include="Collision\BroadphaseCollision\BroadphasePair.cs" /> | 37 | <Compile Include="Collision\BroadphaseCollision\BroadphasePair.cs" /> |
38 | <Compile Include="Collision\BroadphaseCollision\BroadphaseProxy.cs" /> | 38 | <Compile Include="Collision\BroadphaseCollision\BroadphaseProxy.cs" /> |
39 | <Compile Include="Collision\BroadphaseCollision\CollisionAlgorithm.cs" /> | 39 | <Compile Include="Collision\BroadphaseCollision\CollisionAlgorithm.cs" /> |
40 | <Compile Include="Collision\BroadphaseCollision\CollisionAlgorithmConstructionInfo.cs" /> | 40 | <Compile Include="Collision\BroadphaseCollision\CollisionAlgorithmConstructionInfo.cs" /> |
41 | <Compile Include="Collision\BroadphaseCollision\DispatcherInfo.cs" /> | 41 | <Compile Include="Collision\BroadphaseCollision\DispatcherInfo.cs" /> |
42 | <Compile Include="Collision\BroadphaseCollision\IBroadphase.cs" /> | 42 | <Compile Include="Collision\BroadphaseCollision\IBroadphase.cs" /> |
43 | <Compile Include="Collision\BroadphaseCollision\IDispatcher.cs" /> | 43 | <Compile Include="Collision\BroadphaseCollision\IDispatcher.cs" /> |
44 | <Compile Include="Collision\BroadphaseCollision\IOverlapCallback.cs" /> | 44 | <Compile Include="Collision\BroadphaseCollision\IOverlapCallback.cs" /> |
45 | <Compile Include="Collision\BroadphaseCollision\OverlappingPairCache.cs" /> | 45 | <Compile Include="Collision\BroadphaseCollision\OverlappingPairCache.cs" /> |
46 | <Compile Include="Collision\BroadphaseCollision\SimpleBroadphase.cs" /> | 46 | <Compile Include="Collision\BroadphaseCollision\SimpleBroadphase.cs" /> |
47 | <Compile Include="Collision\BroadphaseCollision\SimpleBroadphaseProxy.cs" /> | 47 | <Compile Include="Collision\BroadphaseCollision\SimpleBroadphaseProxy.cs" /> |
48 | <Compile Include="Collision\CollisionDispatch\BridgeTriangleRaycastCallback.cs" /> | 48 | <Compile Include="Collision\CollisionDispatch\BridgeTriangleRaycastCallback.cs" /> |
49 | <Compile Include="Collision\CollisionDispatch\CollisionAlgorithmCreateFunc.cs" /> | 49 | <Compile Include="Collision\CollisionDispatch\CollisionAlgorithmCreateFunc.cs" /> |
50 | <Compile Include="Collision\CollisionDispatch\CollisionDispatcher.cs" /> | 50 | <Compile Include="Collision\CollisionDispatch\CollisionDispatcher.cs" /> |
51 | <Compile Include="Collision\CollisionDispatch\CollisionObject.cs" /> | 51 | <Compile Include="Collision\CollisionDispatch\CollisionObject.cs" /> |
52 | <Compile Include="Collision\CollisionDispatch\CollisionPairCallback.cs" /> | 52 | <Compile Include="Collision\CollisionDispatch\CollisionPairCallback.cs" /> |
53 | <Compile Include="Collision\CollisionDispatch\CollisionWorld.cs" /> | 53 | <Compile Include="Collision\CollisionDispatch\CollisionWorld.cs" /> |
54 | <Compile Include="Collision\CollisionDispatch\CompoundCollisionAlgorithm.cs" /> | 54 | <Compile Include="Collision\CollisionDispatch\CompoundCollisionAlgorithm.cs" /> |
55 | <Compile Include="Collision\CollisionDispatch\ConvexConcaveCollisionAlgorithm.cs" /> | 55 | <Compile Include="Collision\CollisionDispatch\ConvexConcaveCollisionAlgorithm.cs" /> |
56 | <Compile Include="Collision\CollisionDispatch\ConvexConvexCollisionAlgorithm.cs" /> | 56 | <Compile Include="Collision\CollisionDispatch\ConvexConvexCollisionAlgorithm.cs" /> |
57 | <Compile Include="Collision\CollisionDispatch\ConvexTriangleCallback.cs" /> | 57 | <Compile Include="Collision\CollisionDispatch\ConvexTriangleCallback.cs" /> |
58 | <Compile Include="Collision\CollisionDispatch\EmptyAlgorithm.cs" /> | 58 | <Compile Include="Collision\CollisionDispatch\EmptyAlgorithm.cs" /> |
59 | <Compile Include="Collision\CollisionDispatch\ManifoldResult.cs" /> | 59 | <Compile Include="Collision\CollisionDispatch\ManifoldResult.cs" /> |
60 | <Compile Include="Collision\CollisionDispatch\SimulationIslandManager.cs" /> | 60 | <Compile Include="Collision\CollisionDispatch\SimulationIslandManager.cs" /> |
61 | <Compile Include="Collision\CollisionDispatch\SphereBoxCollisionAlgorithm.cs" /> | 61 | <Compile Include="Collision\CollisionDispatch\SphereBoxCollisionAlgorithm.cs" /> |
62 | <Compile Include="Collision\CollisionDispatch\SphereSphereCollisionAlgorithm.cs" /> | 62 | <Compile Include="Collision\CollisionDispatch\SphereSphereCollisionAlgorithm.cs" /> |
63 | <Compile Include="Collision\CollisionDispatch\SphereTriangleCollisionAlgorithm.cs" /> | 63 | <Compile Include="Collision\CollisionDispatch\SphereTriangleCollisionAlgorithm.cs" /> |
64 | <Compile Include="Collision\CollisionDispatch\SphereTriangleDetector.cs" /> | 64 | <Compile Include="Collision\CollisionDispatch\SphereTriangleDetector.cs" /> |
65 | <Compile Include="Collision\CollisionDispatch\UnionFind.cs" /> | 65 | <Compile Include="Collision\CollisionDispatch\UnionFind.cs" /> |
66 | <Compile Include="Collision\CollisionShapes\BoxShape.cs" /> | 66 | <Compile Include="Collision\CollisionShapes\BoxShape.cs" /> |
67 | <Compile Include="Collision\CollisionShapes\BUSimplex1to4.cs" /> | 67 | <Compile Include="Collision\CollisionShapes\BUSimplex1to4.cs" /> |
68 | <Compile Include="Collision\CollisionShapes\BvhTriangleMeshShape.cs" /> | 68 | <Compile Include="Collision\CollisionShapes\BvhTriangleMeshShape.cs" /> |
69 | <Compile Include="Collision\CollisionShapes\CollisionShape.cs" /> | 69 | <Compile Include="Collision\CollisionShapes\CollisionShape.cs" /> |
70 | <Compile Include="Collision\CollisionShapes\CompoundShape.cs" /> | 70 | <Compile Include="Collision\CollisionShapes\CompoundShape.cs" /> |
71 | <Compile Include="Collision\CollisionShapes\ConcaveShape.cs" /> | 71 | <Compile Include="Collision\CollisionShapes\ConcaveShape.cs" /> |
72 | <Compile Include="Collision\CollisionShapes\ConeShape.cs" /> | 72 | <Compile Include="Collision\CollisionShapes\ConeShape.cs" /> |
73 | <Compile Include="Collision\CollisionShapes\ConvexHullShape.cs" /> | 73 | <Compile Include="Collision\CollisionShapes\ConvexHullShape.cs" /> |
74 | <Compile Include="Collision\CollisionShapes\ConvexShape.cs" /> | 74 | <Compile Include="Collision\CollisionShapes\ConvexShape.cs" /> |
75 | <Compile Include="Collision\CollisionShapes\ConvexTriangleMeshShape.cs" /> | 75 | <Compile Include="Collision\CollisionShapes\ConvexTriangleMeshShape.cs" /> |
76 | <Compile Include="Collision\CollisionShapes\CylinderShape.cs" /> | 76 | <Compile Include="Collision\CollisionShapes\CylinderShape.cs" /> |
77 | <Compile Include="Collision\CollisionShapes\CylinderShapeX.cs" /> | 77 | <Compile Include="Collision\CollisionShapes\CylinderShapeX.cs" /> |
78 | <Compile Include="Collision\CollisionShapes\CylinderShapeZ.cs" /> | 78 | <Compile Include="Collision\CollisionShapes\CylinderShapeZ.cs" /> |
79 | <Compile Include="Collision\CollisionShapes\EmptyShape.cs" /> | 79 | <Compile Include="Collision\CollisionShapes\EmptyShape.cs" /> |
80 | <Compile Include="Collision\CollisionShapes\FilteredCallback.cs" /> | 80 | <Compile Include="Collision\CollisionShapes\FilteredCallback.cs" /> |
81 | <Compile Include="Collision\CollisionShapes\InternalTriangleIndexCallback.cs" /> | 81 | <Compile Include="Collision\CollisionShapes\InternalTriangleIndexCallback.cs" /> |
82 | <Compile Include="Collision\CollisionShapes\LocalSupportVertexCallback.cs" /> | 82 | <Compile Include="Collision\CollisionShapes\LocalSupportVertexCallback.cs" /> |
83 | <Compile Include="Collision\CollisionShapes\MinkowskiSumShape.cs" /> | 83 | <Compile Include="Collision\CollisionShapes\MinkowskiSumShape.cs" /> |
84 | <Compile Include="Collision\CollisionShapes\MultiSphereShape.cs" /> | 84 | <Compile Include="Collision\CollisionShapes\MultiSphereShape.cs" /> |
85 | <Compile Include="Collision\CollisionShapes\NodeOverlapCallback.cs" /> | 85 | <Compile Include="Collision\CollisionShapes\NodeOverlapCallback.cs" /> |
86 | <Compile Include="Collision\CollisionShapes\OptimizedBvh.cs" /> | 86 | <Compile Include="Collision\CollisionShapes\OptimizedBvh.cs" /> |
87 | <Compile Include="Collision\CollisionShapes\OptimizedBvhNode.cs" /> | 87 | <Compile Include="Collision\CollisionShapes\OptimizedBvhNode.cs" /> |
88 | <Compile Include="Collision\CollisionShapes\PolyhedralConvexShape.cs" /> | 88 | <Compile Include="Collision\CollisionShapes\PolyhedralConvexShape.cs" /> |
89 | <Compile Include="Collision\CollisionShapes\SphereShape.cs" /> | 89 | <Compile Include="Collision\CollisionShapes\SphereShape.cs" /> |
90 | <Compile Include="Collision\CollisionShapes\StaticPlaneShape.cs" /> | 90 | <Compile Include="Collision\CollisionShapes\StaticPlaneShape.cs" /> |
91 | <Compile Include="Collision\CollisionShapes\StridingMeshInterface.cs" /> | 91 | <Compile Include="Collision\CollisionShapes\StridingMeshInterface.cs" /> |
92 | <Compile Include="Collision\CollisionShapes\SupportVertexCallback.cs" /> | 92 | <Compile Include="Collision\CollisionShapes\SupportVertexCallback.cs" /> |
93 | <Compile Include="Collision\CollisionShapes\TriangleBuffer.cs" /> | 93 | <Compile Include="Collision\CollisionShapes\TriangleBuffer.cs" /> |
94 | <Compile Include="Collision\CollisionShapes\TriangleCallback.cs" /> | 94 | <Compile Include="Collision\CollisionShapes\TriangleCallback.cs" /> |
95 | <Compile Include="Collision\CollisionShapes\TriangleIndexVertexArray.cs" /> | 95 | <Compile Include="Collision\CollisionShapes\TriangleIndexVertexArray.cs" /> |
96 | <Compile Include="Collision\CollisionShapes\TriangleMesh.cs" /> | 96 | <Compile Include="Collision\CollisionShapes\TriangleMesh.cs" /> |
97 | <Compile Include="Collision\CollisionShapes\TriangleMeshShape.cs" /> | 97 | <Compile Include="Collision\CollisionShapes\TriangleMeshShape.cs" /> |
98 | <Compile Include="Collision\CollisionShapes\TriangleShape.cs" /> | 98 | <Compile Include="Collision\CollisionShapes\TriangleShape.cs" /> |
99 | <Compile Include="Collision\NarrowPhaseCollision\ContinuousConvexCollision.cs" /> | 99 | <Compile Include="Collision\NarrowPhaseCollision\ContinuousConvexCollision.cs" /> |
100 | <Compile Include="Collision\NarrowPhaseCollision\ConvexCast.cs" /> | 100 | <Compile Include="Collision\NarrowPhaseCollision\ConvexCast.cs" /> |
101 | <Compile Include="Collision\NarrowPhaseCollision\DiscreteCollisionDetectorInterface.cs" /> | 101 | <Compile Include="Collision\NarrowPhaseCollision\DiscreteCollisionDetectorInterface.cs" /> |
102 | <Compile Include="Collision\NarrowPhaseCollision\GjkConvexCast.cs" /> | 102 | <Compile Include="Collision\NarrowPhaseCollision\GjkConvexCast.cs" /> |
103 | <Compile Include="Collision\NarrowPhaseCollision\GjkEpa.cs" /> | 103 | <Compile Include="Collision\NarrowPhaseCollision\GjkEpa.cs" /> |
104 | <Compile Include="Collision\NarrowPhaseCollision\GjkEpaPenetrationDepthSolver.cs" /> | 104 | <Compile Include="Collision\NarrowPhaseCollision\GjkEpaPenetrationDepthSolver.cs" /> |
105 | <Compile Include="Collision\NarrowPhaseCollision\GjkEpaSolver.cs" /> | 105 | <Compile Include="Collision\NarrowPhaseCollision\GjkEpaSolver.cs" /> |
106 | <Compile Include="Collision\NarrowPhaseCollision\GjkPairDetector.cs" /> | 106 | <Compile Include="Collision\NarrowPhaseCollision\GjkPairDetector.cs" /> |
107 | <Compile Include="Collision\NarrowPhaseCollision\IConvexPenetrationDepthSolver.cs" /> | 107 | <Compile Include="Collision\NarrowPhaseCollision\IConvexPenetrationDepthSolver.cs" /> |
108 | <Compile Include="Collision\NarrowPhaseCollision\ISimplexSolver.cs" /> | 108 | <Compile Include="Collision\NarrowPhaseCollision\ISimplexSolver.cs" /> |
109 | <Compile Include="Collision\NarrowPhaseCollision\ManifoldPoint.cs" /> | 109 | <Compile Include="Collision\NarrowPhaseCollision\ManifoldPoint.cs" /> |
110 | <Compile Include="Collision\NarrowPhaseCollision\MinkowskiPenetrationDepthSolver.cs" /> | 110 | <Compile Include="Collision\NarrowPhaseCollision\MinkowskiPenetrationDepthSolver.cs" /> |
111 | <Compile Include="Collision\NarrowPhaseCollision\PersistentManifold.cs" /> | 111 | <Compile Include="Collision\NarrowPhaseCollision\PersistentManifold.cs" /> |
112 | <Compile Include="Collision\NarrowPhaseCollision\PointCollector.cs" /> | 112 | <Compile Include="Collision\NarrowPhaseCollision\PointCollector.cs" /> |
113 | <Compile Include="Collision\NarrowPhaseCollision\SubsimplexConvexCast.cs" /> | 113 | <Compile Include="Collision\NarrowPhaseCollision\SubsimplexConvexCast.cs" /> |
114 | <Compile Include="Collision\NarrowPhaseCollision\TriangleRaycastCallback.cs" /> | 114 | <Compile Include="Collision\NarrowPhaseCollision\TriangleRaycastCallback.cs" /> |
115 | <Compile Include="Collision\NarrowPhaseCollision\VoronoiSimplexSolver.cs" /> | 115 | <Compile Include="Collision\NarrowPhaseCollision\VoronoiSimplexSolver.cs" /> |
116 | <Compile Include="Dynamics\ConstraintSolver\ContactConstraint.cs" /> | 116 | <Compile Include="Dynamics\ConstraintSolver\ContactConstraint.cs" /> |
117 | <Compile Include="Dynamics\ConstraintSolver\ContactSolverInfo.cs" /> | 117 | <Compile Include="Dynamics\ConstraintSolver\ContactSolverInfo.cs" /> |
118 | <Compile Include="Dynamics\ConstraintSolver\Generic6DofConstraint.cs" /> | 118 | <Compile Include="Dynamics\ConstraintSolver\Generic6DofConstraint.cs" /> |
119 | <Compile Include="Dynamics\ConstraintSolver\HingeConstraint.cs" /> | 119 | <Compile Include="Dynamics\ConstraintSolver\HingeConstraint.cs" /> |
120 | <Compile Include="Dynamics\ConstraintSolver\IConstraintSolver.cs" /> | 120 | <Compile Include="Dynamics\ConstraintSolver\IConstraintSolver.cs" /> |
121 | <Compile Include="Dynamics\ConstraintSolver\JacobianEntry.cs" /> | 121 | <Compile Include="Dynamics\ConstraintSolver\JacobianEntry.cs" /> |
122 | <Compile Include="Dynamics\ConstraintSolver\Point2PointConstraint.cs" /> | 122 | <Compile Include="Dynamics\ConstraintSolver\Point2PointConstraint.cs" /> |
123 | <Compile Include="Dynamics\ConstraintSolver\SequentialImpulseConstraintSolver.cs" /> | 123 | <Compile Include="Dynamics\ConstraintSolver\SequentialImpulseConstraintSolver.cs" /> |
124 | <Compile Include="Dynamics\ConstraintSolver\Solve2LinearConstraint.cs" /> | 124 | <Compile Include="Dynamics\ConstraintSolver\Solve2LinearConstraint.cs" /> |
125 | <Compile Include="Dynamics\ConstraintSolver\SolverBody.cs" /> | 125 | <Compile Include="Dynamics\ConstraintSolver\SolverBody.cs" /> |
126 | <Compile Include="Dynamics\ConstraintSolver\SolverConstraint.cs" /> | 126 | <Compile Include="Dynamics\ConstraintSolver\SolverConstraint.cs" /> |
127 | <Compile Include="Dynamics\ConstraintSolver\TypedConstraint.cs" /> | 127 | <Compile Include="Dynamics\ConstraintSolver\TypedConstraint.cs" /> |
128 | <Compile Include="Dynamics\DiscreteDynamicsWorld.cs" /> | 128 | <Compile Include="Dynamics\DiscreteDynamicsWorld.cs" /> |
129 | <Compile Include="Dynamics\DynamicsWorld.cs" /> | 129 | <Compile Include="Dynamics\DynamicsWorld.cs" /> |
130 | <Compile Include="Dynamics\RigidBody.cs" /> | 130 | <Compile Include="Dynamics\RigidBody.cs" /> |
131 | <Compile Include="Dynamics\SimpleDynamicsWorld.cs" /> | 131 | <Compile Include="Dynamics\SimpleDynamicsWorld.cs" /> |
132 | <Compile Include="Dynamics\Vehicle\RaycastVehicle.cs" /> | 132 | <Compile Include="Dynamics\Vehicle\RaycastVehicle.cs" /> |
133 | <Compile Include="Dynamics\Vehicle\VehicleRaycaster.cs" /> | 133 | <Compile Include="Dynamics\Vehicle\VehicleRaycaster.cs" /> |
134 | <Compile Include="Dynamics\Vehicle\WheelInfo.cs" /> | 134 | <Compile Include="Dynamics\Vehicle\WheelInfo.cs" /> |
135 | <Compile Include="Exceptions\BulletException.cs" /> | 135 | <Compile Include="Exceptions\BulletException.cs" /> |
136 | <Compile Include="IDebugDraw.cs" /> | 136 | <Compile Include="IDebugDraw.cs" /> |
137 | <Compile Include="LinearMath\DefaultMotionState.cs" /> | 137 | <Compile Include="LinearMath\DefaultMotionState.cs" /> |
138 | <Compile Include="LinearMath\MathHelper.cs" /> | 138 | <Compile Include="LinearMath\MathHelper.cs" /> |
139 | <Compile Include="LinearMath\MatrixOperations.cs" /> | 139 | <Compile Include="LinearMath\MatrixOperations.cs" /> |
140 | <Compile Include="LinearMath\MotionState.cs" /> | 140 | <Compile Include="LinearMath\MotionState.cs" /> |
141 | <Compile Include="LinearMath\QuadWord.cs" /> | 141 | <Compile Include="LinearMath\QuadWord.cs" /> |
142 | <Compile Include="LinearMath\Quaternion.cs" /> | 142 | <Compile Include="LinearMath\Quaternion.cs" /> |
143 | <Compile Include="LinearMath\TransformUtil.cs" /> | 143 | <Compile Include="LinearMath\TransformUtil.cs" /> |
144 | <Compile Include="LinearMath\Vector3.cs" /> | 144 | <Compile Include="LinearMath\Vector3.cs" /> |
145 | <Compile Include="LinearMath\Vector4.cs" /> | 145 | <Compile Include="LinearMath\Vector4.cs" /> |
146 | <Compile Include="Properties\AssemblyInfo.cs" /> | 146 | <Compile Include="Properties\AssemblyInfo.cs" /> |
147 | </ItemGroup> | 147 | </ItemGroup> |
148 | <ItemGroup> | 148 | <ItemGroup> |
149 | <ProjectReference Include="..\MonoXnaCompactMaths\MonoXnaCompactMaths.csproj"> | 149 | <ProjectReference Include="..\MonoXnaCompactMaths\MonoXnaCompactMaths.csproj"> |
150 | <Project>{121147BC-B06B-406C-84E9-907F268CF0EB}</Project> | 150 | <Project>{121147BC-B06B-406C-84E9-907F268CF0EB}</Project> |
151 | <Name>MonoXnaCompactMaths</Name> | 151 | <Name>MonoXnaCompactMaths</Name> |
152 | </ProjectReference> | 152 | </ProjectReference> |
153 | </ItemGroup> | 153 | </ItemGroup> |
154 | <ItemGroup> | 154 | <ItemGroup> |
155 | <None Include="BulletX.snk" /> | 155 | <None Include="BulletX.snk" /> |
156 | </ItemGroup> | 156 | </ItemGroup> |
157 | <ItemGroup> | 157 | <ItemGroup> |
158 | <Folder Include="Content\" /> | 158 | <Folder Include="Content\" /> |
159 | </ItemGroup> | 159 | </ItemGroup> |
160 | <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" /> | 160 | <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" /> |
161 | <!-- To modify your build process, add your task inside one of the targets below and uncomment it. | 161 | <!-- To modify your build process, add your task inside one of the targets below and uncomment it. |
162 | Other similar extension points exist, see Microsoft.Common.targets. | 162 | Other similar extension points exist, see Microsoft.Common.targets. |
163 | <Target Name="BeforeBuild"> | 163 | <Target Name="BeforeBuild"> |
164 | </Target> | 164 | </Target> |
165 | <Target Name="AfterBuild"> | 165 | <Target Name="AfterBuild"> |
166 | </Target> | 166 | </Target> |
167 | --> | 167 | --> |
168 | </Project> \ No newline at end of file | 168 | </Project> \ No newline at end of file |
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs index efc8fd6..03d1e3e 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs | |||
@@ -1,35 +1,35 @@ | |||
1 | using System.Reflection; | 1 | using System.Reflection; |
2 | using System.Runtime.CompilerServices; | 2 | using System.Runtime.CompilerServices; |
3 | using System.Runtime.InteropServices; | 3 | using System.Runtime.InteropServices; |
4 | 4 | ||
5 | // La información general sobre un ensamblado se controla mediante el siguiente | 5 | // La información general sobre un ensamblado se controla mediante el siguiente |
6 | // conjunto de atributos. Cambie estos atributos para modificar la información | 6 | // conjunto de atributos. Cambie estos atributos para modificar la información |
7 | // asociada con un ensamblado. | 7 | // asociada con un ensamblado. |
8 | [assembly: AssemblyTitle("Modified.XnaDevRu.BulletX")] | 8 | [assembly: AssemblyTitle("Modified.XnaDevRu.BulletX")] |
9 | [assembly: AssemblyDescription("Modified Bullet for XNA")] | 9 | [assembly: AssemblyDescription("Modified Bullet for XNA")] |
10 | [assembly: AssemblyConfiguration("")] | 10 | [assembly: AssemblyConfiguration("")] |
11 | [assembly: AssemblyCompany("XNADev.ru")] | 11 | [assembly: AssemblyCompany("XNADev.ru")] |
12 | [assembly: AssemblyProduct("Bullet for XNA")] | 12 | [assembly: AssemblyProduct("Bullet for XNA")] |
13 | [assembly: AssemblyCopyright("Copyright © 2007 XNADev.ru")] | 13 | [assembly: AssemblyCopyright("Copyright © 2007 XNADev.ru")] |
14 | [assembly: AssemblyTrademark("")] | 14 | [assembly: AssemblyTrademark("")] |
15 | [assembly: AssemblyCulture("")] | 15 | [assembly: AssemblyCulture("")] |
16 | 16 | ||
17 | // Si establece ComVisible como false hace que los tipos de este ensamblado no sean visibles | 17 | // Si establece ComVisible como false hace que los tipos de este ensamblado no sean visibles |
18 | // a los componentes COM. Si necesita obtener acceso a un tipo en este ensamblado desde | 18 | // a los componentes COM. Si necesita obtener acceso a un tipo en este ensamblado desde |
19 | // COM, establezca el atributo ComVisible como true en este tipo. | 19 | // COM, establezca el atributo ComVisible como true en este tipo. |
20 | [assembly: ComVisible(false)] | 20 | [assembly: ComVisible(false)] |
21 | 21 | ||
22 | // El siguiente GUID sirve como identificador de la biblioteca de tipos si este proyecto se expone a COM | 22 | // El siguiente GUID sirve como identificador de la biblioteca de tipos si este proyecto se expone a COM |
23 | [assembly: Guid("a6148224-0271-4cd8-9d0a-5d4955cd2b2c")] | 23 | [assembly: Guid("a6148224-0271-4cd8-9d0a-5d4955cd2b2c")] |
24 | 24 | ||
25 | // La información de versión de un ensamblado consta de los cuatro valores siguientes: | 25 | // La información de versión de un ensamblado consta de los cuatro valores siguientes: |
26 | // | 26 | // |
27 | // Versión principal | 27 | // Versión principal |
28 | // Versión secundaria | 28 | // Versión secundaria |
29 | // Número de versión de compilación | 29 | // Número de versión de compilación |
30 | // Revisión | 30 | // Revisión |
31 | // | 31 | // |
32 | // Puede especificar todos los valores o puede establecer como valores predeterminados los números de revisión y generación | 32 | // Puede especificar todos los valores o puede establecer como valores predeterminados los números de revisión y generación |
33 | // mediante el asterisco ('*'), como se muestra a continuación: | 33 | // mediante el asterisco ('*'), como se muestra a continuación: |
34 | [assembly: AssemblyVersion("1.0.0.0")] | 34 | [assembly: AssemblyVersion("1.0.0.0")] |
35 | [assembly: AssemblyFileVersion("1.0.0.0")] | 35 | [assembly: AssemblyFileVersion("1.0.0.0")] |