diff options
Diffstat (limited to '')
125 files changed, 21428 insertions, 0 deletions
diff --git a/ThirdPartyLicenses/Bullet for Xna (ModifiedBulletX).txt b/ThirdPartyLicenses/Bullet for Xna (ModifiedBulletX).txt new file mode 100644 index 0000000..127d54f --- /dev/null +++ b/ThirdPartyLicenses/Bullet for Xna (ModifiedBulletX).txt | |||
@@ -0,0 +1,19 @@ | |||
1 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
2 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
3 | |||
4 | This software is provided 'as-is', without any express or implied | ||
5 | warranty. In no event will the authors be held liable for any damages | ||
6 | arising from the use of this software. | ||
7 | |||
8 | Permission is granted to anyone to use this software for any purpose, | ||
9 | including commercial applications, and to alter it and redistribute it | ||
10 | freely, subject to the following restrictions: | ||
11 | |||
12 | 1. The origin of this software must not be misrepresented; you must not | ||
13 | claim that you wrote the original software. If you use this software | ||
14 | in a product, an acknowledgment in the product documentation would be | ||
15 | appreciated but is not required. | ||
16 | 2. Altered source versions must be plainly marked as such, and must not be | ||
17 | misrepresented as being the original software. | ||
18 | 3. This notice may not be removed or altered from any source distribution. | ||
19 | \ No newline at end of file | ||
diff --git a/ThirdPartyLicenses/Mono.Xna (MonoXnaCompactMaths).txt b/ThirdPartyLicenses/Mono.Xna (MonoXnaCompactMaths).txt new file mode 100644 index 0000000..02e92cf --- /dev/null +++ b/ThirdPartyLicenses/Mono.Xna (MonoXnaCompactMaths).txt | |||
@@ -0,0 +1,22 @@ | |||
1 | MIT License | ||
2 | Copyright © 2006 The Mono.Xna Team | ||
3 | |||
4 | All rights reserved. | ||
5 | |||
6 | Permission is hereby granted, free of charge, to any person obtaining a copy | ||
7 | of this software and associated documentation files (the "Software"), to deal | ||
8 | in the Software without restriction, including without limitation the rights | ||
9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
10 | copies of the Software, and to permit persons to whom the Software is | ||
11 | furnished to do so, subject to the following conditions: | ||
12 | |||
13 | The above copyright notice and this permission notice shall be included in all | ||
14 | copies or substantial portions of the Software. | ||
15 | |||
16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE | ||
22 | SOFTWARE. \ No newline at end of file | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX.sln b/libraries/ModifiedBulletX/ModifiedBulletX.sln new file mode 100644 index 0000000..0eccfdd --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX.sln | |||
@@ -0,0 +1,42 @@ | |||
1 |  | ||
2 | Microsoft Visual Studio Solution File, Format Version 9.00 | ||
3 | # Visual C# Express 2005 | ||
4 | Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "MonoXnaCompactMaths", "MonoXnaCompactMaths\MonoXnaCompactMaths.csproj", "{121147BC-B06B-406C-84E9-907F268CF0EB}" | ||
5 | EndProject | ||
6 | Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Modified.XnaDevRu.BulletX", "ModifiedBulletX\Modified.XnaDevRu.BulletX.csproj", "{44270344-ACA7-4875-B585-81D5C06D0489}" | ||
7 | EndProject | ||
8 | Global | ||
9 | GlobalSection(SolutionConfigurationPlatforms) = preSolution | ||
10 | Debug|Any CPU = Debug|Any CPU | ||
11 | Debug|Mixed Platforms = Debug|Mixed Platforms | ||
12 | Debug|x86 = Debug|x86 | ||
13 | Release|Any CPU = Release|Any CPU | ||
14 | Release|Mixed Platforms = Release|Mixed Platforms | ||
15 | Release|x86 = Release|x86 | ||
16 | EndGlobalSection | ||
17 | GlobalSection(ProjectConfigurationPlatforms) = postSolution | ||
18 | {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Any CPU.ActiveCfg = Debug|Any CPU | ||
19 | {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Any CPU.Build.0 = Debug|Any CPU | ||
20 | {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Mixed Platforms.ActiveCfg = Debug|Any CPU | ||
21 | {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Mixed Platforms.Build.0 = Debug|Any CPU | ||
22 | {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|x86.ActiveCfg = Debug|Any CPU | ||
23 | {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Any CPU.ActiveCfg = Release|Any CPU | ||
24 | {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Any CPU.Build.0 = Release|Any CPU | ||
25 | {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Mixed Platforms.ActiveCfg = Release|Any CPU | ||
26 | {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Mixed Platforms.Build.0 = Release|Any CPU | ||
27 | {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|x86.ActiveCfg = Release|Any CPU | ||
28 | {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Any CPU.ActiveCfg = Debug|Any CPU | ||
29 | {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Any CPU.Build.0 = Debug|Any CPU | ||
30 | {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Mixed Platforms.ActiveCfg = Debug|Any CPU | ||
31 | {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Mixed Platforms.Build.0 = Debug|Any CPU | ||
32 | {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|x86.ActiveCfg = Debug|Any CPU | ||
33 | {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Any CPU.ActiveCfg = Release|Any CPU | ||
34 | {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Any CPU.Build.0 = Release|Any CPU | ||
35 | {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Mixed Platforms.ActiveCfg = Release|Any CPU | ||
36 | {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Mixed Platforms.Build.0 = Release|Any CPU | ||
37 | {44270344-ACA7-4875-B585-81D5C06D0489}.Release|x86.ActiveCfg = Release|Any CPU | ||
38 | EndGlobalSection | ||
39 | GlobalSection(SolutionProperties) = preSolution | ||
40 | HideSolutionNode = FALSE | ||
41 | EndGlobalSection | ||
42 | EndGlobal | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX.suo b/libraries/ModifiedBulletX/ModifiedBulletX.suo new file mode 100644 index 0000000..4f0acd9 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX.suo | |||
Binary files differ | |||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs b/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs new file mode 100644 index 0000000..d0d5845 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs | |||
@@ -0,0 +1,68 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using System.Diagnostics; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | internal static class BulletDebug | ||
30 | { | ||
31 | [Conditional("DEBUG")] | ||
32 | public static void Assert(Boolean condition) | ||
33 | { | ||
34 | //if (!condition) | ||
35 | //{ | ||
36 | // Throw("No info available"); | ||
37 | //} | ||
38 | Debug.Assert(condition); | ||
39 | } | ||
40 | |||
41 | [Conditional("DEBUG")] | ||
42 | public static void Assert(Boolean condition, String message) | ||
43 | { | ||
44 | //if (!condition) | ||
45 | //{ | ||
46 | // Throw(message); | ||
47 | //} | ||
48 | Debug.Assert(condition, message); | ||
49 | } | ||
50 | |||
51 | [Conditional("DEBUG")] | ||
52 | public static void Assert(Boolean condition, String message, String detailMessage) | ||
53 | { | ||
54 | //if (!condition) | ||
55 | //{ | ||
56 | // Throw(message); | ||
57 | //} | ||
58 | Debug.Assert(condition, message, detailMessage); | ||
59 | } | ||
60 | |||
61 | private static void Throw(String message) | ||
62 | { | ||
63 | String msg = String.Format("Assertion Error: {0}", message); | ||
64 | |||
65 | throw new BulletException(msg); | ||
66 | } | ||
67 | } | ||
68 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/BulletX.snk b/libraries/ModifiedBulletX/ModifiedBulletX/BulletX.snk new file mode 100644 index 0000000..4ce5907 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/BulletX.snk | |||
Binary files differ | |||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs new file mode 100644 index 0000000..12692ea --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs | |||
@@ -0,0 +1,623 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class AxisSweep3: OverlappingPairCache | ||
30 | { | ||
31 | Vector3 _worldAabbMin; | ||
32 | Vector3 _worldAabbMax; | ||
33 | |||
34 | Vector3 _quantize; | ||
35 | |||
36 | int _numHandles; | ||
37 | int _maxHandles; | ||
38 | |||
39 | Handle[] _handles; | ||
40 | Edge[][] _edges = new Edge[3][]; | ||
41 | |||
42 | ushort _firstFreeHandle; | ||
43 | |||
44 | int _invalidPair; | ||
45 | |||
46 | public AxisSweep3(Vector3 worldAabbMin, Vector3 worldAabbMax, int maxHandles) | ||
47 | : base() | ||
48 | { | ||
49 | BulletDebug.Assert(maxHandles > 1 && maxHandles < 32767); | ||
50 | |||
51 | // init bounds | ||
52 | _worldAabbMin = worldAabbMin; | ||
53 | _worldAabbMax = worldAabbMax; | ||
54 | |||
55 | Vector3 aabbSize = _worldAabbMax - _worldAabbMin; | ||
56 | _quantize = new Vector3(65535.0f, 65535.0f, 65535.0f) / aabbSize; | ||
57 | |||
58 | // allocate handles buffer and put all handles on free list | ||
59 | _handles = new Handle[maxHandles]; | ||
60 | for (int i = 0; i < maxHandles; i++) | ||
61 | _handles[i] = new Handle(); | ||
62 | _maxHandles = maxHandles; | ||
63 | _numHandles = 0; | ||
64 | |||
65 | // handle 0 is reserved as the null index, and is also used as the sentinel | ||
66 | _firstFreeHandle = 1; | ||
67 | { | ||
68 | for (int i = _firstFreeHandle; i < maxHandles; i++) | ||
69 | { | ||
70 | _handles[i].NextFree = (ushort)(i + 1); | ||
71 | } | ||
72 | _handles[maxHandles - 1].NextFree = 0; | ||
73 | } | ||
74 | |||
75 | { | ||
76 | // allocate edge buffers | ||
77 | for (int i = 0; i < 3; i++) | ||
78 | { | ||
79 | _edges[i] = new Edge[maxHandles * 2]; | ||
80 | for (int j = 0; j < maxHandles * 2; j++) | ||
81 | { | ||
82 | _edges[i][j] = new Edge(); | ||
83 | } | ||
84 | } | ||
85 | } | ||
86 | //removed overlap management | ||
87 | |||
88 | // make boundary sentinels | ||
89 | |||
90 | _handles[0].ClientData = 0; | ||
91 | |||
92 | for (int axis = 0; axis < 3; axis++) | ||
93 | { | ||
94 | _handles[0].MinEdges[axis] = 0; | ||
95 | _handles[0].MaxEdges[axis] = 1; | ||
96 | |||
97 | _edges[axis][0].Position = 0; | ||
98 | _edges[axis][0].Handle = 0; | ||
99 | _edges[axis][1].Position = 0xffff; | ||
100 | _edges[axis][1].Handle = 0; | ||
101 | } | ||
102 | } | ||
103 | |||
104 | public ushort AddHandle(Vector3 aabbMin, Vector3 aabbMax, object owner, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) | ||
105 | { | ||
106 | ushort[] min = new ushort[3], max = new ushort[3]; | ||
107 | Quantize(out min, aabbMin, 0); | ||
108 | Quantize(out max, aabbMax, 1); | ||
109 | |||
110 | ushort handle = AllocateHandle(); | ||
111 | Handle oHandle = GetHandle(handle); | ||
112 | |||
113 | oHandle.HandleID = handle; | ||
114 | oHandle.ClientData = owner; | ||
115 | oHandle.CollisionFilterGroup = collisionFilterGroup; | ||
116 | oHandle.CollisionFilterMask = collisionFilterMask; | ||
117 | |||
118 | int limit = _numHandles * 2; | ||
119 | |||
120 | // (Gluk ) | ||
121 | // ( Inside ) | ||
122 | for (int axis = 0; axis < 3; axis++) | ||
123 | { | ||
124 | _handles[0].MaxEdges[axis] += 2; | ||
125 | |||
126 | _edges[axis][limit + 1].Position = _edges[axis][limit - 1].Position; | ||
127 | _edges[axis][limit + 1].Handle = _edges[axis][limit - 1].Handle; | ||
128 | |||
129 | _edges[axis][limit - 1].Position = min[axis]; | ||
130 | _edges[axis][limit - 1].Handle = handle; | ||
131 | |||
132 | _edges[axis][limit].Position = max[axis]; | ||
133 | _edges[axis][limit].Handle = handle; | ||
134 | |||
135 | oHandle.MinEdges[axis] = (ushort)(limit - 1); | ||
136 | oHandle.MaxEdges[axis] = (ushort)limit; | ||
137 | } | ||
138 | |||
139 | SortMinDown(0, oHandle.MinEdges[0], false); | ||
140 | SortMaxDown(0, oHandle.MaxEdges[0], false); | ||
141 | SortMinDown(1, oHandle.MinEdges[1], false); | ||
142 | SortMaxDown(1, oHandle.MaxEdges[1], false); | ||
143 | SortMinDown(2, oHandle.MinEdges[2], true); | ||
144 | SortMaxDown(2, oHandle.MaxEdges[2], true); | ||
145 | |||
146 | return handle; | ||
147 | } | ||
148 | |||
149 | public void RemoveHandle(ushort handle) | ||
150 | { | ||
151 | Handle pHandle = GetHandle(handle); | ||
152 | |||
153 | //explicitly remove the pairs containing the proxy | ||
154 | //we could do it also in the sortMinUp (passing true) | ||
155 | //todo: compare performance | ||
156 | RemoveOverlappingPairsContainingProxy(pHandle); | ||
157 | |||
158 | |||
159 | // compute current limit of edge arrays | ||
160 | int limit = _numHandles * 2; | ||
161 | int axis; | ||
162 | |||
163 | for (axis = 0; axis < 3; axis++) | ||
164 | { | ||
165 | _handles[0].MaxEdges[axis] -= 2; | ||
166 | } | ||
167 | |||
168 | // remove the edges by sorting them up to the end of the list | ||
169 | for (axis = 0; axis < 3; axis++) | ||
170 | { | ||
171 | Edge[] pEdges = _edges[axis]; | ||
172 | ushort max = pHandle.MaxEdges[axis]; | ||
173 | pEdges[max].Position = 0xffff; | ||
174 | |||
175 | SortMaxUp(axis, max, false); | ||
176 | |||
177 | ushort i = pHandle.MinEdges[axis]; | ||
178 | pEdges[i].Position = 0xffff; | ||
179 | |||
180 | SortMinUp(axis, i, false); | ||
181 | |||
182 | pEdges[limit - 1].Handle = 0; | ||
183 | pEdges[limit - 1].Position = 0xffff; | ||
184 | } | ||
185 | |||
186 | // free the handle | ||
187 | FreeHandle(handle); | ||
188 | } | ||
189 | |||
190 | public override void ProcessAllOverlappingPairs(IOverlapCallback callback) | ||
191 | { | ||
192 | OverlappingPairs.Sort(new Comparison<BroadphasePair>(BroadphasePair.ComparisonSort)); | ||
193 | |||
194 | if (_invalidPair != 0) | ||
195 | OverlappingPairs.RemoveRange(OverlappingPairs.Count - _invalidPair, _invalidPair); | ||
196 | _invalidPair = 0; | ||
197 | |||
198 | BroadphasePair previousPair = new BroadphasePair(); | ||
199 | previousPair.ProxyA = null; | ||
200 | previousPair.ProxyB = null; | ||
201 | previousPair.CollisionAlgorithm = null; | ||
202 | |||
203 | List<BroadphasePair> removal = new List<BroadphasePair>(); | ||
204 | |||
205 | for (int i = 0; i < OverlappingPairs.Count; i++) | ||
206 | { | ||
207 | bool isDuplicate = (OverlappingPairs[i] == previousPair); | ||
208 | previousPair = OverlappingPairs[i]; | ||
209 | bool needsRemoval; | ||
210 | if (!isDuplicate) | ||
211 | { | ||
212 | bool hasOverlap = TestOverlap(previousPair.ProxyA, previousPair.ProxyB); | ||
213 | if (hasOverlap) | ||
214 | { | ||
215 | needsRemoval = callback.ProcessOverlap(ref previousPair); | ||
216 | } | ||
217 | else | ||
218 | { | ||
219 | needsRemoval = true; | ||
220 | } | ||
221 | } | ||
222 | else | ||
223 | { | ||
224 | needsRemoval = true; | ||
225 | BulletDebug.Assert(previousPair.CollisionAlgorithm == null); | ||
226 | } | ||
227 | |||
228 | if (needsRemoval) | ||
229 | { | ||
230 | removal.Add(previousPair); | ||
231 | } | ||
232 | } | ||
233 | |||
234 | for (int i = 0; i < removal.Count; i++) | ||
235 | { | ||
236 | BroadphasePair pair = removal[i]; | ||
237 | CleanOverlappingPair(ref pair); | ||
238 | pair.ProxyA = null; | ||
239 | pair.ProxyB = null; | ||
240 | _invalidPair++; | ||
241 | OverlappingPairCount--; | ||
242 | } | ||
243 | } | ||
244 | |||
245 | private bool TestOverlap(BroadphaseProxy proxyA, BroadphaseProxy proxyB) | ||
246 | { | ||
247 | if (proxyA == null || proxyB == null) | ||
248 | return false; | ||
249 | |||
250 | Handle handleA = proxyA as Handle; | ||
251 | Handle handleB = proxyB as Handle; | ||
252 | |||
253 | for (int axis = 0; axis < 3; axis++) | ||
254 | { | ||
255 | if (handleA.MaxEdges[axis] < handleB.MinEdges[axis] || | ||
256 | handleB.MaxEdges[axis] < handleA.MinEdges[axis]) | ||
257 | { | ||
258 | return false; | ||
259 | } | ||
260 | } | ||
261 | return true; | ||
262 | } | ||
263 | |||
264 | private bool TestOverlap(int ignoreAxis, Handle pHandleA, Handle pHandleB) | ||
265 | { | ||
266 | for (int axis = 0; axis < 3; axis++) | ||
267 | { | ||
268 | if (axis != ignoreAxis) | ||
269 | { | ||
270 | if (pHandleA.MaxEdges[axis] < pHandleB.MinEdges[axis] || | ||
271 | pHandleB.MaxEdges[axis] < pHandleA.MinEdges[axis]) | ||
272 | { | ||
273 | return false; | ||
274 | } | ||
275 | } | ||
276 | } | ||
277 | |||
278 | return true; | ||
279 | } | ||
280 | |||
281 | private ushort AllocateHandle() | ||
282 | { | ||
283 | ushort handle = _firstFreeHandle; | ||
284 | _firstFreeHandle = GetHandle(handle).NextFree; | ||
285 | _numHandles++; | ||
286 | |||
287 | return handle; | ||
288 | } | ||
289 | |||
290 | private void FreeHandle(ushort handle) | ||
291 | { | ||
292 | BulletDebug.Assert(handle > 0 && handle < _maxHandles); | ||
293 | |||
294 | GetHandle(handle).NextFree = _firstFreeHandle; | ||
295 | _firstFreeHandle = handle; | ||
296 | |||
297 | _numHandles--; | ||
298 | } | ||
299 | |||
300 | private Handle GetHandle(ushort handle) | ||
301 | { | ||
302 | return _handles[handle]; | ||
303 | } | ||
304 | |||
305 | private void UpdateHandle(ushort handle, Vector3 aabbMin, Vector3 aabbMax) | ||
306 | { | ||
307 | Handle pHandle = GetHandle(handle); | ||
308 | |||
309 | // quantize the new bounds | ||
310 | ushort[] min = new ushort[3]; | ||
311 | ushort[] max = new ushort[3]; | ||
312 | Quantize(out min, aabbMin, 0); | ||
313 | Quantize(out max, aabbMax, 1); | ||
314 | |||
315 | // update changed edges | ||
316 | for (int axis = 0; axis < 3; axis++) | ||
317 | { | ||
318 | ushort emin = pHandle.MinEdges[axis]; | ||
319 | ushort emax = pHandle.MaxEdges[axis]; | ||
320 | |||
321 | int dmin = (int)min[axis] - (int)_edges[axis][emin].Position; | ||
322 | int dmax = (int)max[axis] - (int)_edges[axis][emax].Position; | ||
323 | |||
324 | _edges[axis][emin].Position = min[axis]; | ||
325 | _edges[axis][emax].Position = max[axis]; | ||
326 | |||
327 | // expand (only adds overlaps) | ||
328 | if (dmin < 0) | ||
329 | SortMinDown(axis, emin, true); | ||
330 | |||
331 | if (dmax > 0) | ||
332 | SortMaxUp(axis, emax, true); | ||
333 | |||
334 | // shrink (only removes overlaps) | ||
335 | if (dmin > 0) | ||
336 | SortMinUp(axis, emin, true); | ||
337 | |||
338 | if (dmax < 0) | ||
339 | SortMaxDown(axis, emax, true); | ||
340 | } | ||
341 | } | ||
342 | |||
343 | private void Quantize(out ushort[] result, Vector3 point, int isMax) | ||
344 | { | ||
345 | Vector3 clampedPoint = new Vector3( | ||
346 | point.X, | ||
347 | point.Y, | ||
348 | point.Z | ||
349 | ); | ||
350 | |||
351 | MathHelper.SetMax(ref clampedPoint, _worldAabbMin); | ||
352 | MathHelper.SetMin(ref clampedPoint, _worldAabbMax); | ||
353 | |||
354 | Vector3 v = (clampedPoint - _worldAabbMin) * _quantize; | ||
355 | |||
356 | result = new ushort[3]; | ||
357 | result[0] = (ushort)(((int)v.X & 0xfffe) | isMax); | ||
358 | result[1] = (ushort)(((int)v.Y & 0xfffe) | isMax); | ||
359 | result[2] = (ushort)(((int)v.Z & 0xfffe) | isMax); | ||
360 | } | ||
361 | |||
362 | private void SortMinDown(int axis, ushort edge, bool updateOverlaps) | ||
363 | { | ||
364 | Edge pEdge = _edges[axis][edge]; | ||
365 | Edge pPrev = _edges[axis][edge - 1]; | ||
366 | Handle pHandleEdge = GetHandle(pEdge.Handle); | ||
367 | |||
368 | while (pEdge.Position < pPrev.Position) | ||
369 | { | ||
370 | Handle pHandlePrev = GetHandle(pPrev.Handle); | ||
371 | |||
372 | if (pPrev.IsMax()) | ||
373 | { | ||
374 | // if previous edge is a maximum check the bounds and add an overlap if necessary | ||
375 | if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandlePrev)) | ||
376 | { | ||
377 | AddOverlappingPair(pHandleEdge, pHandlePrev); | ||
378 | } | ||
379 | |||
380 | // update edge reference in other handle | ||
381 | pHandlePrev.MaxEdges[axis]++; | ||
382 | } | ||
383 | else | ||
384 | pHandlePrev.MinEdges[axis]++; | ||
385 | |||
386 | pHandleEdge.MinEdges[axis]--; | ||
387 | |||
388 | // swap the edges | ||
389 | pEdge.Swap(ref pPrev); | ||
390 | |||
391 | // decrement | ||
392 | edge--; | ||
393 | pEdge = _edges[axis][edge]; | ||
394 | pPrev = _edges[axis][edge - 1]; | ||
395 | } | ||
396 | } | ||
397 | |||
398 | private void SortMinUp(int axis, ushort edge, bool updateOverlaps) | ||
399 | { | ||
400 | Edge pEdge = _edges[axis][edge]; | ||
401 | Edge pNext = _edges[axis][edge + 1]; | ||
402 | Handle pHandleEdge = GetHandle(pEdge.Handle); | ||
403 | |||
404 | while ((pNext.Handle != 0) && (pEdge.Position >= pNext.Position)) | ||
405 | { | ||
406 | Handle pHandleNext = GetHandle(pNext.Handle); | ||
407 | |||
408 | if (pNext.IsMax()) | ||
409 | { | ||
410 | // if next edge is maximum remove any overlap between the two handles | ||
411 | if (updateOverlaps) | ||
412 | { | ||
413 | //Handle handle0 = GetHandle(pEdge.Handle); | ||
414 | //Handle handle1 = GetHandle(pNext.Handle); | ||
415 | //BroadphasePair tmpPair = new BroadphasePair(handle0, handle1); | ||
416 | //RemoveOverlappingPair(tmpPair); | ||
417 | } | ||
418 | |||
419 | // update edge reference in other handle | ||
420 | pHandleNext.MaxEdges[axis]--; | ||
421 | } | ||
422 | else | ||
423 | pHandleNext.MinEdges[axis]--; | ||
424 | |||
425 | pHandleEdge.MinEdges[axis]++; | ||
426 | |||
427 | // swap the edges | ||
428 | pEdge.Swap(ref pNext); | ||
429 | |||
430 | // increment | ||
431 | edge++; | ||
432 | pEdge = _edges[axis][edge]; | ||
433 | pNext = _edges[axis][edge + 1]; | ||
434 | } | ||
435 | } | ||
436 | |||
437 | private void SortMaxDown(int axis, ushort edge, bool updateOverlaps) | ||
438 | { | ||
439 | Edge pEdge = _edges[axis][edge]; | ||
440 | Edge pPrev = _edges[axis][edge - 1]; | ||
441 | Handle pHandleEdge = GetHandle(pEdge.Handle); | ||
442 | |||
443 | while (pEdge.Position < pPrev.Position) | ||
444 | { | ||
445 | Handle pHandlePrev = GetHandle(pPrev.Handle); | ||
446 | |||
447 | if (!pPrev.IsMax()) | ||
448 | { | ||
449 | // if previous edge was a minimum remove any overlap between the two handles | ||
450 | if (updateOverlaps) | ||
451 | { | ||
452 | //this is done during the overlappingpairarray iteration/narrowphase collision | ||
453 | //Handle handle0 = GetHandle(pEdge.Handle); | ||
454 | //Handle handle1 = GetHandle(pPrev.Handle); | ||
455 | //BroadphasePair pair = FindPair(handle0, handle1); | ||
456 | |||
457 | //if (pair != null) | ||
458 | //{ | ||
459 | // RemoveOverlappingPair(pair); | ||
460 | //} | ||
461 | } | ||
462 | |||
463 | // update edge reference in other handle | ||
464 | pHandlePrev.MinEdges[axis]++; ; | ||
465 | } | ||
466 | else | ||
467 | pHandlePrev.MaxEdges[axis]++; | ||
468 | |||
469 | pHandleEdge.MaxEdges[axis]--; | ||
470 | |||
471 | // swap the edges | ||
472 | pEdge.Swap(ref pPrev); | ||
473 | |||
474 | // decrement | ||
475 | edge--; | ||
476 | pEdge = _edges[axis][edge]; | ||
477 | pPrev = _edges[axis][edge - 1]; | ||
478 | } | ||
479 | } | ||
480 | |||
481 | private void SortMaxUp(int axis, ushort edge, bool updateOverlaps) | ||
482 | { | ||
483 | Edge pEdge = _edges[axis][edge]; | ||
484 | Edge pNext = _edges[axis][edge + 1]; | ||
485 | Handle pHandleEdge = GetHandle(pEdge.Handle); | ||
486 | |||
487 | while ((pNext.Handle!=0) && (pEdge.Position >= pNext.Position)) | ||
488 | { | ||
489 | Handle pHandleNext = GetHandle(pNext.Handle); | ||
490 | |||
491 | if (!pNext.IsMax()) | ||
492 | { | ||
493 | // if next edge is a minimum check the bounds and add an overlap if necessary | ||
494 | if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandleNext)) | ||
495 | { | ||
496 | Handle handle0 = GetHandle(pEdge.Handle); | ||
497 | Handle handle1 = GetHandle(pNext.Handle); | ||
498 | AddOverlappingPair(handle0, handle1); | ||
499 | } | ||
500 | |||
501 | // update edge reference in other handle | ||
502 | pHandleNext.MinEdges[axis]--; | ||
503 | } | ||
504 | else | ||
505 | pHandleNext.MaxEdges[axis]--; | ||
506 | |||
507 | pHandleEdge.MaxEdges[axis]++; | ||
508 | |||
509 | // swap the edges | ||
510 | pEdge.Swap(ref pNext); | ||
511 | |||
512 | // increment | ||
513 | edge++; | ||
514 | pEdge = _edges[axis][edge]; | ||
515 | pNext = _edges[axis][edge + 1]; | ||
516 | } | ||
517 | } | ||
518 | |||
519 | #region Abstract | ||
520 | |||
521 | public override void RefreshOverlappingPairs() | ||
522 | { | ||
523 | } | ||
524 | |||
525 | public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) | ||
526 | { | ||
527 | ushort handleId = AddHandle(min, max, userData, collisionFilterGroup, collisionFilterMask); | ||
528 | |||
529 | Handle handle = GetHandle(handleId); | ||
530 | |||
531 | return handle; | ||
532 | } | ||
533 | |||
534 | public override void DestroyProxy(BroadphaseProxy proxy) | ||
535 | { | ||
536 | Handle handle = proxy as Handle; | ||
537 | RemoveHandle(handle.HandleID); | ||
538 | } | ||
539 | |||
540 | public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax) | ||
541 | { | ||
542 | Handle handle = proxy as Handle; | ||
543 | UpdateHandle(handle.HandleID, aabbMin, aabbMax); | ||
544 | } | ||
545 | #endregion | ||
546 | } | ||
547 | |||
548 | public class Edge | ||
549 | { | ||
550 | ushort position; | ||
551 | ushort handle; | ||
552 | |||
553 | public ushort Position | ||
554 | { | ||
555 | get { return position; } | ||
556 | set { position = value; } | ||
557 | } | ||
558 | |||
559 | public ushort Handle | ||
560 | { | ||
561 | get { return handle; } | ||
562 | set { handle = value; } | ||
563 | } | ||
564 | |||
565 | public bool IsMax() | ||
566 | { | ||
567 | return (position & (ushort)1) == 1; | ||
568 | } | ||
569 | |||
570 | public void Swap(ref Edge e) | ||
571 | { | ||
572 | ushort tmpPosition = this.position; | ||
573 | ushort tmpHandle = this.handle; | ||
574 | this.position = e.position; | ||
575 | this.handle = e.handle; | ||
576 | e.position = tmpPosition; | ||
577 | e.handle = tmpHandle; | ||
578 | } | ||
579 | } | ||
580 | |||
581 | public class Handle: BroadphaseProxy | ||
582 | { | ||
583 | ushort[] minEdges, maxEdges; | ||
584 | ushort pad; | ||
585 | ushort handleID; | ||
586 | |||
587 | public ushort[] MinEdges | ||
588 | { | ||
589 | get { return minEdges; } | ||
590 | set { minEdges = value; } | ||
591 | } | ||
592 | |||
593 | public ushort[] MaxEdges | ||
594 | { | ||
595 | get { return maxEdges; } | ||
596 | set { maxEdges = value; } | ||
597 | } | ||
598 | |||
599 | public ushort HandleID | ||
600 | { | ||
601 | get { return handleID; } | ||
602 | set { handleID = value; } | ||
603 | } | ||
604 | |||
605 | public ushort Pad | ||
606 | { | ||
607 | get { return pad; } | ||
608 | set { pad = value; } | ||
609 | } | ||
610 | |||
611 | public ushort NextFree | ||
612 | { | ||
613 | get { return minEdges[0]; } | ||
614 | set { minEdges[0] = value;} | ||
615 | } | ||
616 | |||
617 | public Handle() | ||
618 | { | ||
619 | minEdges = new ushort[3]; | ||
620 | maxEdges = new ushort[3]; | ||
621 | } | ||
622 | } | ||
623 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs new file mode 100644 index 0000000..dcfd0d8 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs | |||
@@ -0,0 +1,68 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | /// Dispatcher uses these types | ||
29 | /// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave | ||
30 | /// to facilitate type checking | ||
31 | public enum BroadphaseNativeTypes | ||
32 | { | ||
33 | // polyhedral convex shapes | ||
34 | Box, | ||
35 | Triangle, | ||
36 | Tetrahedral, | ||
37 | ConvexTriangleMesh, | ||
38 | ConvexHull, | ||
39 | //implicit convex shapes | ||
40 | ImplicitConvexShapes, | ||
41 | Sphere, | ||
42 | MultiSphere, | ||
43 | Capsule, | ||
44 | Cone, | ||
45 | Convex, | ||
46 | Cylinder, | ||
47 | MinkowskiSum, | ||
48 | MinkowskiDifference, | ||
49 | //concave shapes | ||
50 | ConcaveShapesStart, | ||
51 | //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy! | ||
52 | TriangleMesh, | ||
53 | //used for demo integration FAST/Swift collision library and Bullet | ||
54 | FastConcaveMesh, | ||
55 | //terrain | ||
56 | Terrain, | ||
57 | //Used for GIMPACT Trimesh integration | ||
58 | Gimpact, | ||
59 | |||
60 | Empty, | ||
61 | StaticPlane, | ||
62 | ConcaveShapesEnd, | ||
63 | |||
64 | Compound, | ||
65 | |||
66 | MaxBroadphaseCollisionTypes, | ||
67 | } | ||
68 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs new file mode 100644 index 0000000..2b9e114 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs | |||
@@ -0,0 +1,113 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public class BroadphasePair | ||
29 | { | ||
30 | private BroadphaseProxy _proxyA; | ||
31 | private BroadphaseProxy _proxyB; | ||
32 | |||
33 | private CollisionAlgorithm _algorithm; | ||
34 | private object _userInfo; | ||
35 | |||
36 | public BroadphasePair() | ||
37 | { | ||
38 | } | ||
39 | |||
40 | public BroadphasePair(BroadphasePair other) | ||
41 | { | ||
42 | _proxyA = other._proxyA; | ||
43 | _proxyB = other._proxyB; | ||
44 | |||
45 | _algorithm = other._algorithm; | ||
46 | _userInfo = null; | ||
47 | } | ||
48 | |||
49 | public BroadphasePair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) | ||
50 | { | ||
51 | _proxyA = proxyA; | ||
52 | _proxyB = proxyB; | ||
53 | |||
54 | _algorithm = null; | ||
55 | _userInfo = null; | ||
56 | } | ||
57 | |||
58 | public BroadphaseProxy ProxyA { get { return _proxyA; } set { _proxyA = value; } } | ||
59 | public BroadphaseProxy ProxyB { get { return _proxyB; } set { _proxyB = value; } } | ||
60 | |||
61 | public CollisionAlgorithm CollisionAlgorithm { get { return _algorithm; } set { _algorithm = value; } } | ||
62 | public object UserInfo { get { return _userInfo; } set { _userInfo = value; } } | ||
63 | |||
64 | public override int GetHashCode() | ||
65 | { | ||
66 | return _proxyA.GetHashCode() ^ _proxyB.GetHashCode(); | ||
67 | } | ||
68 | |||
69 | public override bool Equals(object obj) | ||
70 | { | ||
71 | if (obj is BroadphasePair) | ||
72 | return this == (BroadphasePair)obj; | ||
73 | return false; | ||
74 | } | ||
75 | |||
76 | public static int ComparisonSort(BroadphasePair a, BroadphasePair b) | ||
77 | { | ||
78 | int aAId = a.ProxyA != null ? a.ProxyA.ComparisonID : -1; | ||
79 | int aBId = a.ProxyB != null ? a.ProxyB.ComparisonID : -1; | ||
80 | int aCId = a.CollisionAlgorithm != null ? a.CollisionAlgorithm.ComparisonID : -1; | ||
81 | int bAId = b.ProxyA != null ? b.ProxyA.ComparisonID : -1; | ||
82 | int bBId = b.ProxyB != null ? b.ProxyB.ComparisonID : -1; | ||
83 | int bCId = b.CollisionAlgorithm != null ? b.CollisionAlgorithm.ComparisonID : -1; | ||
84 | |||
85 | if (aAId > bAId || | ||
86 | (a.ProxyA == b.ProxyA && aBId > bBId) || | ||
87 | (a.ProxyA == b.ProxyA && a.ProxyB == b.ProxyB && aCId > bCId)) | ||
88 | return -1; | ||
89 | else | ||
90 | return 1; | ||
91 | } | ||
92 | |||
93 | public static bool operator ==(BroadphasePair a, BroadphasePair b) | ||
94 | { | ||
95 | if (object.Equals(a, null) && object.Equals(b, null)) | ||
96 | return true; | ||
97 | if (object.Equals(a, null) || object.Equals(b, null)) | ||
98 | return false; | ||
99 | |||
100 | return (a.ProxyA == b.ProxyA) && (a.ProxyB == b.ProxyB); | ||
101 | } | ||
102 | |||
103 | public static bool operator !=(BroadphasePair a, BroadphasePair b) | ||
104 | { | ||
105 | if (object.Equals(a, null) && object.Equals(b, null)) | ||
106 | return true; | ||
107 | if (object.Equals(a, null) || object.Equals(b, null)) | ||
108 | return false; | ||
109 | |||
110 | return (a.ProxyA != b.ProxyA) || (a.ProxyB != b.ProxyB); | ||
111 | } | ||
112 | } | ||
113 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs new file mode 100644 index 0000000..aed82d5 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs | |||
@@ -0,0 +1,91 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public class BroadphaseProxy | ||
29 | { | ||
30 | //Usually the client CollisionObject or Rigidbody class | ||
31 | private object _clientObject; | ||
32 | private CollisionFilterGroups _collisionFilterGroup; | ||
33 | private CollisionFilterGroups _collisionFilterMask; | ||
34 | private readonly int _comparisonID; | ||
35 | |||
36 | private static int _globalCount = 0; | ||
37 | |||
38 | public BroadphaseProxy() | ||
39 | { | ||
40 | _comparisonID = _globalCount++; | ||
41 | } | ||
42 | |||
43 | public BroadphaseProxy(object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask) | ||
44 | : this() | ||
45 | { | ||
46 | _clientObject = userData; | ||
47 | _collisionFilterGroup = collisionFilterGroup; | ||
48 | _collisionFilterMask = collisionFilterMask; | ||
49 | } | ||
50 | |||
51 | public object ClientData { get { return _clientObject; } set { _clientObject = value; } } | ||
52 | public CollisionFilterGroups CollisionFilterGroup { get { return _collisionFilterGroup; } set { _collisionFilterGroup = value; } } | ||
53 | public CollisionFilterGroups CollisionFilterMask { get { return _collisionFilterMask; } set { _collisionFilterMask = value; } } | ||
54 | internal int ComparisonID { get { return _comparisonID; } } | ||
55 | |||
56 | public static bool IsPolyhedral(BroadphaseNativeTypes proxyType) | ||
57 | { | ||
58 | return (proxyType < BroadphaseNativeTypes.ImplicitConvexShapes); | ||
59 | } | ||
60 | |||
61 | public static bool IsConvex(BroadphaseNativeTypes proxyType) | ||
62 | { | ||
63 | return (proxyType < BroadphaseNativeTypes.ConcaveShapesStart); | ||
64 | } | ||
65 | |||
66 | public static bool IsConcave(BroadphaseNativeTypes proxyType) | ||
67 | { | ||
68 | return ((proxyType > BroadphaseNativeTypes.ConcaveShapesStart) && | ||
69 | (proxyType < BroadphaseNativeTypes.ConcaveShapesEnd)); | ||
70 | } | ||
71 | public static bool IsCompound(BroadphaseNativeTypes proxyType) | ||
72 | { | ||
73 | return (proxyType == BroadphaseNativeTypes.Compound); | ||
74 | } | ||
75 | public static bool IsInfinite(BroadphaseNativeTypes proxyType) | ||
76 | { | ||
77 | return (proxyType == BroadphaseNativeTypes.StaticPlane); | ||
78 | } | ||
79 | |||
80 | //optional filtering to cull potential collisions | ||
81 | public enum CollisionFilterGroups | ||
82 | { | ||
83 | Default = 1, | ||
84 | Static = 2, | ||
85 | Kinematic = 4, | ||
86 | Debris = 8, | ||
87 | Sensor = 16, | ||
88 | All = Default | Static | Kinematic | Debris | Sensor, | ||
89 | } | ||
90 | } | ||
91 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs new file mode 100644 index 0000000..1e59ad8 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs | |||
@@ -0,0 +1,51 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | /// <summary> | ||
29 | /// CollisionAlgorithm is an collision interface that is compatible with the Broadphase and Dispatcher. | ||
30 | /// It is persistent over frames | ||
31 | /// </summary> | ||
32 | public abstract class CollisionAlgorithm | ||
33 | { | ||
34 | private IDispatcher _dispatcher; | ||
35 | private readonly int _comparisonID = 0; | ||
36 | |||
37 | private static int _globalCount = 0; | ||
38 | |||
39 | public CollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) | ||
40 | { | ||
41 | _comparisonID = _globalCount++; | ||
42 | _dispatcher = collisionAlgorithmConstructionInfo.Dispatcher; | ||
43 | } | ||
44 | |||
45 | protected IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } } | ||
46 | internal int ComparisonID { get { return _comparisonID; } } | ||
47 | |||
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); | ||
50 | } | ||
51 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs new file mode 100644 index 0000000..381752a --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs | |||
@@ -0,0 +1,42 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public struct CollisionAlgorithmConstructionInfo | ||
29 | { | ||
30 | private IDispatcher _dispatcher; | ||
31 | private PersistentManifold _manifold; | ||
32 | |||
33 | public CollisionAlgorithmConstructionInfo(IDispatcher dispatcher) | ||
34 | { | ||
35 | _dispatcher = dispatcher; | ||
36 | _manifold = null; | ||
37 | } | ||
38 | |||
39 | public IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } } | ||
40 | public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } | ||
41 | } | ||
42 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs new file mode 100644 index 0000000..8c0367f --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs | |||
@@ -0,0 +1,54 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public enum DispatchFunction | ||
29 | { | ||
30 | Discrete = 1, | ||
31 | Continuous, | ||
32 | } | ||
33 | |||
34 | public class DispatcherInfo | ||
35 | { | ||
36 | private float _timeStep; | ||
37 | private int _stepCount; | ||
38 | private DispatchFunction _dispatchFunc = DispatchFunction.Discrete; | ||
39 | private float _timeOfImpact = 1; | ||
40 | private bool _useContinuous; | ||
41 | private bool _enableSatConvex; | ||
42 | private bool _enableSpu; | ||
43 | private IDebugDraw _debugDraw; | ||
44 | |||
45 | public float TimeStep { get { return _timeStep; } set { _timeStep = value; } } | ||
46 | public int StepCount { get { return _stepCount; } set { _stepCount = value; } } | ||
47 | public DispatchFunction DispatchFunction { get { return _dispatchFunc; } set { _dispatchFunc = value; } } | ||
48 | public float TimeOfImpact { get { return _timeOfImpact; } set { _timeOfImpact = value; } } | ||
49 | public bool UseContinuous { get { return _useContinuous; } set { _useContinuous = value; } } | ||
50 | public bool EnableSatConvex { get { return _enableSatConvex; } set { _enableSatConvex = value; } } | ||
51 | public bool enableSpu { get { return _enableSpu; } set { _enableSpu = value; } } | ||
52 | public IDebugDraw DebugDraw { get { return _debugDraw; } set { _debugDraw = value; } } | ||
53 | } | ||
54 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs new file mode 100644 index 0000000..89618af --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs | |||
@@ -0,0 +1,36 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public interface IBroadphase | ||
30 | { | ||
31 | BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask); | ||
32 | void DestroyProxy(BroadphaseProxy proxy); | ||
33 | void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax); | ||
34 | void CleanProxyFromPairs(BroadphaseProxy proxy); | ||
35 | } | ||
36 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs new file mode 100644 index 0000000..52831b7 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs | |||
@@ -0,0 +1,42 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public interface IDispatcher | ||
29 | { | ||
30 | CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold); | ||
31 | CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB); | ||
32 | PersistentManifold GetNewManifold(object bodyA, object bodyB); | ||
33 | void ReleaseManifold(PersistentManifold manifold); | ||
34 | void ClearManifold(PersistentManifold manifold); | ||
35 | bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB); | ||
36 | bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB); | ||
37 | void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo); | ||
38 | PersistentManifold GetManifoldByIndex(int index); | ||
39 | |||
40 | int ManifoldCount { get; } | ||
41 | } | ||
42 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs new file mode 100644 index 0000000..4e128da --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs | |||
@@ -0,0 +1,33 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public interface IOverlapCallback | ||
29 | { | ||
30 | //return true for deletion of the pair | ||
31 | bool ProcessOverlap(ref BroadphasePair pair); | ||
32 | } | ||
33 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs new file mode 100644 index 0000000..165912a --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs | |||
@@ -0,0 +1,159 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public abstract class OverlappingPairCache : IBroadphase | ||
29 | { | ||
30 | private static int _overlappingPairCount = 0; | ||
31 | private List<BroadphasePair> _overlappingPairs = new List<BroadphasePair>(); | ||
32 | //during the dispatch, check that user doesn't destroy/create proxy | ||
33 | private bool _blockedForChanges; | ||
34 | |||
35 | public List<BroadphasePair> OverlappingPairs { get { return _overlappingPairs; } set { _overlappingPairs = value; } } | ||
36 | public bool BlockedForChanges { get { return _blockedForChanges; } set { _blockedForChanges = value; } } | ||
37 | |||
38 | public static int OverlappingPairCount { get { return _overlappingPairCount; } set { _overlappingPairCount = value; } } | ||
39 | |||
40 | public void RemoveOverlappingPair(BroadphasePair pair) | ||
41 | { | ||
42 | if (!_overlappingPairs.Contains(pair)) | ||
43 | return; | ||
44 | |||
45 | CleanOverlappingPair(ref pair); | ||
46 | _overlappingPairs.Remove(pair); | ||
47 | } | ||
48 | |||
49 | public void AddOverlappingPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) | ||
50 | { | ||
51 | //don't add overlap with own | ||
52 | bool test = proxyA != proxyB; | ||
53 | BulletDebug.Assert(proxyA != proxyB); | ||
54 | |||
55 | if (!NeedsBroadphaseCollision(proxyA, proxyB)) | ||
56 | return; | ||
57 | |||
58 | BroadphasePair pair = new BroadphasePair(proxyA, proxyB); | ||
59 | _overlappingPairs.Add(pair); | ||
60 | _overlappingPairCount++; | ||
61 | } | ||
62 | |||
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. | ||
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 | ||
67 | public BroadphasePair FindPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) | ||
68 | { | ||
69 | if (!NeedsBroadphaseCollision(proxyA, proxyB)) | ||
70 | return null; | ||
71 | |||
72 | BroadphasePair pair = new BroadphasePair(proxyA, proxyB); | ||
73 | for (int i = 0; i < _overlappingPairs.Count; i++) | ||
74 | { | ||
75 | if (_overlappingPairs[i] == pair) | ||
76 | { | ||
77 | return _overlappingPairs[i]; | ||
78 | } | ||
79 | } | ||
80 | |||
81 | return null; | ||
82 | } | ||
83 | |||
84 | public void CleanProxyFromPairs(BroadphaseProxy proxy) | ||
85 | { | ||
86 | for (int i = 0; i < _overlappingPairs.Count; i++) | ||
87 | { | ||
88 | BroadphasePair pair = _overlappingPairs[i]; | ||
89 | if (pair.ProxyA == proxy || | ||
90 | pair.ProxyB == proxy) | ||
91 | { | ||
92 | CleanOverlappingPair(ref pair); | ||
93 | _overlappingPairs[i] = pair; | ||
94 | } | ||
95 | } | ||
96 | } | ||
97 | |||
98 | public void RemoveOverlappingPairsContainingProxy(BroadphaseProxy proxy) | ||
99 | { | ||
100 | for (int i = _overlappingPairs.Count - 1; i >= 0; i--) | ||
101 | { | ||
102 | BroadphasePair pair = _overlappingPairs[i]; | ||
103 | if (pair.ProxyA == proxy || | ||
104 | pair.ProxyB == proxy) | ||
105 | { | ||
106 | RemoveOverlappingPair(pair); | ||
107 | i++; | ||
108 | } | ||
109 | } | ||
110 | } | ||
111 | |||
112 | public bool NeedsBroadphaseCollision(BroadphaseProxy proxy0, BroadphaseProxy proxy1) | ||
113 | { | ||
114 | bool collides = (proxy0.CollisionFilterGroup & proxy1.CollisionFilterMask) != 0; | ||
115 | collides = collides && ((proxy1.CollisionFilterGroup & proxy0.CollisionFilterMask) != 0); | ||
116 | |||
117 | return collides; | ||
118 | } | ||
119 | |||
120 | public virtual void ProcessAllOverlappingPairs(IOverlapCallback callback) | ||
121 | { | ||
122 | List<BroadphasePair> deleting = new List<BroadphasePair>(); | ||
123 | for (int i = 0; i < _overlappingPairs.Count; i++) | ||
124 | { | ||
125 | BroadphasePair p = _overlappingPairs[i]; | ||
126 | if (callback.ProcessOverlap(ref p)) | ||
127 | { | ||
128 | CleanOverlappingPair(ref p); | ||
129 | deleting.Add(p); | ||
130 | _overlappingPairCount--; | ||
131 | } | ||
132 | } | ||
133 | |||
134 | for (int i = 0; i < deleting.Count; i++) | ||
135 | _overlappingPairs.Remove(deleting[i]); | ||
136 | } | ||
137 | |||
138 | public void CleanOverlappingPair(ref BroadphasePair pair) | ||
139 | { | ||
140 | if (pair.CollisionAlgorithm != null) | ||
141 | { | ||
142 | if (pair.CollisionAlgorithm is IDisposable) | ||
143 | (pair.CollisionAlgorithm as IDisposable).Dispose(); | ||
144 | pair.CollisionAlgorithm = null; | ||
145 | } | ||
146 | } | ||
147 | |||
148 | public abstract void RefreshOverlappingPairs(); | ||
149 | |||
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); | ||
152 | |||
153 | public abstract void DestroyProxy(BroadphaseProxy proxy); | ||
154 | |||
155 | public abstract void SetAabb(BroadphaseProxy proxy, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax); | ||
156 | |||
157 | #endregion | ||
158 | } | ||
159 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs new file mode 100644 index 0000000..1dc3f34 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs | |||
@@ -0,0 +1,128 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class SimpleBroadphase : OverlappingPairCache | ||
30 | { | ||
31 | private int _maxProxies; | ||
32 | private List<SimpleBroadphaseProxy> _proxies = new List<SimpleBroadphaseProxy>(); | ||
33 | |||
34 | public SimpleBroadphase() | ||
35 | : this(16384) { } | ||
36 | |||
37 | public SimpleBroadphase(int maxProxies) | ||
38 | : base() | ||
39 | { | ||
40 | _maxProxies = maxProxies; | ||
41 | } | ||
42 | |||
43 | public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) | ||
44 | { | ||
45 | if (_proxies.Count >= _maxProxies) | ||
46 | { | ||
47 | BulletDebug.Assert(false); | ||
48 | return null; //should never happen, but don't let the game crash ;-) | ||
49 | } | ||
50 | BulletDebug.Assert(min.X <= max.X && min.Y <= max.Y && min.Z <= max.Z); | ||
51 | |||
52 | SimpleBroadphaseProxy proxy = new SimpleBroadphaseProxy(min, max, shapeType, userData, collisionFilterGroup, collisionFilterMask); | ||
53 | _proxies.Add(proxy); | ||
54 | |||
55 | return proxy; | ||
56 | } | ||
57 | |||
58 | |||
59 | public override void DestroyProxy(BroadphaseProxy proxy) | ||
60 | { | ||
61 | RemoveOverlappingPairsContainingProxy(proxy); | ||
62 | _proxies.Remove(proxy as SimpleBroadphaseProxy); | ||
63 | } | ||
64 | |||
65 | public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax) | ||
66 | { | ||
67 | SimpleBroadphaseProxy simpleProxy = GetSimpleProxyFromProxy(proxy); | ||
68 | simpleProxy.Minimum = aabbMin; | ||
69 | simpleProxy.Maximum = aabbMax; | ||
70 | } | ||
71 | |||
72 | private SimpleBroadphaseProxy GetSimpleProxyFromProxy(BroadphaseProxy proxy) | ||
73 | { | ||
74 | return proxy as SimpleBroadphaseProxy; | ||
75 | } | ||
76 | |||
77 | public override void RefreshOverlappingPairs() | ||
78 | { | ||
79 | for (int i = 0; i < _proxies.Count; i++) | ||
80 | { | ||
81 | SimpleBroadphaseProxy proxyA = _proxies[i]; | ||
82 | |||
83 | for (int j = i + 1; j < _proxies.Count; j++) | ||
84 | { | ||
85 | SimpleBroadphaseProxy proxyB = _proxies[j]; | ||
86 | |||
87 | if (AabbOverlap(proxyA, proxyB)) | ||
88 | { | ||
89 | if (FindPair(proxyA, proxyB) == null) | ||
90 | { | ||
91 | AddOverlappingPair(proxyA, proxyB); | ||
92 | } | ||
93 | } | ||
94 | } | ||
95 | } | ||
96 | |||
97 | CheckOverlapCallback check = new CheckOverlapCallback(); | ||
98 | ProcessAllOverlappingPairs(check); | ||
99 | } | ||
100 | |||
101 | public static bool AabbOverlap(SimpleBroadphaseProxy proxyA, SimpleBroadphaseProxy proxyB) | ||
102 | { | ||
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 && | ||
105 | proxyA.Minimum.Z <= proxyB.Maximum.Z && proxyB.Minimum.Z <= proxyA.Maximum.Z; | ||
106 | } | ||
107 | |||
108 | private void Validate() | ||
109 | { | ||
110 | for (int i = 0; i < _proxies.Count; i++) | ||
111 | { | ||
112 | for (int j = i + 1; j < _proxies.Count; j++) | ||
113 | { | ||
114 | if (_proxies[i] == _proxies[j]) | ||
115 | throw new BulletException(); | ||
116 | } | ||
117 | } | ||
118 | } | ||
119 | } | ||
120 | |||
121 | public class CheckOverlapCallback : IOverlapCallback | ||
122 | { | ||
123 | public bool ProcessOverlap(ref BroadphasePair pair) | ||
124 | { | ||
125 | return (!SimpleBroadphase.AabbOverlap(pair.ProxyA as SimpleBroadphaseProxy, pair.ProxyB as SimpleBroadphaseProxy)); | ||
126 | } | ||
127 | } | ||
128 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs new file mode 100644 index 0000000..f473ef2 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs | |||
@@ -0,0 +1,46 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class SimpleBroadphaseProxy : BroadphaseProxy | ||
30 | { | ||
31 | private Vector3 _min; | ||
32 | private Vector3 _max; | ||
33 | |||
34 | public SimpleBroadphaseProxy() { } | ||
35 | |||
36 | public SimpleBroadphaseProxy(Vector3 minPoint, Vector3 maxPoint, BroadphaseNativeTypes shapeType, object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask) | ||
37 | : base(userData, collisionFilterGroup, collisionFilterMask) | ||
38 | { | ||
39 | _min = minPoint; | ||
40 | _max = maxPoint; | ||
41 | } | ||
42 | |||
43 | public Vector3 Minimum { get { return _min; } set { _min = value; } } | ||
44 | public Vector3 Maximum { get { return _max; } set { _max = value; } } | ||
45 | } | ||
46 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs new file mode 100644 index 0000000..ba638d1 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs | |||
@@ -0,0 +1,59 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | internal class BridgeTriangleRaycastCallback : TriangleRaycastCallback | ||
30 | { | ||
31 | CollisionWorld.RayResultCallback _resultCallback; | ||
32 | CollisionObject _collisionObject; | ||
33 | TriangleMeshShape _triangleMesh; | ||
34 | |||
35 | public BridgeTriangleRaycastCallback(Vector3 from, Vector3 to, | ||
36 | CollisionWorld.RayResultCallback resultCallback, CollisionObject collisionObject, TriangleMeshShape triangleMesh) | ||
37 | : base(from, to) | ||
38 | { | ||
39 | _resultCallback = resultCallback; | ||
40 | _collisionObject = collisionObject; | ||
41 | _triangleMesh = triangleMesh; | ||
42 | } | ||
43 | |||
44 | public override float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex) | ||
45 | { | ||
46 | CollisionWorld.LocalShapeInfo shapeInfo = new CollisionWorld.LocalShapeInfo(); | ||
47 | shapeInfo.ShapePart = partId; | ||
48 | shapeInfo.TriangleIndex = triangleIndex; | ||
49 | |||
50 | CollisionWorld.LocalRayResult rayResult = new CollisionWorld.LocalRayResult | ||
51 | (_collisionObject, | ||
52 | shapeInfo, | ||
53 | hitNormalLocal, | ||
54 | hitFraction); | ||
55 | |||
56 | return _resultCallback.AddSingleResult(rayResult); | ||
57 | } | ||
58 | } | ||
59 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs new file mode 100644 index 0000000..4e3903b --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs | |||
@@ -0,0 +1,40 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class CollisionAlgorithmCreateFunction | ||
30 | { | ||
31 | private bool _swapped; | ||
32 | |||
33 | public bool IsSwapped { get { return _swapped; } set { _swapped = value; } } | ||
34 | |||
35 | public virtual CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) | ||
36 | { | ||
37 | return null; | ||
38 | } | ||
39 | } | ||
40 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs new file mode 100644 index 0000000..75d35b0 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs | |||
@@ -0,0 +1,280 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public delegate void NearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo); | ||
30 | |||
31 | public class CollisionDispatcher : IDispatcher | ||
32 | { | ||
33 | private List<PersistentManifold> _manifolds = new List<PersistentManifold>(); | ||
34 | |||
35 | //private bool _useIslands; | ||
36 | private NearCallback _nearCallback; | ||
37 | |||
38 | //private ManifoldResult _defaultManifoldResult; | ||
39 | |||
40 | private CollisionAlgorithmCreateFunction[,] _doubleDispatch = new CollisionAlgorithmCreateFunction[(int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes, (int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes]; | ||
41 | |||
42 | //default CreationFunctions, filling the m_doubleDispatch table | ||
43 | private CollisionAlgorithmCreateFunction _convexConvexCreateFunc; | ||
44 | private CollisionAlgorithmCreateFunction _convexConcaveCreateFunc; | ||
45 | private CollisionAlgorithmCreateFunction _swappedConvexConcaveCreateFunc; | ||
46 | private CollisionAlgorithmCreateFunction _compoundCreateFunc; | ||
47 | private CollisionAlgorithmCreateFunction _swappedCompoundCreateFunc; | ||
48 | private CollisionAlgorithmCreateFunction _emptyCreateFunc; | ||
49 | |||
50 | private int _count; | ||
51 | private static int _manifoldCount = 0; | ||
52 | |||
53 | public CollisionDispatcher() | ||
54 | { | ||
55 | NearCallback = DefaultNearCallback; | ||
56 | //_useIslands = true; | ||
57 | //default CreationFunctions, filling the m_doubleDispatch table | ||
58 | _convexConvexCreateFunc = new ConvexConvexAlgorithm.CreateFunc(); | ||
59 | _convexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.CreateFunc(); | ||
60 | _swappedConvexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.SwappedCreateFunc(); | ||
61 | _compoundCreateFunc = new CompoundCollisionAlgorithm.CreateFunc(); | ||
62 | _swappedCompoundCreateFunc = new CompoundCollisionAlgorithm.SwappedCreateFunc(); | ||
63 | _emptyCreateFunc = new EmptyAlgorithm.CreateFunc(); | ||
64 | |||
65 | for (BroadphaseNativeTypes i = BroadphaseNativeTypes.Box; i < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; i++) | ||
66 | { | ||
67 | for (BroadphaseNativeTypes j = BroadphaseNativeTypes.Box; j < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; j++) | ||
68 | { | ||
69 | _doubleDispatch[(int)i, (int)j] = FindCreateFunction(i, j); | ||
70 | if (_doubleDispatch[(int)i, (int)j] == null) | ||
71 | throw new BulletException(); | ||
72 | } | ||
73 | } | ||
74 | } | ||
75 | |||
76 | public int Count { get { return _count; } set { _count = value; } } | ||
77 | public int ManifoldCount { get { return _manifolds.Count; } } | ||
78 | public List<PersistentManifold> Manifolds { get { return _manifolds; } } | ||
79 | |||
80 | public static int GlobalManifoldCount { get { return _manifoldCount; } set { _manifoldCount = value; } } | ||
81 | |||
82 | public PersistentManifold GetManifoldByIndex(int index) | ||
83 | { | ||
84 | return _manifolds[index]; | ||
85 | } | ||
86 | |||
87 | //registerCollisionCreateFunc allows registration of custom/alternative collision create functions | ||
88 | public void RegisterCollisionCreateFunc(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB, CollisionAlgorithmCreateFunction createFunc) | ||
89 | { | ||
90 | _doubleDispatch[(int)proxyTypeA, (int)proxyTypeB] = createFunc; | ||
91 | } | ||
92 | |||
93 | public virtual PersistentManifold GetNewManifold(object bodyA, object bodyB) | ||
94 | { | ||
95 | _manifoldCount++; | ||
96 | |||
97 | CollisionObject body0 = bodyA as CollisionObject; | ||
98 | CollisionObject body1 = bodyB as CollisionObject; | ||
99 | |||
100 | PersistentManifold manifold = new PersistentManifold(body0, body1); | ||
101 | _manifolds.Add(manifold); | ||
102 | |||
103 | return manifold; | ||
104 | } | ||
105 | |||
106 | public virtual void ReleaseManifold(PersistentManifold manifold) | ||
107 | { | ||
108 | _manifoldCount--; | ||
109 | |||
110 | ClearManifold(manifold); | ||
111 | _manifolds.Remove(manifold); | ||
112 | } | ||
113 | |||
114 | |||
115 | public virtual void ClearManifold(PersistentManifold manifold) | ||
116 | { | ||
117 | manifold.ClearManifold(); | ||
118 | } | ||
119 | |||
120 | public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB) | ||
121 | { | ||
122 | return FindAlgorithm(bodyA, bodyB, null); | ||
123 | } | ||
124 | |||
125 | public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold) | ||
126 | { | ||
127 | CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo(); | ||
128 | collisionAlgorithmConstructionInfo.Dispatcher = this; | ||
129 | collisionAlgorithmConstructionInfo.Manifold = sharedManifold; | ||
130 | CollisionAlgorithm collisionAlgorithm = _doubleDispatch[(int)bodyA.CollisionShape.ShapeType, (int)bodyB.CollisionShape.ShapeType].CreateCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB); | ||
131 | return collisionAlgorithm; | ||
132 | } | ||
133 | |||
134 | /*public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1) | ||
135 | { | ||
136 | return internalFindAlgorithm(body0, body1, null); | ||
137 | } | ||
138 | |||
139 | public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold) | ||
140 | { | ||
141 | m_count++; | ||
142 | |||
143 | CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo(); | ||
144 | ci.m_dispatcher = this; | ||
145 | |||
146 | if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConvex()) | ||
147 | { | ||
148 | return new ConvexConvexAlgorithm(sharedManifold, ci, body0, body1); | ||
149 | } | ||
150 | |||
151 | if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConcave()) | ||
152 | { | ||
153 | return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, false); | ||
154 | } | ||
155 | |||
156 | if (body1.getCollisionShape().isConvex() && body0.getCollisionShape().isConcave()) | ||
157 | { | ||
158 | return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, true); | ||
159 | } | ||
160 | |||
161 | if (body0.getCollisionShape().isCompound()) | ||
162 | { | ||
163 | return new CompoundCollisionAlgorithm(ci, body0, body1, false); | ||
164 | } | ||
165 | else | ||
166 | { | ||
167 | if (body1.getCollisionShape().isCompound()) | ||
168 | { | ||
169 | return new CompoundCollisionAlgorithm(ci, body0, body1, true); | ||
170 | } | ||
171 | } | ||
172 | |||
173 | //failed to find an algorithm | ||
174 | return new EmptyAlgorithm(ci); | ||
175 | }*/ | ||
176 | |||
177 | public virtual bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB) | ||
178 | { | ||
179 | if (bodyA == null || bodyB == null) | ||
180 | throw new BulletException(); | ||
181 | |||
182 | bool needsCollision = true; | ||
183 | |||
184 | //broadphase filtering already deals with this | ||
185 | /*if ((body0.isStaticObject() || body0.isKinematicObject()) && | ||
186 | (body1.isStaticObject() || body1.isKinematicObject())) | ||
187 | { | ||
188 | printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); | ||
189 | }*/ | ||
190 | |||
191 | if ((!bodyA.IsActive) && (!bodyB.IsActive)) | ||
192 | needsCollision = false; | ||
193 | |||
194 | return needsCollision; | ||
195 | } | ||
196 | |||
197 | public virtual bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB) | ||
198 | { | ||
199 | //here you can do filtering | ||
200 | bool hasResponse = bodyA.HasContactResponse && bodyB.HasContactResponse; | ||
201 | hasResponse = hasResponse && (!bodyA.IsStaticOrKinematicObject || !bodyB.IsStaticOrKinematicObject); | ||
202 | return hasResponse; | ||
203 | } | ||
204 | |||
205 | public virtual void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo) | ||
206 | { | ||
207 | CollisionPairCallback collisionCallback = new CollisionPairCallback(dispatchInfo, this); | ||
208 | pairCache.ProcessAllOverlappingPairs(collisionCallback); | ||
209 | } | ||
210 | |||
211 | private CollisionAlgorithmCreateFunction FindCreateFunction(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB) | ||
212 | { | ||
213 | if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConvex(proxyTypeB)) | ||
214 | { | ||
215 | return _convexConvexCreateFunc; | ||
216 | } | ||
217 | |||
218 | if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConcave(proxyTypeB)) | ||
219 | { | ||
220 | return _convexConcaveCreateFunc; | ||
221 | } | ||
222 | |||
223 | if (BroadphaseProxy.IsConvex(proxyTypeB) && BroadphaseProxy.IsConcave(proxyTypeA)) | ||
224 | { | ||
225 | return _swappedConvexConcaveCreateFunc; | ||
226 | } | ||
227 | |||
228 | if (BroadphaseProxy.IsCompound(proxyTypeA)) | ||
229 | { | ||
230 | return _compoundCreateFunc; | ||
231 | } | ||
232 | else | ||
233 | { | ||
234 | if (BroadphaseProxy.IsCompound(proxyTypeB)) | ||
235 | { | ||
236 | return _swappedCompoundCreateFunc; | ||
237 | } | ||
238 | } | ||
239 | |||
240 | //failed to find an algorithm | ||
241 | return _emptyCreateFunc; | ||
242 | } | ||
243 | |||
244 | public NearCallback NearCallback { get { return _nearCallback; } set { _nearCallback = value; } } | ||
245 | |||
246 | //by default, Bullet will use this near callback | ||
247 | public static void DefaultNearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo) | ||
248 | { | ||
249 | CollisionObject collisionObjectA = collisionPair.ProxyA.ClientData as CollisionObject; | ||
250 | CollisionObject collisionObjectB = collisionPair.ProxyB.ClientData as CollisionObject; | ||
251 | |||
252 | if (dispatcher.NeedsCollision(collisionObjectA, collisionObjectB)) | ||
253 | { | ||
254 | //dispatcher will keep algorithms persistent in the collision pair | ||
255 | if (collisionPair.CollisionAlgorithm == null) | ||
256 | { | ||
257 | collisionPair.CollisionAlgorithm = dispatcher.FindAlgorithm(collisionObjectA, collisionObjectB); | ||
258 | } | ||
259 | |||
260 | if (collisionPair.CollisionAlgorithm != null) | ||
261 | { | ||
262 | ManifoldResult contactPointResult = new ManifoldResult(collisionObjectA, collisionObjectB); | ||
263 | |||
264 | if (dispatchInfo.DispatchFunction == DispatchFunction.Discrete) | ||
265 | { | ||
266 | //discrete collision detection query | ||
267 | collisionPair.CollisionAlgorithm.ProcessCollision(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult); | ||
268 | } | ||
269 | else | ||
270 | { | ||
271 | //continuous collision detection query, time of impact (toi) | ||
272 | float timeOfImpact = collisionPair.CollisionAlgorithm.CalculateTimeOfImpact(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult); | ||
273 | if (dispatchInfo.TimeOfImpact > timeOfImpact) | ||
274 | dispatchInfo.TimeOfImpact = timeOfImpact; | ||
275 | } | ||
276 | } | ||
277 | } | ||
278 | } | ||
279 | } | ||
280 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs new file mode 100644 index 0000000..4e9cf58 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs | |||
@@ -0,0 +1,163 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public enum ActivationState | ||
30 | { | ||
31 | Nothing = 0, | ||
32 | Active, | ||
33 | IslandSleeping, | ||
34 | WantsDeactivation, | ||
35 | DisableDeactivation, | ||
36 | DisableSimulation, | ||
37 | } | ||
38 | |||
39 | public enum CollisionOptions | ||
40 | { | ||
41 | StaticObject = 1, | ||
42 | KinematicObject = 2, | ||
43 | NoContactResponse = 4, | ||
44 | CustomMaterialCallback = 8,//this allows per-triangle material (friction/restitution) | ||
45 | } | ||
46 | |||
47 | /// <summary> | ||
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. | ||
50 | /// They can be added to the btCollisionWorld. | ||
51 | /// </summary> | ||
52 | public class CollisionObject | ||
53 | { | ||
54 | protected Matrix _worldTransform; | ||
55 | private BroadphaseProxy _broadphase; | ||
56 | private CollisionShape _collisionShape; | ||
57 | |||
58 | //m_interpolationWorldTransform is used for CCD and interpolation | ||
59 | //it can be either previous or future (predicted) transform | ||
60 | private Matrix _interpolationWorldTransform; | ||
61 | |||
62 | private CollisionOptions _collisionFlags; | ||
63 | |||
64 | private int _islandTag; | ||
65 | private ActivationState _activationState; | ||
66 | private float _deactivationTime; | ||
67 | |||
68 | private float _friction; | ||
69 | private float _restitution; | ||
70 | |||
71 | //users can point to their objects, m_userPointer is not used by Bullet | ||
72 | private object _userData; | ||
73 | |||
74 | //m_internalOwner one is used by optional Bullet high level interface | ||
75 | private object _internalOwner; | ||
76 | |||
77 | //time of impact calculation | ||
78 | private float _hitFraction; | ||
79 | |||
80 | //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: | ||
81 | private float _ccdSweptSphereRadius; | ||
82 | |||
83 | // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold | ||
84 | private float _ccdSquareMotionThreshold; | ||
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) | ||
87 | private Vector3 _interpolationLinearVelocity; | ||
88 | private Vector3 _interpolationAngularVelocity; | ||
89 | |||
90 | private int _companionID; | ||
91 | |||
92 | public CollisionObject() | ||
93 | { | ||
94 | _activationState = ActivationState.Active; | ||
95 | _hitFraction = 1; | ||
96 | } | ||
97 | |||
98 | public bool IsStaticObject { get { return (_collisionFlags & CollisionOptions.StaticObject) != 0; } } | ||
99 | public bool IsKinematicObject { get { return (_collisionFlags & CollisionOptions.KinematicObject) != 0; } } | ||
100 | public bool IsStaticOrKinematicObject { get { return (_collisionFlags & (CollisionOptions.KinematicObject | CollisionOptions.StaticObject)) != 0; } } | ||
101 | |||
102 | public bool HasContactResponse { get { return (_collisionFlags & CollisionOptions.NoContactResponse) == 0; } } | ||
103 | public bool MergesSimulationIslands | ||
104 | { | ||
105 | get | ||
106 | { | ||
107 | //static objects, kinematic and object without contact response don't merge islands | ||
108 | return (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject | CollisionOptions.NoContactResponse)) == 0; | ||
109 | } | ||
110 | } | ||
111 | |||
112 | public ActivationState ActivationState | ||
113 | { | ||
114 | get { return _activationState; } | ||
115 | set | ||
116 | { | ||
117 | if ((_activationState != ActivationState.DisableDeactivation) && (_activationState != ActivationState.DisableSimulation)) | ||
118 | _activationState = value; | ||
119 | } | ||
120 | } | ||
121 | |||
122 | public bool IsActive { get { return ((ActivationState != ActivationState.IslandSleeping) && (ActivationState != ActivationState.DisableSimulation)); } } | ||
123 | public float Restitution { get { return _restitution; } set { _restitution = value; } } | ||
124 | public float Friction { get { return _friction; } set { _friction = value; } } | ||
125 | public CollisionShape CollisionShape { get { return _collisionShape; } set { _collisionShape = value; } } | ||
126 | public float DeactivationTime { get { return _deactivationTime; } set { _deactivationTime = value; } } | ||
127 | public object Owner { get { return _internalOwner; } protected set { _internalOwner = value; } } | ||
128 | public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } } | ||
129 | public BroadphaseProxy Broadphase { get { return _broadphase; } set { _broadphase = value; } } | ||
130 | public Matrix InterpolationWorldTransform { get { return _interpolationWorldTransform; } set { _interpolationWorldTransform = value; } } | ||
131 | public Vector3 InterpolationLinearVelocity { get { return _interpolationLinearVelocity; } protected set { _interpolationLinearVelocity = value; } } | ||
132 | public Vector3 InterpolationAngularVelocity { get { return _interpolationAngularVelocity; } protected set { _interpolationAngularVelocity = value; } } | ||
133 | public int IslandTag { get { return _islandTag; } set { _islandTag = value; } } | ||
134 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } | ||
135 | public CollisionOptions CollisionFlags { get { return _collisionFlags; } set { _collisionFlags = value; } } | ||
136 | //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm | ||
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 | ||
139 | public float CcdSquareMotionThreshold { get { return _ccdSquareMotionThreshold; } set { _ccdSquareMotionThreshold = value; } } | ||
140 | //users can point to their objects, userPointer is not used by Bullet | ||
141 | public object UserData { get { return _userData; } set { _userData = value; } } | ||
142 | public int CompanionID { get { return _companionID; } set { _companionID = value; } } | ||
143 | |||
144 | public void ForceActivationState(ActivationState newState) | ||
145 | { | ||
146 | _activationState = newState; | ||
147 | } | ||
148 | |||
149 | public void Activate() | ||
150 | { | ||
151 | Activate(false); | ||
152 | } | ||
153 | |||
154 | public void Activate(bool forceActivation) | ||
155 | { | ||
156 | if (forceActivation || (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject)) == 0) | ||
157 | { | ||
158 | ActivationState = ActivationState.Active; | ||
159 | _deactivationTime = 0; | ||
160 | } | ||
161 | } | ||
162 | } | ||
163 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs new file mode 100644 index 0000000..7a85731 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs | |||
@@ -0,0 +1,48 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class CollisionPairCallback : IOverlapCallback | ||
30 | { | ||
31 | private DispatcherInfo _dispatchInfo; | ||
32 | private CollisionDispatcher _dispatcher; | ||
33 | |||
34 | public CollisionPairCallback(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher) | ||
35 | { | ||
36 | _dispatchInfo = dispatchInfo; | ||
37 | _dispatcher = dispatcher; | ||
38 | } | ||
39 | |||
40 | #region IOverlapCallback Members | ||
41 | public bool ProcessOverlap(ref BroadphasePair pair) | ||
42 | { | ||
43 | _dispatcher.NearCallback(ref pair, _dispatcher, _dispatchInfo); | ||
44 | return false; | ||
45 | } | ||
46 | #endregion | ||
47 | } | ||
48 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs new file mode 100644 index 0000000..968e219 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs | |||
@@ -0,0 +1,358 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class CollisionWorld | ||
30 | { | ||
31 | private List<CollisionObject> _collisionObjects = new List<CollisionObject>(); | ||
32 | private IDispatcher _dispatcher; | ||
33 | private OverlappingPairCache _broadphasePairCache; | ||
34 | private bool _ownsDispatcher; | ||
35 | private bool _ownsBroadphasePairCache; | ||
36 | private DispatcherInfo _dispatchInfo = new DispatcherInfo(); | ||
37 | |||
38 | /// <summary> | ||
39 | /// this constructor doesn't own the dispatcher and paircache/broadphase | ||
40 | /// </summary> | ||
41 | /// <param name="dispatcher"></param> | ||
42 | /// <param name="pairCache"></param> | ||
43 | public CollisionWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) | ||
44 | { | ||
45 | _dispatcher = dispatcher; | ||
46 | _broadphasePairCache = pairCache; | ||
47 | _ownsDispatcher = false; | ||
48 | _ownsBroadphasePairCache = false; | ||
49 | } | ||
50 | |||
51 | public DispatcherInfo DispatchInfo { get { return _dispatchInfo; } protected set { _dispatchInfo = value; } } | ||
52 | public List<CollisionObject> CollisionObjects { get { return _collisionObjects; } protected set { _collisionObjects = value; } } | ||
53 | public IBroadphase Broadphase { get { return _broadphasePairCache; } } | ||
54 | public OverlappingPairCache BroadphasePairCache { get { return _broadphasePairCache; } protected set { _broadphasePairCache = value; } } | ||
55 | public IDispatcher Dispatcher { get { return _dispatcher; } protected set { _dispatcher = value; } } | ||
56 | public int CollisionObjectsCount { get { return _collisionObjects.Count; } } | ||
57 | protected bool OwnsDispatcher { get { return _ownsDispatcher; } set { _ownsDispatcher = value; } } | ||
58 | protected bool OwnsBroadphasePairCache { get { return _ownsBroadphasePairCache; } set { _ownsBroadphasePairCache = value; } } | ||
59 | |||
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. | ||
62 | public void RayTest(Vector3 rayFromWorld, Vector3 rayToWorld, RayResultCallback resultCallback) | ||
63 | { | ||
64 | Matrix rayFromTrans, rayToTrans; | ||
65 | |||
66 | rayFromTrans = Matrix.Identity; | ||
67 | rayFromTrans.Translation = rayFromWorld; | ||
68 | |||
69 | rayToTrans = Matrix.Identity; | ||
70 | rayToTrans.Translation = rayToWorld; | ||
71 | |||
72 | // brute force go over all objects. Once there is a broadphase, use that, or | ||
73 | // add a raycast against aabb first. | ||
74 | |||
75 | foreach (CollisionObject collisionObject in _collisionObjects) | ||
76 | { | ||
77 | //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); | ||
78 | Vector3 collisionObjectAabbMin, collisionObjectAabbMax; | ||
79 | collisionObject.CollisionShape.GetAabb(collisionObject.WorldTransform, out collisionObjectAabbMin, out collisionObjectAabbMax); | ||
80 | |||
81 | float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing | ||
82 | Vector3 hitNormal = new Vector3(); | ||
83 | |||
84 | //if (MathHelper.TestAabbAgainstAabb2(rayAabbMin, rayAabbMax, collisionObjectAabbMin, collisionObjectAabbMax)) | ||
85 | if (MathHelper.RayAabb(rayFromWorld, rayToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) | ||
86 | { | ||
87 | RayTestSingle(rayFromTrans, rayToTrans, | ||
88 | collisionObject, collisionObject.CollisionShape, | ||
89 | collisionObject.WorldTransform, resultCallback); | ||
90 | |||
91 | } | ||
92 | } | ||
93 | } | ||
94 | |||
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. | ||
97 | // This allows more customization. | ||
98 | public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans, | ||
99 | CollisionObject collisionObject, | ||
100 | CollisionShape collisionShape, | ||
101 | Matrix colObjWorldTransform, | ||
102 | RayResultCallback resultCallback) | ||
103 | { | ||
104 | SphereShape pointShape=new SphereShape(0.0f); | ||
105 | |||
106 | if (collisionShape.IsConvex) | ||
107 | { | ||
108 | CastResult castResult = new CastResult(); | ||
109 | castResult.Fraction = 1f;//?? | ||
110 | |||
111 | ConvexShape convexShape = collisionShape as ConvexShape; | ||
112 | VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); | ||
113 | SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver); | ||
114 | //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); | ||
115 | //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); | ||
116 | |||
117 | if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) | ||
118 | { | ||
119 | //add hit | ||
120 | if (castResult.Normal.LengthSquared() > 0.0001f) | ||
121 | { | ||
122 | castResult.Normal.Normalize(); | ||
123 | if (castResult.Fraction < resultCallback.ClosestHitFraction) | ||
124 | { | ||
125 | |||
126 | CollisionWorld.LocalRayResult localRayResult = new LocalRayResult | ||
127 | ( | ||
128 | collisionObject, | ||
129 | new LocalShapeInfo(), | ||
130 | castResult.Normal, | ||
131 | castResult.Fraction | ||
132 | ); | ||
133 | |||
134 | resultCallback.AddSingleResult(localRayResult); | ||
135 | } | ||
136 | } | ||
137 | } | ||
138 | else | ||
139 | { | ||
140 | if (collisionShape.IsConcave) | ||
141 | { | ||
142 | |||
143 | TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape; | ||
144 | |||
145 | Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform); | ||
146 | |||
147 | Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject); | ||
148 | Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject); | ||
149 | |||
150 | BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh); | ||
151 | rcb.HitFraction = resultCallback.ClosestHitFraction; | ||
152 | |||
153 | Vector3 rayAabbMinLocal = rayFromLocal; | ||
154 | MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal); | ||
155 | Vector3 rayAabbMaxLocal = rayFromLocal; | ||
156 | MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal); | ||
157 | |||
158 | triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal); | ||
159 | } | ||
160 | else | ||
161 | { | ||
162 | //todo: use AABB tree or other BVH acceleration structure! | ||
163 | if (collisionShape.IsCompound) | ||
164 | { | ||
165 | CompoundShape compoundShape = collisionShape as CompoundShape; | ||
166 | for (int i = 0; i < compoundShape.ChildShapeCount; i++) | ||
167 | { | ||
168 | Matrix childTrans = compoundShape.GetChildTransform(i); | ||
169 | CollisionShape childCollisionShape = compoundShape.GetChildShape(i); | ||
170 | Matrix childWorldTrans = colObjWorldTransform * childTrans; | ||
171 | RayTestSingle(rayFromTrans, rayToTrans, | ||
172 | collisionObject, | ||
173 | childCollisionShape, | ||
174 | childWorldTrans, | ||
175 | resultCallback); | ||
176 | } | ||
177 | } | ||
178 | } | ||
179 | } | ||
180 | } | ||
181 | } | ||
182 | |||
183 | public void AddCollisionObject(CollisionObject collisionObject, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) | ||
184 | { | ||
185 | //check that the object isn't already added | ||
186 | if (!_collisionObjects.Contains(collisionObject)) | ||
187 | { | ||
188 | _collisionObjects.Add(collisionObject); | ||
189 | |||
190 | //calculate new AABB | ||
191 | Matrix trans = collisionObject.WorldTransform; | ||
192 | |||
193 | Vector3 minAabb; | ||
194 | Vector3 maxAabb; | ||
195 | collisionObject.CollisionShape.GetAabb(trans, out minAabb, out maxAabb); | ||
196 | |||
197 | BroadphaseNativeTypes type = collisionObject.CollisionShape.ShapeType; | ||
198 | collisionObject.Broadphase = Broadphase.CreateProxy( | ||
199 | minAabb, | ||
200 | maxAabb, | ||
201 | type, | ||
202 | collisionObject, | ||
203 | collisionFilterGroup, | ||
204 | collisionFilterMask | ||
205 | ); | ||
206 | } | ||
207 | } | ||
208 | |||
209 | public void AddCollisionObject(CollisionObject collisionObject) | ||
210 | { | ||
211 | AddCollisionObject(collisionObject, BroadphaseProxy.CollisionFilterGroups.Default, BroadphaseProxy.CollisionFilterGroups.Default); | ||
212 | } | ||
213 | |||
214 | public void RemoveCollisionObject(CollisionObject collisionObject) | ||
215 | { | ||
216 | BroadphaseProxy bp = collisionObject.Broadphase; | ||
217 | if (bp != null) | ||
218 | { | ||
219 | // | ||
220 | // only clear the cached algorithms | ||
221 | // | ||
222 | Broadphase.CleanProxyFromPairs(bp); | ||
223 | Broadphase.DestroyProxy(bp); | ||
224 | collisionObject.Broadphase = null; | ||
225 | } | ||
226 | |||
227 | _collisionObjects.Remove(collisionObject); | ||
228 | } | ||
229 | |||
230 | public virtual void PerformDiscreteCollisionDetection() | ||
231 | { | ||
232 | DispatcherInfo dispatchInfo = DispatchInfo; | ||
233 | //update aabb (of all moved objects) | ||
234 | |||
235 | Vector3 aabbMin, aabbMax; | ||
236 | for (int i = 0; i < _collisionObjects.Count; i++) | ||
237 | { | ||
238 | _collisionObjects[i].CollisionShape.GetAabb(_collisionObjects[i].WorldTransform, out aabbMin, out aabbMax); | ||
239 | _broadphasePairCache.SetAabb(_collisionObjects[i].Broadphase, aabbMin, aabbMax); | ||
240 | } | ||
241 | |||
242 | _broadphasePairCache.RefreshOverlappingPairs(); | ||
243 | |||
244 | IDispatcher dispatcher = Dispatcher; | ||
245 | if (dispatcher != null) | ||
246 | dispatcher.DispatchAllCollisionPairs(_broadphasePairCache, dispatchInfo); | ||
247 | } | ||
248 | |||
249 | public void Dispose(bool disposing) | ||
250 | { | ||
251 | if (disposing) | ||
252 | { | ||
253 | //clean up remaining objects | ||
254 | foreach (CollisionObject collisionObject in _collisionObjects) | ||
255 | { | ||
256 | BroadphaseProxy bp = collisionObject.Broadphase; | ||
257 | if (bp != null) | ||
258 | { | ||
259 | // | ||
260 | // only clear the cached algorithms | ||
261 | // | ||
262 | Broadphase.CleanProxyFromPairs(bp); | ||
263 | Broadphase.DestroyProxy(bp); | ||
264 | } | ||
265 | } | ||
266 | } | ||
267 | } | ||
268 | |||
269 | /// <summary> | ||
270 | /// LocalShapeInfo gives extra information for complex shapes | ||
271 | /// Currently, only TriangleMeshShape is available, so it just contains triangleIndex and subpart | ||
272 | /// </summary> | ||
273 | public struct LocalShapeInfo | ||
274 | { | ||
275 | private int _shapePart; | ||
276 | private int _triangleIndex; | ||
277 | |||
278 | public int ShapePart { get { return _shapePart; } set { _shapePart = value; } } | ||
279 | public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } | ||
280 | } | ||
281 | |||
282 | public struct LocalRayResult | ||
283 | { | ||
284 | private CollisionObject _collisionObject; | ||
285 | private LocalShapeInfo _localShapeInfo; | ||
286 | private Vector3 _hitNormalLocal; | ||
287 | private float _hitFraction; | ||
288 | |||
289 | public LocalRayResult(CollisionObject collisionObject, | ||
290 | LocalShapeInfo localShapeInfo, | ||
291 | Vector3 hitNormalLocal, | ||
292 | float hitFraction) | ||
293 | { | ||
294 | _collisionObject = collisionObject; | ||
295 | _localShapeInfo = localShapeInfo; | ||
296 | _hitNormalLocal = hitNormalLocal; | ||
297 | _hitFraction = hitFraction; | ||
298 | } | ||
299 | |||
300 | public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } } | ||
301 | public LocalShapeInfo LocalShapeInfo { get { return _localShapeInfo; } set { _localShapeInfo = value; } } | ||
302 | public Vector3 HitNormalLocal { get { return _hitNormalLocal; } set { _hitNormalLocal = value; } } | ||
303 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } | ||
304 | } | ||
305 | |||
306 | /// <summary> | ||
307 | /// RayResultCallback is used to report new raycast results | ||
308 | /// </summary> | ||
309 | public abstract class RayResultCallback | ||
310 | { | ||
311 | private float _closestHitFraction; | ||
312 | |||
313 | public RayResultCallback() | ||
314 | { | ||
315 | _closestHitFraction = 1; | ||
316 | } | ||
317 | |||
318 | public float ClosestHitFraction { get { return _closestHitFraction; } set { _closestHitFraction = value; } } | ||
319 | public bool HasHit { get { return _closestHitFraction < 1; } } | ||
320 | |||
321 | public abstract float AddSingleResult(LocalRayResult rayResult); | ||
322 | } | ||
323 | |||
324 | public class ClosestRayResultCallback : RayResultCallback | ||
325 | { | ||
326 | private Vector3 _rayFromWorld;//used to calculate hitPointWorld from hitFraction | ||
327 | private Vector3 _rayToWorld; | ||
328 | |||
329 | private Vector3 _hitNormalWorld; | ||
330 | private Vector3 _hitPointWorld; | ||
331 | private CollisionObject _collisionObject; | ||
332 | |||
333 | public ClosestRayResultCallback(Vector3 rayFromWorld, Vector3 rayToWorld) | ||
334 | { | ||
335 | _rayFromWorld = rayFromWorld; | ||
336 | _rayToWorld = rayToWorld; | ||
337 | _collisionObject = null; | ||
338 | } | ||
339 | |||
340 | public Vector3 RayFromWorld { get { return _rayFromWorld; } set { _rayFromWorld = value; } } | ||
341 | public Vector3 RayToWorld { get { return _rayToWorld; } set { _rayToWorld = value; } } | ||
342 | public Vector3 HitNormalWorld { get { return _hitNormalWorld; } set { _hitNormalWorld = value; } } | ||
343 | public Vector3 HitPointWorld { get { return _hitPointWorld; } set { _hitPointWorld = value; } } | ||
344 | public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } } | ||
345 | |||
346 | public override float AddSingleResult(LocalRayResult rayResult) | ||
347 | { | ||
348 | //caller already does the filter on the m_closestHitFraction | ||
349 | //assert(rayResult.m_hitFraction <= m_closestHitFraction); | ||
350 | ClosestHitFraction = rayResult.HitFraction; | ||
351 | _collisionObject = rayResult.CollisionObject; | ||
352 | _hitNormalWorld = Vector3.TransformNormal(rayResult.HitNormalLocal, _collisionObject.WorldTransform); | ||
353 | MathHelper.SetInterpolate3(_rayFromWorld, _rayToWorld, rayResult.HitFraction, ref _hitPointWorld); | ||
354 | return rayResult.HitFraction; | ||
355 | } | ||
356 | } | ||
357 | } | ||
358 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs new file mode 100644 index 0000000..3fec7d0 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs | |||
@@ -0,0 +1,157 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using System.Diagnostics; | ||
26 | using MonoXnaCompactMaths; | ||
27 | |||
28 | namespace XnaDevRu.BulletX | ||
29 | { | ||
30 | public class CompoundCollisionAlgorithm : CollisionAlgorithm | ||
31 | { | ||
32 | private List<CollisionAlgorithm> _childCollisionAlgorithms; | ||
33 | private bool _isSwapped; | ||
34 | |||
35 | public CompoundCollisionAlgorithm( | ||
36 | CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, | ||
37 | CollisionObject bodyA, | ||
38 | CollisionObject bodyB, bool isSwapped) | ||
39 | : base(collisionAlgorithmConstructionInfo) | ||
40 | { | ||
41 | //Begin | ||
42 | _isSwapped = isSwapped; | ||
43 | |||
44 | CollisionObject collisionObject = isSwapped ? bodyB : bodyA; | ||
45 | CollisionObject otherObject = isSwapped ? bodyA : bodyB; | ||
46 | |||
47 | BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); | ||
48 | |||
49 | CompoundShape compoundShape = collisionObject.CollisionShape as CompoundShape; | ||
50 | int childrenNumber = compoundShape.ChildShapeCount; | ||
51 | int index = 0; | ||
52 | |||
53 | _childCollisionAlgorithms = new List<CollisionAlgorithm>(childrenNumber); | ||
54 | |||
55 | for (index = 0; index < childrenNumber; index++) | ||
56 | { | ||
57 | CollisionShape childShape = compoundShape.GetChildShape(index); | ||
58 | CollisionShape orgShape = collisionObject.CollisionShape; | ||
59 | |||
60 | collisionObject.CollisionShape = childShape; | ||
61 | _childCollisionAlgorithms[index] = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(collisionObject, otherObject); | ||
62 | collisionObject.CollisionShape = orgShape; | ||
63 | } | ||
64 | } | ||
65 | |||
66 | public override void ProcessCollision( | ||
67 | CollisionObject bodyA, | ||
68 | CollisionObject bodyB, | ||
69 | DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
70 | { | ||
71 | //Begin | ||
72 | |||
73 | CollisionObject collisionObject = _isSwapped ? bodyB : bodyB; | ||
74 | CollisionObject otherObject = _isSwapped ? bodyA : bodyB; | ||
75 | |||
76 | //Debug.Assert(collisionObject.getCollisionShape().isCompound()); | ||
77 | BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); | ||
78 | |||
79 | CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape; | ||
80 | |||
81 | int childrenNumber = _childCollisionAlgorithms.Count; | ||
82 | |||
83 | for (int i = 0; i < childrenNumber; i++) | ||
84 | { | ||
85 | CompoundShape childShape = compoundShape.GetChildShape(i) as CompoundShape; | ||
86 | |||
87 | Matrix orgTransform = collisionObject.WorldTransform; | ||
88 | CollisionShape orgShape = collisionObject.CollisionShape; | ||
89 | |||
90 | Matrix childTransform = compoundShape.GetChildTransform(i); | ||
91 | Matrix newChildWorld = orgTransform * childTransform; | ||
92 | |||
93 | collisionObject.WorldTransform = newChildWorld; | ||
94 | collisionObject.CollisionShape = childShape; | ||
95 | _childCollisionAlgorithms[i].ProcessCollision(collisionObject, otherObject, dispatchInfo, resultOut); | ||
96 | |||
97 | collisionObject.CollisionShape = orgShape; | ||
98 | collisionObject.WorldTransform = orgTransform; | ||
99 | } | ||
100 | } | ||
101 | |||
102 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
103 | { | ||
104 | CollisionObject collisionObject = _isSwapped ? bodyB : bodyA; | ||
105 | CollisionObject otherObject = _isSwapped ? bodyA : bodyB; | ||
106 | |||
107 | BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); | ||
108 | |||
109 | CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape; | ||
110 | |||
111 | float hitFraction = 1.0f; | ||
112 | |||
113 | for (int i = 0; i < _childCollisionAlgorithms.Count; i++) | ||
114 | { | ||
115 | CollisionShape childShape = compoundShape.GetChildShape(i); | ||
116 | |||
117 | Matrix orgTransform = collisionObject.WorldTransform; | ||
118 | CollisionShape orgShape = collisionObject.CollisionShape; | ||
119 | |||
120 | Matrix childTransform = compoundShape.GetChildTransform(i); | ||
121 | Matrix newChildWorld = orgTransform * childTransform; | ||
122 | collisionObject.WorldTransform = newChildWorld; | ||
123 | |||
124 | collisionObject.CollisionShape = childShape; | ||
125 | float frac = _childCollisionAlgorithms[i].CalculateTimeOfImpact( | ||
126 | collisionObject, otherObject, dispatchInfo, resultOut | ||
127 | ); | ||
128 | |||
129 | if (frac < hitFraction) | ||
130 | { | ||
131 | hitFraction = frac; | ||
132 | } | ||
133 | |||
134 | collisionObject.CollisionShape = orgShape; | ||
135 | collisionObject.WorldTransform = orgTransform; | ||
136 | } | ||
137 | |||
138 | return hitFraction; | ||
139 | } | ||
140 | |||
141 | public class CreateFunc : CollisionAlgorithmCreateFunction | ||
142 | { | ||
143 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
144 | { | ||
145 | return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false); | ||
146 | } | ||
147 | }; | ||
148 | |||
149 | public class SwappedCreateFunc : CollisionAlgorithmCreateFunction | ||
150 | { | ||
151 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
152 | { | ||
153 | return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true); | ||
154 | } | ||
155 | }; | ||
156 | } | ||
157 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs new file mode 100644 index 0000000..c4949f0 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs | |||
@@ -0,0 +1,189 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class ConvexConcaveCollisionAlgorithm : CollisionAlgorithm | ||
30 | { | ||
31 | private bool _isSwapped; | ||
32 | private ConvexTriangleCallback _convexTriangleCallback; | ||
33 | |||
34 | public ConvexConcaveCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) | ||
35 | : base(collisionAlgorithmConstructionInfo) | ||
36 | { | ||
37 | _isSwapped = isSwapped; | ||
38 | _convexTriangleCallback = new ConvexTriangleCallback(collisionAlgorithmConstructionInfo.Dispatcher, bodyA, bodyB, isSwapped); | ||
39 | } | ||
40 | |||
41 | public void ClearCache() | ||
42 | { | ||
43 | _convexTriangleCallback.ClearCache(); | ||
44 | } | ||
45 | |||
46 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
47 | { | ||
48 | CollisionObject convexBody = _isSwapped ? bodyB : bodyA; | ||
49 | CollisionObject triBody = _isSwapped ? bodyA : bodyB; | ||
50 | |||
51 | if (triBody.CollisionShape.IsConcave) | ||
52 | { | ||
53 | CollisionObject triOb = triBody; | ||
54 | ConcaveShape concaveShape = triOb.CollisionShape as ConcaveShape; | ||
55 | |||
56 | if (convexBody.CollisionShape.IsConvex) | ||
57 | { | ||
58 | float collisionMarginTriangle = concaveShape.Margin; | ||
59 | |||
60 | resultOut.SetPersistentManifold(_convexTriangleCallback.Manifold); | ||
61 | _convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut); | ||
62 | |||
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); | ||
65 | |||
66 | _convexTriangleCallback.Manifold.SetBodies(convexBody, triBody); | ||
67 | concaveShape.ProcessAllTriangles(_convexTriangleCallback, _convexTriangleCallback.AabbMin, _convexTriangleCallback.AabbMax); | ||
68 | } | ||
69 | } | ||
70 | } | ||
71 | |||
72 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
73 | { | ||
74 | CollisionObject convexbody = _isSwapped ? bodyB : bodyA; | ||
75 | CollisionObject triBody = _isSwapped ? bodyA : bodyB; | ||
76 | |||
77 | |||
78 | //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) | ||
79 | |||
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... | ||
82 | float squareMot0 = (convexbody.InterpolationWorldTransform.Translation - convexbody.WorldTransform.Translation).LengthSquared(); | ||
83 | if (squareMot0 < convexbody.CcdSquareMotionThreshold) | ||
84 | { | ||
85 | return 1; | ||
86 | } | ||
87 | |||
88 | Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform); | ||
89 | Matrix convexFromLocal = triInv * convexbody.WorldTransform; | ||
90 | Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform; | ||
91 | |||
92 | if (triBody.CollisionShape.IsConcave) | ||
93 | { | ||
94 | Vector3 rayAabbMin = convexFromLocal.Translation; | ||
95 | MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation); | ||
96 | Vector3 rayAabbMax = convexFromLocal.Translation; | ||
97 | MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation); | ||
98 | float ccdRadius0 = convexbody.CcdSweptSphereRadius; | ||
99 | rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); | ||
100 | rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); | ||
101 | |||
102 | float curHitFraction = 1f; //is this available? | ||
103 | LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal, | ||
104 | convexbody.CcdSweptSphereRadius, curHitFraction); | ||
105 | |||
106 | raycastCallback.HitFraction = convexbody.HitFraction; | ||
107 | |||
108 | CollisionObject concavebody = triBody; | ||
109 | |||
110 | ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape; | ||
111 | |||
112 | if (triangleMesh != null) | ||
113 | { | ||
114 | triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax); | ||
115 | } | ||
116 | |||
117 | if (raycastCallback.HitFraction < convexbody.HitFraction) | ||
118 | { | ||
119 | convexbody.HitFraction = raycastCallback.HitFraction; | ||
120 | return raycastCallback.HitFraction; | ||
121 | } | ||
122 | } | ||
123 | |||
124 | return 1; | ||
125 | } | ||
126 | |||
127 | public class CreateFunc : CollisionAlgorithmCreateFunction | ||
128 | { | ||
129 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
130 | { | ||
131 | return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false); | ||
132 | } | ||
133 | } | ||
134 | |||
135 | public class SwappedCreateFunc : CollisionAlgorithmCreateFunction | ||
136 | { | ||
137 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
138 | { | ||
139 | return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true); | ||
140 | } | ||
141 | } | ||
142 | |||
143 | private class LocalTriangleSphereCastCallback : ITriangleCallback | ||
144 | { | ||
145 | private Matrix _ccdSphereFromTrans; | ||
146 | private Matrix _ccdSphereToTrans; | ||
147 | private Matrix _meshTransform; | ||
148 | |||
149 | private float _ccdSphereRadius; | ||
150 | private float _hitFraction; | ||
151 | |||
152 | public LocalTriangleSphereCastCallback(Matrix from, Matrix to, float ccdSphereRadius, float hitFraction) | ||
153 | { | ||
154 | _ccdSphereFromTrans = from; | ||
155 | _ccdSphereToTrans = to; | ||
156 | _ccdSphereRadius = ccdSphereRadius; | ||
157 | _hitFraction = hitFraction; | ||
158 | } | ||
159 | |||
160 | public Matrix CcdSphereFromTrans { get { return _ccdSphereFromTrans; } set { _ccdSphereFromTrans = value; } } | ||
161 | public Matrix CcdSphereToTrans { get { return _ccdSphereToTrans; } set { _ccdSphereToTrans = value; } } | ||
162 | public Matrix MeshTransform { get { return _meshTransform; } set { _meshTransform = value; } } | ||
163 | public float CcdSphereRadius { get { return _ccdSphereRadius; } set { _ccdSphereRadius = value; } } | ||
164 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } | ||
165 | |||
166 | public void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex) | ||
167 | { | ||
168 | //do a swept sphere for now | ||
169 | Matrix ident = Matrix.Identity; | ||
170 | CastResult castResult = new CastResult(); | ||
171 | castResult.Fraction = _hitFraction; | ||
172 | SphereShape pointShape = new SphereShape(_ccdSphereRadius); | ||
173 | TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); | ||
174 | VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); | ||
175 | SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver); | ||
176 | //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); | ||
177 | //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); | ||
178 | //local space? | ||
179 | |||
180 | if (convexCaster.CalcTimeOfImpact(_ccdSphereFromTrans, _ccdSphereToTrans, | ||
181 | ident, ident, castResult)) | ||
182 | { | ||
183 | if (_hitFraction > castResult.Fraction) | ||
184 | _hitFraction = castResult.Fraction; | ||
185 | } | ||
186 | } | ||
187 | } | ||
188 | } | ||
189 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs new file mode 100644 index 0000000..8fa4837 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs | |||
@@ -0,0 +1,193 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public class ConvexConvexAlgorithm : CollisionAlgorithm, IDisposable | ||
29 | { | ||
30 | private const bool DisableCcd = false; | ||
31 | private GjkPairDetector _gjkPairDetector; | ||
32 | private bool _ownManifold; | ||
33 | private PersistentManifold _manifold; | ||
34 | private bool _lowLevelOfDetail; | ||
35 | |||
36 | public ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) | ||
37 | : base(collisionAlgorithmConstructionInfo) | ||
38 | { | ||
39 | _gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver); | ||
40 | _ownManifold = false; | ||
41 | _manifold = manifold; | ||
42 | _lowLevelOfDetail = false; | ||
43 | } | ||
44 | |||
45 | public bool LowLevelOfDetail { get { return _lowLevelOfDetail; } set { _lowLevelOfDetail = value; } } | ||
46 | public bool OwnManifold { get { return _ownManifold; } set { _ownManifold = value; } } | ||
47 | public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } | ||
48 | |||
49 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
50 | { | ||
51 | if (_manifold == null) | ||
52 | { | ||
53 | //swapped? | ||
54 | _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); | ||
55 | _ownManifold = true; | ||
56 | } | ||
57 | resultOut.SetPersistentManifold(_manifold); | ||
58 | |||
59 | ConvexShape min0 = bodyA.CollisionShape as ConvexShape; | ||
60 | ConvexShape min1 = bodyB.CollisionShape as ConvexShape; | ||
61 | |||
62 | GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | ||
63 | |||
64 | //TODO: if (dispatchInfo.m_useContinuous) | ||
65 | _gjkPairDetector.setMinkowskiA(min0); | ||
66 | _gjkPairDetector.setMinkowskiB(min1); | ||
67 | input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold; | ||
68 | input.MaximumDistanceSquared *= input.MaximumDistanceSquared; | ||
69 | |||
70 | // input.m_maximumDistanceSquared = 1e30f; | ||
71 | |||
72 | input.TransformA = bodyA.WorldTransform; | ||
73 | input.TransformB = bodyB.WorldTransform; | ||
74 | |||
75 | _gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw); | ||
76 | } | ||
77 | |||
78 | public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
79 | { | ||
80 | //Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold | ||
81 | |||
82 | //Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold | ||
83 | //col0->m_worldTransform, | ||
84 | float resultFraction = 1f; | ||
85 | |||
86 | float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared(); | ||
87 | float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared(); | ||
88 | |||
89 | if (squareMotA < colA.CcdSquareMotionThreshold && | ||
90 | squareMotB < colB.CcdSquareMotionThreshold) | ||
91 | return resultFraction; | ||
92 | |||
93 | if (DisableCcd) | ||
94 | return 1f; | ||
95 | |||
96 | //An adhoc way of testing the Continuous Collision Detection algorithms | ||
97 | //One object is approximated as a sphere, to simplify things | ||
98 | //Starting in penetration should report no time of impact | ||
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) | ||
101 | |||
102 | // Convex0 against sphere for Convex1 | ||
103 | { | ||
104 | ConvexShape convexA = colA.CollisionShape as ConvexShape; | ||
105 | |||
106 | SphereShape sphereB = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation | ||
107 | CastResult result = new CastResult(); | ||
108 | VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); | ||
109 | //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); | ||
110 | //Simplification, one object is simplified as a sphere | ||
111 | GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex); | ||
112 | //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); | ||
113 | if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform, | ||
114 | colB.WorldTransform, colB.InterpolationWorldTransform, result)) | ||
115 | { | ||
116 | //store result.m_fraction in both bodies | ||
117 | if (colA.HitFraction > result.Fraction) | ||
118 | colA.HitFraction = result.Fraction; | ||
119 | |||
120 | if (colB.HitFraction > result.Fraction) | ||
121 | colB.HitFraction = result.Fraction; | ||
122 | |||
123 | if (resultFraction > result.Fraction) | ||
124 | resultFraction = result.Fraction; | ||
125 | } | ||
126 | } | ||
127 | |||
128 | // Sphere (for convex0) against Convex1 | ||
129 | { | ||
130 | ConvexShape convexB = colB.CollisionShape as ConvexShape; | ||
131 | |||
132 | SphereShape sphereA = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation | ||
133 | CastResult result = new CastResult(); | ||
134 | VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); | ||
135 | //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); | ||
136 | ///Simplification, one object is simplified as a sphere | ||
137 | GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex); | ||
138 | //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); | ||
139 | if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform, | ||
140 | colB.WorldTransform, colB.InterpolationWorldTransform, result)) | ||
141 | { | ||
142 | //store result.m_fraction in both bodies | ||
143 | if (colA.HitFraction > result.Fraction) | ||
144 | colA.HitFraction = result.Fraction; | ||
145 | |||
146 | if (colB.HitFraction > result.Fraction) | ||
147 | colB.HitFraction = result.Fraction; | ||
148 | |||
149 | if (resultFraction > result.Fraction) | ||
150 | resultFraction = result.Fraction; | ||
151 | } | ||
152 | } | ||
153 | return resultFraction; | ||
154 | } | ||
155 | |||
156 | public class CreateFunc : CollisionAlgorithmCreateFunction | ||
157 | { | ||
158 | private IConvexPenetrationDepthSolver _penetrationDepthSolver; | ||
159 | private ISimplexSolver _simplexSolver; | ||
160 | //private bool _ownsSolvers; | ||
161 | |||
162 | public CreateFunc() | ||
163 | { | ||
164 | //_ownsSolvers = true; | ||
165 | _simplexSolver = new VoronoiSimplexSolver(); | ||
166 | _penetrationDepthSolver = new GjkEpaPenetrationDepthSolver(); | ||
167 | } | ||
168 | |||
169 | public CreateFunc(ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) | ||
170 | { | ||
171 | //_ownsSolvers = false; | ||
172 | _simplexSolver = simplexSolver; | ||
173 | _penetrationDepthSolver = penetrationDepthSolver; | ||
174 | } | ||
175 | |||
176 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
177 | { | ||
178 | return new ConvexConvexAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, _simplexSolver, _penetrationDepthSolver); | ||
179 | } | ||
180 | } | ||
181 | |||
182 | #region IDisposable Members | ||
183 | public void Dispose() | ||
184 | { | ||
185 | if (_ownManifold) | ||
186 | { | ||
187 | if (_manifold != null) | ||
188 | Dispatcher.ReleaseManifold(_manifold); | ||
189 | } | ||
190 | } | ||
191 | #endregion | ||
192 | } | ||
193 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs new file mode 100644 index 0000000..5355817 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs | |||
@@ -0,0 +1,130 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class ConvexTriangleCallback : ITriangleCallback, IDisposable | ||
30 | { | ||
31 | private CollisionObject _convexBody; | ||
32 | private CollisionObject _triBody; | ||
33 | |||
34 | private Vector3 _aabbMin; | ||
35 | private Vector3 _aabbMax; | ||
36 | |||
37 | private ManifoldResult _resultOut; | ||
38 | |||
39 | private IDispatcher _dispatcher; | ||
40 | private DispatcherInfo _dispatchInfo; | ||
41 | private float _collisionMarginTriangle; | ||
42 | |||
43 | private int _triangleCount; | ||
44 | |||
45 | private PersistentManifold _manifold; | ||
46 | |||
47 | public ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) | ||
48 | { | ||
49 | _dispatcher = dispatcher; | ||
50 | _dispatchInfo = null; | ||
51 | _convexBody = isSwapped ? bodyB : bodyA; | ||
52 | _triBody = isSwapped ? bodyA : bodyB; | ||
53 | |||
54 | // create the manifold from the dispatcher 'manifold pool' | ||
55 | _manifold = _dispatcher.GetNewManifold(_convexBody, _triBody); | ||
56 | ClearCache(); | ||
57 | } | ||
58 | |||
59 | public Vector3 AabbMin { get { return _aabbMin; } } | ||
60 | public Vector3 AabbMax { get { return _aabbMax; } } | ||
61 | public int TriangleCount { get { return _triangleCount; } set { _triangleCount = value; } } | ||
62 | public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } | ||
63 | |||
64 | public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
65 | { | ||
66 | _dispatchInfo = dispatchInfo; | ||
67 | _collisionMarginTriangle = collisionMarginTriangle; | ||
68 | _resultOut = resultOut; | ||
69 | |||
70 | //recalc aabbs | ||
71 | Matrix convexInTriangleSpace = MathHelper.InvertMatrix(_triBody.WorldTransform) * _convexBody.WorldTransform; | ||
72 | CollisionShape convexShape = _convexBody.CollisionShape; | ||
73 | //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape); | ||
74 | convexShape.GetAabb(convexInTriangleSpace, out _aabbMin, out _aabbMax); | ||
75 | float extraMargin = collisionMarginTriangle; | ||
76 | Vector3 extra = new Vector3(extraMargin, extraMargin, extraMargin); | ||
77 | |||
78 | _aabbMax += extra; | ||
79 | _aabbMin -= extra; | ||
80 | } | ||
81 | |||
82 | public void ClearCache() | ||
83 | { | ||
84 | _dispatcher.ClearManifold(_manifold); | ||
85 | } | ||
86 | |||
87 | #region ITriangleCallback Members | ||
88 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) | ||
89 | { | ||
90 | //aabb filter is already applied! | ||
91 | CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo(); | ||
92 | collisionAlgorithmConstructionInfo.Dispatcher = _dispatcher; | ||
93 | |||
94 | CollisionObject collisionObject = _triBody; | ||
95 | |||
96 | //debug drawing of the overlapping triangles | ||
97 | /*if (m_dispatchInfoPtr && m_dispatchInfoPtr.m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) | ||
98 | { | ||
99 | Vector3 color = new Vector3(255, 255, 0); | ||
100 | btTransform & tr = ob->WorldTransform; | ||
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); | ||
103 | m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]), tr(triangle[0]), color); | ||
104 | }*/ | ||
105 | |||
106 | if (_convexBody.CollisionShape.IsConvex) | ||
107 | { | ||
108 | TriangleShape triangleShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); | ||
109 | triangleShape.Margin=_collisionMarginTriangle; | ||
110 | |||
111 | CollisionShape tempShape = collisionObject.CollisionShape; | ||
112 | collisionObject.CollisionShape = triangleShape; | ||
113 | |||
114 | CollisionAlgorithm collisionAlgorithm = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(_convexBody, _triBody, _manifold); | ||
115 | |||
116 | _resultOut.SetShapeIdentifiers(-1, -1, partID, triangleIndex); | ||
117 | collisionAlgorithm.ProcessCollision(_convexBody, _triBody, _dispatchInfo, _resultOut); | ||
118 | collisionObject.CollisionShape = tempShape; | ||
119 | } | ||
120 | } | ||
121 | #endregion | ||
122 | #region IDisposable Members | ||
123 | public void Dispose() | ||
124 | { | ||
125 | ClearCache(); | ||
126 | _dispatcher.ReleaseManifold(_manifold); | ||
127 | } | ||
128 | #endregion | ||
129 | } | ||
130 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs new file mode 100644 index 0000000..13cb3d9 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs | |||
@@ -0,0 +1,52 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | /// <summary> | ||
29 | /// EmptyAlgorithm is a stub for unsupported collision pairs. | ||
30 | /// The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame. | ||
31 | /// </summary> | ||
32 | public class EmptyAlgorithm : CollisionAlgorithm | ||
33 | { | ||
34 | public EmptyAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) | ||
35 | : base(collisionAlgorithmConstructionInfo) { } | ||
36 | |||
37 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { } | ||
38 | |||
39 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
40 | { | ||
41 | return 1f; | ||
42 | } | ||
43 | |||
44 | public class CreateFunc : CollisionAlgorithmCreateFunction | ||
45 | { | ||
46 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
47 | { | ||
48 | return new EmptyAlgorithm(collisionAlgorithmConstructionInfo); | ||
49 | } | ||
50 | }; | ||
51 | } | ||
52 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs new file mode 100644 index 0000000..63f37ea --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs | |||
@@ -0,0 +1,147 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public delegate bool ContactAddedCallback(ManifoldPoint contactPoint, CollisionObject collisionObjectA, int partIdA, int indexA, CollisionObject collisionObjectB, int partIdB, int indexB); | ||
30 | |||
31 | public class ManifoldResult : DiscreteCollisionDetectorInterface.Result | ||
32 | { | ||
33 | private PersistentManifold _manifold; | ||
34 | private static ContactAddedCallback _contactAddedCallback = null; | ||
35 | |||
36 | //we need this for compounds | ||
37 | private Matrix _rootTransA; | ||
38 | private Matrix _rootTransB; | ||
39 | |||
40 | private CollisionObject _bodyA; | ||
41 | private CollisionObject _bodyB; | ||
42 | private int _partIdA; | ||
43 | private int _partIdB; | ||
44 | private int _indexA; | ||
45 | private int _indexB; | ||
46 | |||
47 | public ManifoldResult() | ||
48 | { | ||
49 | } | ||
50 | |||
51 | public ManifoldResult(CollisionObject bodyA, CollisionObject bodyB) | ||
52 | { | ||
53 | _bodyA = bodyA; | ||
54 | _bodyB = bodyB; | ||
55 | _rootTransA = bodyA.WorldTransform; | ||
56 | _rootTransB = bodyB.WorldTransform; | ||
57 | } | ||
58 | |||
59 | public static ContactAddedCallback ContactAddedCallback { get { return _contactAddedCallback; } set { _contactAddedCallback = value; } } | ||
60 | |||
61 | public void SetPersistentManifold(PersistentManifold manifold) | ||
62 | { | ||
63 | _manifold = manifold; | ||
64 | } | ||
65 | |||
66 | public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB) | ||
67 | { | ||
68 | _partIdA = partIdA; | ||
69 | _partIdB = partIdB; | ||
70 | _indexA = indexA; | ||
71 | _indexB = indexB; | ||
72 | } | ||
73 | |||
74 | public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) | ||
75 | { | ||
76 | if (_manifold == null) | ||
77 | throw new BulletException("Manifold Pointer is null."); | ||
78 | |||
79 | //order in manifold needs to match | ||
80 | |||
81 | if (depth > PersistentManifold.ContactBreakingThreshold) | ||
82 | return; | ||
83 | |||
84 | bool isSwapped = _manifold.BodyA != _bodyA; | ||
85 | |||
86 | Vector3 pointA = pointInWorld + normalOnBInWorld * depth; | ||
87 | Vector3 localA; | ||
88 | Vector3 localB; | ||
89 | |||
90 | if (isSwapped) | ||
91 | { | ||
92 | localA = MathHelper.InvXForm(_rootTransB, pointA); | ||
93 | localB = MathHelper.InvXForm(_rootTransA, pointInWorld); | ||
94 | } | ||
95 | else | ||
96 | { | ||
97 | localA = MathHelper.InvXForm(_rootTransA, pointA); | ||
98 | localB = MathHelper.InvXForm(_rootTransB, pointInWorld); | ||
99 | } | ||
100 | |||
101 | ManifoldPoint newPt = new ManifoldPoint(localA, localB, normalOnBInWorld, depth); | ||
102 | |||
103 | int insertIndex = _manifold.GetCacheEntry(newPt); | ||
104 | |||
105 | newPt.CombinedFriction = CalculateCombinedFriction(_bodyA, _bodyB); | ||
106 | newPt.CombinedRestitution = CalculateCombinedRestitution(_bodyA, _bodyB); | ||
107 | |||
108 | //User can override friction and/or restitution | ||
109 | if (_contactAddedCallback != null && | ||
110 | //and if either of the two bodies requires custom material | ||
111 | ((_bodyA.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0 || | ||
112 | (_bodyB.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0)) | ||
113 | { | ||
114 | //experimental feature info, for per-triangle material etc. | ||
115 | CollisionObject obj0 = isSwapped ? _bodyB : _bodyA; | ||
116 | CollisionObject obj1 = isSwapped ? _bodyA : _bodyB; | ||
117 | _contactAddedCallback(newPt, obj0, _partIdA, _indexA, obj1, _partIdB, _indexB); | ||
118 | } | ||
119 | |||
120 | if (insertIndex >= 0) | ||
121 | { | ||
122 | _manifold.ReplaceContactPoint(newPt, insertIndex); | ||
123 | } | ||
124 | else | ||
125 | { | ||
126 | _manifold.AddManifoldPoint(newPt); | ||
127 | } | ||
128 | } | ||
129 | |||
130 | private float CalculateCombinedFriction(CollisionObject bodyA, CollisionObject bodyB) | ||
131 | { | ||
132 | float friction = bodyA.Friction * bodyB.Friction; | ||
133 | |||
134 | float MaxFriction = 10; | ||
135 | if (friction < -MaxFriction) | ||
136 | friction = -MaxFriction; | ||
137 | if (friction > MaxFriction) | ||
138 | friction = MaxFriction; | ||
139 | return friction; | ||
140 | } | ||
141 | |||
142 | private float CalculateCombinedRestitution(CollisionObject bodyA, CollisionObject bodyB) | ||
143 | { | ||
144 | return bodyA.Restitution * bodyB.Restitution; | ||
145 | } | ||
146 | } | ||
147 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs new file mode 100644 index 0000000..957843b --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs | |||
@@ -0,0 +1,304 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class SimulationIslandManager | ||
30 | { | ||
31 | private UnionFind _unionFind = new UnionFind(); | ||
32 | |||
33 | public void InitUnionFind(int n) | ||
34 | { | ||
35 | _unionFind.Reset(n); | ||
36 | } | ||
37 | |||
38 | public UnionFind UnionFind { get { return _unionFind; } } | ||
39 | |||
40 | public virtual void UpdateActivationState(CollisionWorld world, IDispatcher dispatcher) | ||
41 | { | ||
42 | InitUnionFind(world.CollisionObjectsCount); | ||
43 | |||
44 | // put the index into m_controllers into m_tag | ||
45 | int index = 0; | ||
46 | for (int i = 0; i < world.CollisionObjects.Count; i++) | ||
47 | { | ||
48 | world.CollisionObjects[i].IslandTag = index; | ||
49 | world.CollisionObjects[i].HitFraction = 1; | ||
50 | world.CollisionObjects[i].CompanionID = -1; | ||
51 | index++; | ||
52 | } | ||
53 | // do the union find | ||
54 | FindUnions(dispatcher); | ||
55 | } | ||
56 | |||
57 | public virtual void StoreIslandActivationState(CollisionWorld world) | ||
58 | { | ||
59 | // put the islandId ('find' value) into m_tag | ||
60 | int index = 0; | ||
61 | for (int i = 0; i < world.CollisionObjects.Count; i++) | ||
62 | { | ||
63 | if (world.CollisionObjects[i].MergesSimulationIslands) | ||
64 | { | ||
65 | world.CollisionObjects[i].IslandTag = _unionFind.Find(index); | ||
66 | world.CollisionObjects[i].CompanionID = -1; | ||
67 | } | ||
68 | else | ||
69 | { | ||
70 | world.CollisionObjects[i].IslandTag = -1; | ||
71 | world.CollisionObjects[i].CompanionID = -2; | ||
72 | } | ||
73 | index++; | ||
74 | } | ||
75 | } | ||
76 | |||
77 | public void FindUnions(IDispatcher dispatcher) | ||
78 | { | ||
79 | for (int i = 0; i < dispatcher.ManifoldCount; i++) | ||
80 | { | ||
81 | PersistentManifold manifold = dispatcher.GetManifoldByIndex(i); | ||
82 | //static objects (invmass 0.f) don't merge ! | ||
83 | |||
84 | CollisionObject colObjA = manifold.BodyA as CollisionObject; | ||
85 | CollisionObject colObjB = manifold.BodyB as CollisionObject; | ||
86 | |||
87 | if (((colObjA != null) && (colObjA.MergesSimulationIslands)) && | ||
88 | ((colObjB != null) && (colObjB.MergesSimulationIslands))) | ||
89 | { | ||
90 | _unionFind.Unite(colObjA.IslandTag, colObjB.IslandTag); | ||
91 | } | ||
92 | } | ||
93 | } | ||
94 | |||
95 | public void BuildAndProcessIslands(IDispatcher dispatcher, List<CollisionObject> collisionObjects, IIslandCallback callback) | ||
96 | { | ||
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 | ||
99 | UnionFind.SortIslands(); | ||
100 | int numElem = UnionFind.ElementCount; | ||
101 | |||
102 | int endIslandIndex = 1; | ||
103 | int startIslandIndex; | ||
104 | |||
105 | //update the sleeping state for bodies, if all are sleeping | ||
106 | for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) | ||
107 | { | ||
108 | int islandId = UnionFind[startIslandIndex].ID; | ||
109 | for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++) | ||
110 | { | ||
111 | } | ||
112 | |||
113 | //int numSleeping = 0; | ||
114 | |||
115 | bool allSleeping = true; | ||
116 | |||
117 | int idx; | ||
118 | for (idx = startIslandIndex; idx < endIslandIndex; idx++) | ||
119 | { | ||
120 | int i = UnionFind[idx].Size; | ||
121 | |||
122 | CollisionObject colObjA = collisionObjects[i]; | ||
123 | if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) | ||
124 | { | ||
125 | Console.WriteLine("error in island management"); | ||
126 | } | ||
127 | |||
128 | BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); | ||
129 | if (colObjA.IslandTag == islandId) | ||
130 | { | ||
131 | if (colObjA.ActivationState == ActivationState.Active) | ||
132 | { | ||
133 | allSleeping = false; | ||
134 | } | ||
135 | if (colObjA.ActivationState == ActivationState.DisableDeactivation) | ||
136 | { | ||
137 | allSleeping = false; | ||
138 | } | ||
139 | } | ||
140 | } | ||
141 | |||
142 | |||
143 | if (allSleeping) | ||
144 | { | ||
145 | for (idx = startIslandIndex; idx < endIslandIndex; idx++) | ||
146 | { | ||
147 | int i = UnionFind[idx].Size; | ||
148 | CollisionObject colObjA = collisionObjects[i]; | ||
149 | if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) | ||
150 | { | ||
151 | Console.WriteLine("error in island management"); | ||
152 | } | ||
153 | |||
154 | BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); | ||
155 | |||
156 | if (colObjA.IslandTag == islandId) | ||
157 | { | ||
158 | colObjA.ActivationState =ActivationState.IslandSleeping; | ||
159 | } | ||
160 | } | ||
161 | } | ||
162 | else | ||
163 | { | ||
164 | for (idx = startIslandIndex; idx < endIslandIndex; idx++) | ||
165 | { | ||
166 | int i = UnionFind[idx].Size; | ||
167 | |||
168 | CollisionObject colObjA = collisionObjects[i]; | ||
169 | if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) | ||
170 | { | ||
171 | Console.WriteLine("error in island management"); | ||
172 | } | ||
173 | |||
174 | BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); | ||
175 | |||
176 | if (colObjA.IslandTag == islandId) | ||
177 | { | ||
178 | if (colObjA.ActivationState == ActivationState.IslandSleeping) | ||
179 | { | ||
180 | colObjA.ActivationState = ActivationState.WantsDeactivation; | ||
181 | } | ||
182 | } | ||
183 | } | ||
184 | } | ||
185 | } | ||
186 | |||
187 | //int maxNumManifolds = dispatcher.ManifoldCount; | ||
188 | List<PersistentManifold> islandmanifold = new List<PersistentManifold>(dispatcher.ManifoldCount); | ||
189 | |||
190 | for (int i = 0; i < dispatcher.ManifoldCount; i++) | ||
191 | { | ||
192 | PersistentManifold manifold = dispatcher.GetManifoldByIndex(i); | ||
193 | |||
194 | CollisionObject colObjA = manifold.BodyA as CollisionObject; | ||
195 | CollisionObject colObjB = manifold.BodyB as CollisionObject; | ||
196 | |||
197 | //todo: check sleeping conditions! | ||
198 | if (((colObjA != null) && colObjA.ActivationState != ActivationState.IslandSleeping) || | ||
199 | ((colObjB != null) && colObjB.ActivationState != ActivationState.IslandSleeping)) | ||
200 | { | ||
201 | |||
202 | //kinematic objects don't merge islands, but wake up all connected objects | ||
203 | if (colObjA.IsStaticOrKinematicObject && colObjA.ActivationState != ActivationState.IslandSleeping) | ||
204 | { | ||
205 | colObjB.Activate(); | ||
206 | } | ||
207 | if (colObjB.IsStaticOrKinematicObject && colObjB.ActivationState != ActivationState.IslandSleeping) | ||
208 | { | ||
209 | colObjA.Activate(); | ||
210 | } | ||
211 | |||
212 | //filtering for response | ||
213 | if (dispatcher.NeedsResponse(colObjA, colObjB)) | ||
214 | islandmanifold.Add(manifold); | ||
215 | } | ||
216 | } | ||
217 | |||
218 | int numManifolds = islandmanifold.Count; | ||
219 | |||
220 | // Sort manifolds, based on islands | ||
221 | // Sort the vector using predicate and std::sort | ||
222 | islandmanifold.Sort(new Comparison<PersistentManifold>(PersistentManifoldSortPredicate)); | ||
223 | |||
224 | //now process all active islands (sets of manifolds for now) | ||
225 | int startManifoldIndex = 0; | ||
226 | int endManifoldIndex = 1; | ||
227 | |||
228 | List<CollisionObject> islandBodies = new List<CollisionObject>(); | ||
229 | |||
230 | for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) | ||
231 | { | ||
232 | int islandId = UnionFind[startIslandIndex].ID; | ||
233 | bool islandSleeping = false; | ||
234 | for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++) | ||
235 | { | ||
236 | int i = UnionFind[endIslandIndex].Size; | ||
237 | CollisionObject colObjA = collisionObjects[i]; | ||
238 | islandBodies.Add(colObjA); | ||
239 | if (!colObjA.IsActive) | ||
240 | islandSleeping = true; | ||
241 | } | ||
242 | |||
243 | //find the accompanying contact manifold for this islandId | ||
244 | int numIslandManifolds = 0; | ||
245 | List<PersistentManifold> startManifold = new List<PersistentManifold>(numIslandManifolds); | ||
246 | |||
247 | if (startManifoldIndex < numManifolds) | ||
248 | { | ||
249 | int curIslandID = GetIslandId(islandmanifold[startManifoldIndex]); | ||
250 | if (curIslandID == islandId) | ||
251 | { | ||
252 | for (int k = startManifoldIndex; k < islandmanifold.Count; k++) | ||
253 | { | ||
254 | startManifold.Add(islandmanifold[k]); | ||
255 | } | ||
256 | for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(islandmanifold[endManifoldIndex])); endManifoldIndex++) { } | ||
257 | |||
258 | // Process the actual simulation, only if not sleeping/deactivated | ||
259 | numIslandManifolds = endManifoldIndex - startManifoldIndex; | ||
260 | } | ||
261 | } | ||
262 | |||
263 | if (!islandSleeping) | ||
264 | { | ||
265 | callback.ProcessIsland(islandBodies, startManifold, numIslandManifolds, islandId); | ||
266 | } | ||
267 | |||
268 | if (numIslandManifolds != 0) | ||
269 | { | ||
270 | startManifoldIndex = endManifoldIndex; | ||
271 | } | ||
272 | |||
273 | islandBodies.Clear(); | ||
274 | } | ||
275 | } | ||
276 | |||
277 | private static int GetIslandId(PersistentManifold lhs) | ||
278 | { | ||
279 | int islandId; | ||
280 | CollisionObject rcolObjA = lhs.BodyA as CollisionObject; | ||
281 | CollisionObject rcolObjB = lhs.BodyB as CollisionObject; | ||
282 | islandId = rcolObjA.IslandTag >= 0 ? rcolObjA.IslandTag : rcolObjB.IslandTag; | ||
283 | return islandId; | ||
284 | } | ||
285 | |||
286 | private static int PersistentManifoldSortPredicate(PersistentManifold lhs, PersistentManifold rhs) | ||
287 | { | ||
288 | int rIslandIdA, lIslandIdB; | ||
289 | rIslandIdA = GetIslandId(rhs); | ||
290 | lIslandIdB = GetIslandId(lhs); | ||
291 | //return lIslandId0 < rIslandId0; | ||
292 | if (lIslandIdB < rIslandIdA) | ||
293 | return -1; | ||
294 | //else if (lIslandIdB > rIslandIdA) | ||
295 | // return 1; | ||
296 | return 1; | ||
297 | } | ||
298 | |||
299 | public interface IIslandCallback | ||
300 | { | ||
301 | void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID); | ||
302 | } | ||
303 | } | ||
304 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs new file mode 100644 index 0000000..8933638 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs | |||
@@ -0,0 +1,270 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// SphereBoxCollisionAlgorithm provides sphere-box collision detection. | ||
31 | /// Other features are frame-coherency (persistent data) and collision response. | ||
32 | /// </summary> | ||
33 | public class SphereBoxCollisionAlgorithm : CollisionAlgorithm, IDisposable | ||
34 | { | ||
35 | private bool _ownManifold; | ||
36 | private PersistentManifold _manifold; | ||
37 | private bool _isSwapped; | ||
38 | |||
39 | public SphereBoxCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject collisionObjectA, CollisionObject collisionObjectB, bool isSwapped) | ||
40 | : base(collisionAlgorithmConstructionInfo) | ||
41 | { | ||
42 | _ownManifold = false; | ||
43 | _manifold = manifold; | ||
44 | _isSwapped = isSwapped; | ||
45 | |||
46 | CollisionObject sphereObject = _isSwapped ? collisionObjectB : collisionObjectA; | ||
47 | CollisionObject boxObject = _isSwapped ? collisionObjectA : collisionObjectB; | ||
48 | |||
49 | if (_manifold == null && Dispatcher.NeedsCollision(sphereObject, boxObject)) | ||
50 | { | ||
51 | _manifold = Dispatcher.GetNewManifold(sphereObject, boxObject); | ||
52 | _ownManifold = true; | ||
53 | } | ||
54 | } | ||
55 | |||
56 | public float GetSphereDistance(CollisionObject boxObject, out Vector3 pointOnBox, out Vector3 pointOnSphere, Vector3 sphereCenter, float radius) | ||
57 | { | ||
58 | pointOnBox = new Vector3(); | ||
59 | pointOnSphere = new Vector3(); | ||
60 | |||
61 | float margins; | ||
62 | Vector3[] bounds = new Vector3[2]; | ||
63 | BoxShape boxShape = boxObject.CollisionShape as BoxShape; | ||
64 | |||
65 | bounds[0] = -boxShape.HalfExtents; | ||
66 | bounds[1] = boxShape.HalfExtents; | ||
67 | |||
68 | margins = boxShape.Margin; //also add sphereShape margin? | ||
69 | |||
70 | Matrix m44T = boxObject.WorldTransform; | ||
71 | |||
72 | Vector3[] boundsVec = new Vector3[2]; | ||
73 | float penetration; | ||
74 | |||
75 | boundsVec[0] = bounds[0]; | ||
76 | boundsVec[1] = bounds[1]; | ||
77 | |||
78 | Vector3 marginsVec = new Vector3(margins, margins, margins); | ||
79 | |||
80 | // add margins | ||
81 | bounds[0] += marginsVec; | ||
82 | bounds[1] -= marginsVec; | ||
83 | |||
84 | ///////////////////////////////////////////////// | ||
85 | |||
86 | Vector3 tmp, prel, normal, v3P; | ||
87 | Vector3[] n = new Vector3[6]; | ||
88 | float sep = 10000000.0f, sepThis; | ||
89 | |||
90 | n[0] = new Vector3(-1.0f, 0.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); | ||
93 | n[3] = new Vector3(1.0f, 0.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); | ||
96 | |||
97 | // convert point in local space | ||
98 | prel = MathHelper.InvXForm(m44T, sphereCenter); | ||
99 | |||
100 | bool found = false; | ||
101 | |||
102 | v3P = prel; | ||
103 | |||
104 | for (int i = 0; i < 6; i++) | ||
105 | { | ||
106 | int j = i < 3 ? 0 : 1; | ||
107 | if ((sepThis = (Vector3.Dot(v3P - bounds[j], n[i]))) > 0.0f) | ||
108 | { | ||
109 | v3P = v3P - n[i] * sepThis; | ||
110 | found = true; | ||
111 | } | ||
112 | } | ||
113 | |||
114 | // | ||
115 | |||
116 | if (found) | ||
117 | { | ||
118 | bounds[0] = boundsVec[0]; | ||
119 | bounds[1] = boundsVec[1]; | ||
120 | |||
121 | normal = Vector3.Normalize(prel - v3P); | ||
122 | pointOnBox = v3P + normal * margins; | ||
123 | pointOnSphere = prel - normal * radius; | ||
124 | |||
125 | if ((Vector3.Dot(pointOnSphere - pointOnBox, normal)) > 0.0f) | ||
126 | { | ||
127 | return 1.0f; | ||
128 | } | ||
129 | |||
130 | // transform back in world space | ||
131 | tmp = MathHelper.MatrixToVector(m44T, pointOnBox); | ||
132 | pointOnBox = tmp; | ||
133 | tmp = MathHelper.MatrixToVector(m44T, pointOnSphere); | ||
134 | pointOnSphere = tmp; | ||
135 | float seps2 = (pointOnBox - pointOnSphere).LengthSquared(); | ||
136 | |||
137 | //if this fails, fallback into deeper penetration case, below | ||
138 | if (seps2 > MathHelper.Epsilon) | ||
139 | { | ||
140 | sep = -(float)Math.Sqrt(seps2); | ||
141 | normal = (pointOnBox - pointOnSphere); | ||
142 | normal *= 1f / sep; | ||
143 | } | ||
144 | return sep; | ||
145 | } | ||
146 | |||
147 | ////////////////////////////////////////////////// | ||
148 | // Deep penetration case | ||
149 | |||
150 | penetration = GetSpherePenetration(boxObject, ref pointOnBox, ref pointOnSphere, sphereCenter, radius, bounds[0], bounds[1]); | ||
151 | |||
152 | bounds[0] = boundsVec[0]; | ||
153 | bounds[1] = boundsVec[1]; | ||
154 | |||
155 | if (penetration <= 0.0f) | ||
156 | return (penetration - margins); | ||
157 | else | ||
158 | return 1.0f; | ||
159 | } | ||
160 | |||
161 | public float GetSpherePenetration(CollisionObject boxObject, ref Vector3 pointOnBox, ref Vector3 pointOnSphere, Vector3 sphereCenter, float radius, Vector3 aabbMin, Vector3 aabbMax) | ||
162 | { | ||
163 | Vector3[] bounds = new Vector3[2]; | ||
164 | |||
165 | bounds[0] = aabbMin; | ||
166 | bounds[1] = aabbMax; | ||
167 | |||
168 | Vector3 p0 = new Vector3(), tmp, prel, normal = new Vector3(); | ||
169 | Vector3[] n = new Vector3[6]; | ||
170 | float sep = -10000000.0f, sepThis; | ||
171 | |||
172 | n[0] = new Vector3(-1.0f, 0.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); | ||
175 | n[3] = new Vector3(1.0f, 0.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); | ||
178 | |||
179 | Matrix m44T = boxObject.WorldTransform; | ||
180 | |||
181 | // convert point in local space | ||
182 | prel = MathHelper.InvXForm(m44T, sphereCenter); | ||
183 | |||
184 | /////////// | ||
185 | |||
186 | for (int i = 0; i < 6; i++) | ||
187 | { | ||
188 | int j = i < 3 ? 0 : 1; | ||
189 | if ((sepThis = (Vector3.Dot(prel - bounds[j], n[i])) - radius) > 0.0f) return 1.0f; | ||
190 | if (sepThis > sep) | ||
191 | { | ||
192 | p0 = bounds[j]; | ||
193 | normal = n[i]; | ||
194 | sep = sepThis; | ||
195 | } | ||
196 | } | ||
197 | |||
198 | pointOnBox = prel - normal * (Vector3.Dot(normal, (prel - p0))); | ||
199 | pointOnSphere = pointOnBox + normal * sep; | ||
200 | |||
201 | // transform back in world space | ||
202 | tmp = MathHelper.MatrixToVector(m44T, pointOnBox); | ||
203 | pointOnBox = tmp; | ||
204 | tmp = MathHelper.MatrixToVector(m44T, pointOnSphere); | ||
205 | pointOnSphere = tmp; | ||
206 | normal = Vector3.Normalize(pointOnBox - pointOnSphere); | ||
207 | |||
208 | return sep; | ||
209 | } | ||
210 | |||
211 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
212 | { | ||
213 | if (_manifold == null) | ||
214 | return; | ||
215 | |||
216 | CollisionObject sphereObject = _isSwapped ? bodyB : bodyA; | ||
217 | CollisionObject boxObject = _isSwapped ? bodyA : bodyB; | ||
218 | |||
219 | SphereShape sphereA = sphereObject.CollisionShape as SphereShape; | ||
220 | |||
221 | Vector3 pOnBox, pOnSphere; | ||
222 | Vector3 sphereCenter = sphereObject.WorldTransform.Translation; | ||
223 | float radius = sphereA.Radius; | ||
224 | |||
225 | float dist = GetSphereDistance(boxObject, out pOnBox, out pOnSphere, sphereCenter, radius); | ||
226 | |||
227 | if (dist < MathHelper.Epsilon) | ||
228 | { | ||
229 | Vector3 normalOnSurfaceB = Vector3.Normalize(pOnBox - pOnSphere); | ||
230 | |||
231 | // report a contact. internally this will be kept persistent, and contact reduction is done | ||
232 | resultOut.SetPersistentManifold(_manifold); | ||
233 | resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, dist); | ||
234 | } | ||
235 | } | ||
236 | |||
237 | public override float CalculateTimeOfImpact(CollisionObject collisionObjectA, CollisionObject collisionObjectB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
238 | { | ||
239 | //not yet | ||
240 | return 1; | ||
241 | } | ||
242 | |||
243 | public class CreateFunc : CollisionAlgorithmCreateFunction | ||
244 | { | ||
245 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
246 | { | ||
247 | if (!IsSwapped) | ||
248 | return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, false); | ||
249 | else | ||
250 | return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, true); | ||
251 | } | ||
252 | } | ||
253 | |||
254 | #region IDisposable Members | ||
255 | public void Dispose() | ||
256 | { | ||
257 | Dispose(true); | ||
258 | } | ||
259 | |||
260 | public void Dispose(bool disposing) | ||
261 | { | ||
262 | if (disposing && _ownManifold) | ||
263 | { | ||
264 | if (_manifold != null) | ||
265 | Dispatcher.ReleaseManifold(_manifold); | ||
266 | } | ||
267 | } | ||
268 | #endregion | ||
269 | } | ||
270 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs new file mode 100644 index 0000000..7a76d24 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs | |||
@@ -0,0 +1,104 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class SphereSphereCollisionAlgorithm : CollisionAlgorithm | ||
30 | { | ||
31 | private bool _ownManifold; | ||
32 | private PersistentManifold _manifold; | ||
33 | |||
34 | public SphereSphereCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
35 | : base(collisionAlgorithmConstructionInfo) | ||
36 | { | ||
37 | _ownManifold = false; | ||
38 | _manifold = manifold; | ||
39 | |||
40 | if (_manifold == null) | ||
41 | { | ||
42 | _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); | ||
43 | _ownManifold = true; | ||
44 | } | ||
45 | } | ||
46 | |||
47 | public SphereSphereCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) | ||
48 | : base(collisionAlgorithmConstructionInfo) { } | ||
49 | |||
50 | ~SphereSphereCollisionAlgorithm() | ||
51 | { | ||
52 | if (_ownManifold) | ||
53 | { | ||
54 | if (_manifold != null) | ||
55 | Dispatcher.ReleaseManifold(_manifold); | ||
56 | } | ||
57 | } | ||
58 | |||
59 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
60 | { | ||
61 | if (_manifold == null) | ||
62 | return; | ||
63 | |||
64 | SphereShape sphereA = bodyA.CollisionShape as SphereShape; | ||
65 | SphereShape sphereB = bodyB.CollisionShape as SphereShape; | ||
66 | |||
67 | Vector3 diff = bodyA.WorldTransform.Translation - bodyB.WorldTransform.Translation; | ||
68 | float len = diff.Length(); | ||
69 | float radiusA = sphereA.Radius; | ||
70 | float radiusB = sphereB.Radius; | ||
71 | |||
72 | //if distance positive, don't generate a new contact | ||
73 | if (len > (radiusA + radiusB)) | ||
74 | return; | ||
75 | |||
76 | //distance (negative means penetration) | ||
77 | float dist = len - (radiusA + radiusB); | ||
78 | |||
79 | Vector3 normalOnSurfaceB = diff / len; | ||
80 | //point on A (worldspace) | ||
81 | Vector3 posA = bodyA.WorldTransform.Translation - radiusA * normalOnSurfaceB; | ||
82 | //point on B (worldspace) | ||
83 | Vector3 posB = bodyB.WorldTransform.Translation + radiusB * normalOnSurfaceB; | ||
84 | |||
85 | // report a contact. internally this will be kept persistent, and contact reduction is done | ||
86 | resultOut.SetPersistentManifold(_manifold); | ||
87 | resultOut.AddContactPoint(normalOnSurfaceB, posB, dist); | ||
88 | } | ||
89 | |||
90 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
91 | { | ||
92 | //not yet | ||
93 | return 1f; | ||
94 | } | ||
95 | |||
96 | public class CreateFunc : CollisionAlgorithmCreateFunction | ||
97 | { | ||
98 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
99 | { | ||
100 | return new SphereSphereCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB); | ||
101 | } | ||
102 | } | ||
103 | } | ||
104 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs new file mode 100644 index 0000000..1ca5cfb --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs | |||
@@ -0,0 +1,100 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// SphereSphereCollisionAlgorithm provides sphere-sphere collision detection. | ||
31 | /// Other features are frame-coherency (persistent data) and collision response. | ||
32 | /// Also provides the most basic sample for custom/user btCollisionAlgorithm | ||
33 | /// </summary> | ||
34 | public class SphereTriangleCollisionAlgorithm : CollisionAlgorithm, IDisposable | ||
35 | { | ||
36 | private bool _ownManifold; | ||
37 | private PersistentManifold _manifold; | ||
38 | private bool _isSwapped; | ||
39 | |||
40 | public SphereTriangleCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) | ||
41 | : base(collisionAlgorithmConstructionInfo) | ||
42 | { | ||
43 | _ownManifold = false; | ||
44 | _manifold = manifold; | ||
45 | _isSwapped = isSwapped; | ||
46 | |||
47 | if (_manifold == null) | ||
48 | { | ||
49 | _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); | ||
50 | _ownManifold = true; | ||
51 | } | ||
52 | } | ||
53 | |||
54 | public SphereTriangleCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) | ||
55 | : base(collisionAlgorithmConstructionInfo) { } | ||
56 | |||
57 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
58 | { | ||
59 | if (_manifold == null) | ||
60 | return; | ||
61 | |||
62 | SphereShape sphere = bodyA.CollisionShape as SphereShape; | ||
63 | TriangleShape triangle = bodyB.CollisionShape as TriangleShape; | ||
64 | |||
65 | /// report a contact. internally this will be kept persistent, and contact reduction is done | ||
66 | resultOut.SetPersistentManifold(_manifold); | ||
67 | SphereTriangleDetector detector = new SphereTriangleDetector(sphere, triangle); | ||
68 | |||
69 | DiscreteCollisionDetectorInterface.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | ||
70 | input.MaximumDistanceSquared = 1e30f;//todo: tighter bounds | ||
71 | input.TransformA = bodyA.WorldTransform; | ||
72 | input.TransformB = bodyB.WorldTransform; | ||
73 | |||
74 | detector.GetClosestPoints(input, resultOut, null); | ||
75 | } | ||
76 | |||
77 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
78 | { | ||
79 | //not yet | ||
80 | return 1f; | ||
81 | } | ||
82 | |||
83 | public class CreateFunc : CollisionAlgorithmCreateFunction | ||
84 | { | ||
85 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
86 | { | ||
87 | return new SphereTriangleCollisionAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, IsSwapped); | ||
88 | } | ||
89 | } | ||
90 | |||
91 | #region IDisposable Members | ||
92 | public void Dispose() | ||
93 | { | ||
94 | if (_ownManifold) | ||
95 | if (_manifold != null) | ||
96 | Dispatcher.ReleaseManifold(_manifold); | ||
97 | } | ||
98 | #endregion | ||
99 | } | ||
100 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs new file mode 100644 index 0000000..865754a --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs | |||
@@ -0,0 +1,214 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class SphereTriangleDetector : DiscreteCollisionDetectorInterface | ||
30 | { | ||
31 | private SphereShape _sphere; | ||
32 | private TriangleShape _triangle; | ||
33 | private const int MaxOverlap = 0; | ||
34 | |||
35 | public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle) | ||
36 | { | ||
37 | this._sphere = sphere; | ||
38 | this._triangle = triangle; | ||
39 | } | ||
40 | |||
41 | public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw) | ||
42 | { | ||
43 | Matrix transformA = input.TransformA; | ||
44 | Matrix transformB = input.TransformB; | ||
45 | |||
46 | Vector3 point = new Vector3(); | ||
47 | Vector3 normal = new Vector3(); | ||
48 | Single timeOfImpact = 1.0f; | ||
49 | Single depth = 0.0f; | ||
50 | |||
51 | //move sphere into triangle space | ||
52 | Matrix sphereInTr = MathHelper.InverseTimes(transformB, transformA); | ||
53 | |||
54 | if (Collide(sphereInTr.Translation, point, normal, depth, timeOfImpact)) | ||
55 | output.AddContactPoint(Vector3.TransformNormal(normal, transformB), Vector3.TransformNormal(point, transformB), depth); | ||
56 | } | ||
57 | |||
58 | /// <summary> | ||
59 | /// See also geometrictools.com | ||
60 | /// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv | ||
61 | /// </summary> | ||
62 | /// <param name="from"></param> | ||
63 | /// <param name="to"></param> | ||
64 | /// <param name="p"></param> | ||
65 | /// <param name="nearest"></param> | ||
66 | /// <returns></returns> | ||
67 | private float SegmentSquareDistance(Vector3 from, Vector3 to, Vector3 point, Vector3 nearest) | ||
68 | { | ||
69 | Vector3 diff = point - from; | ||
70 | Vector3 v = to - from; | ||
71 | float t = Vector3.Dot(v, diff); | ||
72 | |||
73 | if (t > 0) | ||
74 | { | ||
75 | float dotVV = Vector3.Dot(v, v); | ||
76 | if (t < dotVV) | ||
77 | { | ||
78 | t /= dotVV; | ||
79 | diff -= t * v; | ||
80 | } | ||
81 | else | ||
82 | { | ||
83 | t = 1; | ||
84 | diff -= v; | ||
85 | } | ||
86 | } | ||
87 | else | ||
88 | t = 0; | ||
89 | |||
90 | nearest = from + t * v; | ||
91 | return Vector3.Dot(diff, diff); | ||
92 | } | ||
93 | |||
94 | private bool Collide(Vector3 sphereCenter, Vector3 point, Vector3 resultNormal, float depth, float timeOfImpact) | ||
95 | { | ||
96 | Vector3[] vertices = _triangle.Vertices; | ||
97 | Vector3 c = sphereCenter; | ||
98 | float r = _sphere.Radius; | ||
99 | |||
100 | Vector3 delta = new Vector3(); | ||
101 | |||
102 | Vector3 normal = Vector3.Cross(vertices[1] - vertices[0], vertices[2] - vertices[0]); | ||
103 | normal = Vector3.Normalize(normal); | ||
104 | Vector3 p1ToCentre = c - vertices[0]; | ||
105 | float distanceFromPlane = Vector3.Dot(p1ToCentre, normal); | ||
106 | |||
107 | if (distanceFromPlane < 0) | ||
108 | { | ||
109 | //triangle facing the other way | ||
110 | distanceFromPlane *= -1; | ||
111 | normal *= -1; | ||
112 | } | ||
113 | |||
114 | float contactMargin = PersistentManifold.ContactBreakingThreshold; | ||
115 | bool isInsideContactPlane = distanceFromPlane < r + contactMargin; | ||
116 | bool isInsideShellPlane = distanceFromPlane < r; | ||
117 | |||
118 | float deltaDotNormal = Vector3.Dot(delta, normal); | ||
119 | if (!isInsideShellPlane && deltaDotNormal >= 0.0f) | ||
120 | return false; | ||
121 | |||
122 | // Check for contact / intersection | ||
123 | bool hasContact = false; | ||
124 | Vector3 contactPoint = new Vector3(); | ||
125 | if (isInsideContactPlane) | ||
126 | { | ||
127 | if (FaceContains(c, vertices, normal)) | ||
128 | { | ||
129 | // Inside the contact wedge - touches a point on the shell plane | ||
130 | hasContact = true; | ||
131 | contactPoint = c - normal * distanceFromPlane; | ||
132 | } | ||
133 | else | ||
134 | { | ||
135 | // Could be inside one of the contact capsules | ||
136 | float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); | ||
137 | Vector3 nearestOnEdge = new Vector3(); | ||
138 | for (int i = 0; i < _triangle.EdgeCount; i++) | ||
139 | { | ||
140 | Vector3 pa, pb; | ||
141 | _triangle.GetEdge(i, out pa, out pb); | ||
142 | |||
143 | float distanceSqr = SegmentSquareDistance(pa, pb, c, nearestOnEdge); | ||
144 | if (distanceSqr < contactCapsuleRadiusSqr) | ||
145 | { | ||
146 | // Yep, we're inside a capsule | ||
147 | hasContact = true; | ||
148 | contactPoint = nearestOnEdge; | ||
149 | } | ||
150 | } | ||
151 | } | ||
152 | } | ||
153 | |||
154 | if (hasContact) | ||
155 | { | ||
156 | Vector3 contactToCentre = c - contactPoint; | ||
157 | float distanceSqr = contactToCentre.LengthSquared(); | ||
158 | if (distanceSqr < (r - MaxOverlap) * (r - MaxOverlap)) | ||
159 | { | ||
160 | float distance = (float)Math.Sqrt(distanceSqr); | ||
161 | resultNormal = contactToCentre; | ||
162 | resultNormal = Vector3.Normalize(resultNormal); | ||
163 | point = contactPoint; | ||
164 | depth = -(r - distance); | ||
165 | return true; | ||
166 | } | ||
167 | |||
168 | if (Vector3.Dot(delta, contactToCentre) >= 0.0f) | ||
169 | return false; | ||
170 | |||
171 | // Moving towards the contact point -> collision | ||
172 | point = contactPoint; | ||
173 | timeOfImpact = 0.0f; | ||
174 | return true; | ||
175 | } | ||
176 | return false; | ||
177 | } | ||
178 | |||
179 | private bool PointInTriangle(Vector3[] vertices, Vector3 normal, Vector3 p) | ||
180 | { | ||
181 | Vector3 p1 = vertices[0]; | ||
182 | Vector3 p2 = vertices[1]; | ||
183 | Vector3 p3 = vertices[2]; | ||
184 | |||
185 | Vector3 edge1 = p2 - p1; | ||
186 | Vector3 edge2 = p3 - p2; | ||
187 | Vector3 edge3 = p1 - p3; | ||
188 | |||
189 | Vector3 p1ToP = p - p1; | ||
190 | Vector3 p2ToP = p - p2; | ||
191 | Vector3 p3ToP = p - p3; | ||
192 | |||
193 | Vector3 edge1Normal = Vector3.Cross(edge1, normal); | ||
194 | Vector3 edge2Normal = Vector3.Cross(edge2, normal); | ||
195 | Vector3 edge3Normal = Vector3.Cross(edge3, normal); | ||
196 | |||
197 | float r1, r2, r3; | ||
198 | r1 = Vector3.Dot(edge1Normal, p1ToP); | ||
199 | r2 = Vector3.Dot(edge2Normal, p2ToP); | ||
200 | r3 = Vector3.Dot(edge3Normal, p3ToP); | ||
201 | if ((r1 > 0 && r2 > 0 && r3 > 0) || | ||
202 | (r1 <= 0 && r2 <= 0 && r3 <= 0)) | ||
203 | return true; | ||
204 | return false; | ||
205 | } | ||
206 | |||
207 | private bool FaceContains(Vector3 p, Vector3[] vertices, Vector3 normal) | ||
208 | { | ||
209 | Vector3 lp = p; | ||
210 | Vector3 lnormal = normal; | ||
211 | return PointInTriangle(vertices, lnormal, lp); | ||
212 | } | ||
213 | } | ||
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 new file mode 100644 index 0000000..a825fad --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs | |||
@@ -0,0 +1,151 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public class UnionFind : IDisposable | ||
29 | { | ||
30 | private List<Element> _elements = new List<Element>(); | ||
31 | |||
32 | public int ElementCount | ||
33 | { | ||
34 | get { return _elements.Count; } | ||
35 | } | ||
36 | |||
37 | public void SortIslands() | ||
38 | { | ||
39 | for (int i = 0; i < _elements.Count; i++) | ||
40 | { | ||
41 | _elements[i].ID = Find(i); | ||
42 | _elements[i].Size = i; | ||
43 | } | ||
44 | |||
45 | _elements.Sort(Sort); | ||
46 | } | ||
47 | |||
48 | private static int Sort(Element x, Element y) | ||
49 | { | ||
50 | if (x.ID < y.ID) return -1; | ||
51 | //else if (x.ID > y.ID) return 1; | ||
52 | else return 0; | ||
53 | } | ||
54 | |||
55 | public void Reset(int number) | ||
56 | { | ||
57 | Allocate(number); | ||
58 | |||
59 | for (int i = 0; i < number; i++) | ||
60 | { | ||
61 | Element element = new Element(); | ||
62 | element.ID = i; | ||
63 | element.Size = 1; | ||
64 | _elements.Insert(i, element); | ||
65 | } | ||
66 | } | ||
67 | |||
68 | public bool IsRoot(int index) | ||
69 | { | ||
70 | return (_elements[index].Size == index); | ||
71 | } | ||
72 | |||
73 | public Element this[int index] | ||
74 | { | ||
75 | get { return _elements[index]; } | ||
76 | } | ||
77 | |||
78 | public void Allocate(int number) | ||
79 | { | ||
80 | //Does nothing | ||
81 | _elements = new List<Element>(number); | ||
82 | } | ||
83 | |||
84 | public bool Find(int i, int j) | ||
85 | { | ||
86 | return (Find(i) == Find(j)); | ||
87 | } | ||
88 | |||
89 | public int Find(int i) | ||
90 | { | ||
91 | while (i != _elements[i].ID) | ||
92 | { | ||
93 | //Element element = _elements[i]; | ||
94 | //element.ID = _elements[_elements[i].ID].ID; | ||
95 | _elements[i].ID = _elements[_elements[i].ID].ID; | ||
96 | i = _elements[i].ID; | ||
97 | } | ||
98 | |||
99 | return i; | ||
100 | } | ||
101 | |||
102 | public void Unite(int p, int q) | ||
103 | { | ||
104 | int i = Find(p), j = Find(q); | ||
105 | if (i == j) | ||
106 | return; | ||
107 | |||
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) | ||
110 | //{ | ||
111 | // Element element = _elements[i]; | ||
112 | // element.ID = j; | ||
113 | // _elements[i] = element; | ||
114 | |||
115 | // element = _elements[j]; | ||
116 | // element.Size += _elements[i].Size; | ||
117 | // _elements[j] = element; | ||
118 | //} | ||
119 | //else | ||
120 | //{ | ||
121 | // Element element = _elements[j]; | ||
122 | // element.ID = i; | ||
123 | // _elements[j] = element; | ||
124 | |||
125 | // element = _elements[i]; | ||
126 | // element.Size += _elements[j].Size; | ||
127 | // _elements[i] = element; | ||
128 | //} | ||
129 | _elements[i].ID = j; | ||
130 | _elements[j].Size += _elements[i].Size; | ||
131 | } | ||
132 | |||
133 | #region IDisposable Members | ||
134 | |||
135 | public void Dispose() | ||
136 | { | ||
137 | _elements.Clear(); | ||
138 | } | ||
139 | |||
140 | #endregion | ||
141 | } | ||
142 | |||
143 | public class Element | ||
144 | { | ||
145 | private int _id; | ||
146 | private int _size; | ||
147 | |||
148 | public int ID { get { return _id; } set { _id = value; } } | ||
149 | public int Size { get { return _size; } set { _size = value; } } | ||
150 | } | ||
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 new file mode 100644 index 0000000..b75c2b6 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs | |||
@@ -0,0 +1,215 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// BUSimplex1to4 implements feature based and implicit simplex of up to 4 vertices (tetrahedron, triangle, line, vertex). | ||
31 | /// </summary> | ||
32 | public class BUSimplex1to4 : PolyhedralConvexShape | ||
33 | { | ||
34 | private int _numVertices = 0; | ||
35 | private Vector3[] _vertices = new Vector3[4]; | ||
36 | |||
37 | public BUSimplex1to4() { } | ||
38 | |||
39 | public BUSimplex1to4(Vector3 pointA) | ||
40 | { | ||
41 | AddVertex(pointA); | ||
42 | } | ||
43 | |||
44 | public BUSimplex1to4(Vector3 pointA, Vector3 pointB) | ||
45 | { | ||
46 | AddVertex(pointA); | ||
47 | AddVertex(pointB); | ||
48 | } | ||
49 | |||
50 | public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC) | ||
51 | { | ||
52 | AddVertex(pointA); | ||
53 | AddVertex(pointB); | ||
54 | AddVertex(pointC); | ||
55 | } | ||
56 | |||
57 | public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC, Vector3 pointD) | ||
58 | { | ||
59 | AddVertex(pointA); | ||
60 | AddVertex(pointB); | ||
61 | AddVertex(pointC); | ||
62 | AddVertex(pointD); | ||
63 | } | ||
64 | |||
65 | protected Vector3[] Vertices { get { return _vertices; } set { _vertices = value; } } | ||
66 | |||
67 | public override int VertexCount | ||
68 | { | ||
69 | get | ||
70 | { | ||
71 | return _numVertices; | ||
72 | } | ||
73 | } | ||
74 | |||
75 | public override int EdgeCount | ||
76 | { | ||
77 | get | ||
78 | { | ||
79 | //euler formula, F-E+V = 2, so E = F+V-2 | ||
80 | switch (_numVertices) | ||
81 | { | ||
82 | case 0: return 0; | ||
83 | case 1: return 0; | ||
84 | case 2: return 1; | ||
85 | case 3: return 3; | ||
86 | case 4: return 6; | ||
87 | } | ||
88 | return 0; | ||
89 | } | ||
90 | } | ||
91 | |||
92 | public override int PlaneCount | ||
93 | { | ||
94 | get | ||
95 | { | ||
96 | switch (_numVertices) | ||
97 | { | ||
98 | case 0: | ||
99 | return 0; | ||
100 | case 1: | ||
101 | return 0; | ||
102 | case 2: | ||
103 | return 0; | ||
104 | case 3: | ||
105 | return 2; | ||
106 | case 4: | ||
107 | return 4; | ||
108 | } | ||
109 | return 0; | ||
110 | } | ||
111 | } | ||
112 | |||
113 | public override BroadphaseNativeTypes ShapeType | ||
114 | { | ||
115 | get | ||
116 | { | ||
117 | return BroadphaseNativeTypes.Tetrahedral; | ||
118 | } | ||
119 | } | ||
120 | |||
121 | public override string Name | ||
122 | { | ||
123 | get | ||
124 | { | ||
125 | return "BUSimplex1to4"; | ||
126 | } | ||
127 | } | ||
128 | |||
129 | public void AddVertex(Vector3 v) | ||
130 | { | ||
131 | _vertices[_numVertices++] = v; | ||
132 | } | ||
133 | |||
134 | public void Reset() | ||
135 | { | ||
136 | _numVertices = 0; | ||
137 | } | ||
138 | |||
139 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) | ||
140 | { | ||
141 | switch (_numVertices) | ||
142 | { | ||
143 | case 2: | ||
144 | pa = _vertices[0]; | ||
145 | pb = _vertices[1]; | ||
146 | return; | ||
147 | case 3: | ||
148 | switch (i) | ||
149 | { | ||
150 | case 0: | ||
151 | pa = _vertices[0]; | ||
152 | pb = _vertices[1]; | ||
153 | return; | ||
154 | case 1: | ||
155 | pa = _vertices[1]; | ||
156 | pb = _vertices[2]; | ||
157 | return; | ||
158 | case 2: | ||
159 | pa = _vertices[2]; | ||
160 | pb = _vertices[0]; | ||
161 | return; | ||
162 | } | ||
163 | break; | ||
164 | case 4: | ||
165 | switch (i) | ||
166 | { | ||
167 | case 0: | ||
168 | pa = _vertices[0]; | ||
169 | pb = _vertices[1]; | ||
170 | return; | ||
171 | case 1: | ||
172 | pa = _vertices[1]; | ||
173 | pb = _vertices[2]; | ||
174 | return; | ||
175 | case 2: | ||
176 | pa = _vertices[2]; | ||
177 | pb = _vertices[0]; | ||
178 | return; | ||
179 | case 3: | ||
180 | pa = _vertices[0]; | ||
181 | pb = _vertices[3]; | ||
182 | return; | ||
183 | case 4: | ||
184 | pa = _vertices[1]; | ||
185 | pb = _vertices[3]; | ||
186 | return; | ||
187 | case 5: | ||
188 | pa = _vertices[2]; | ||
189 | pb = _vertices[3]; | ||
190 | return; | ||
191 | } | ||
192 | break; | ||
193 | } | ||
194 | |||
195 | pa = new Vector3(); | ||
196 | pb = new Vector3(); | ||
197 | } | ||
198 | |||
199 | public override void GetVertex(int i, out Vector3 vtx) | ||
200 | { | ||
201 | vtx = _vertices[i]; | ||
202 | } | ||
203 | |||
204 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) | ||
205 | { | ||
206 | planeNormal = new Vector3(); | ||
207 | planeSupport = new Vector3(); | ||
208 | } | ||
209 | |||
210 | public override bool IsInside(Vector3 pt, float tolerance) | ||
211 | { | ||
212 | return false; | ||
213 | } | ||
214 | } | ||
215 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs new file mode 100644 index 0000000..ba6f3b7 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs | |||
@@ -0,0 +1,316 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class BoxShape : PolyhedralConvexShape | ||
30 | { | ||
31 | public BoxShape(Vector3 boxHalfExtents) | ||
32 | { | ||
33 | ImplicitShapeDimensions = boxHalfExtents; | ||
34 | } | ||
35 | |||
36 | public override int VertexCount | ||
37 | { | ||
38 | get | ||
39 | { | ||
40 | return 8; | ||
41 | } | ||
42 | } | ||
43 | |||
44 | public override int EdgeCount | ||
45 | { | ||
46 | get | ||
47 | { | ||
48 | return 12; | ||
49 | } | ||
50 | } | ||
51 | |||
52 | public override BroadphaseNativeTypes ShapeType | ||
53 | { | ||
54 | get | ||
55 | { | ||
56 | return BroadphaseNativeTypes.Box; | ||
57 | } | ||
58 | } | ||
59 | |||
60 | public override string Name | ||
61 | { | ||
62 | get | ||
63 | { | ||
64 | return "Box"; | ||
65 | } | ||
66 | } | ||
67 | |||
68 | public override int PreferredPenetrationDirectionsCount | ||
69 | { | ||
70 | get | ||
71 | { | ||
72 | return 6; | ||
73 | } | ||
74 | } | ||
75 | |||
76 | public override int PlaneCount | ||
77 | { | ||
78 | get | ||
79 | { | ||
80 | return 6; | ||
81 | } | ||
82 | } | ||
83 | |||
84 | public Vector3 HalfExtents { get { return ImplicitShapeDimensions * LocalScaling; } } | ||
85 | |||
86 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) | ||
87 | { | ||
88 | int edgeVert0 = 0; | ||
89 | int edgeVert1 = 0; | ||
90 | |||
91 | switch (i) | ||
92 | { | ||
93 | case 0: | ||
94 | edgeVert0 = 0; | ||
95 | edgeVert1 = 1; | ||
96 | break; | ||
97 | case 1: | ||
98 | edgeVert0 = 0; | ||
99 | edgeVert1 = 2; | ||
100 | break; | ||
101 | case 2: | ||
102 | edgeVert0 = 1; | ||
103 | edgeVert1 = 3; | ||
104 | |||
105 | break; | ||
106 | case 3: | ||
107 | edgeVert0 = 2; | ||
108 | edgeVert1 = 3; | ||
109 | break; | ||
110 | case 4: | ||
111 | edgeVert0 = 0; | ||
112 | edgeVert1 = 4; | ||
113 | break; | ||
114 | case 5: | ||
115 | edgeVert0 = 1; | ||
116 | edgeVert1 = 5; | ||
117 | |||
118 | break; | ||
119 | case 6: | ||
120 | edgeVert0 = 2; | ||
121 | edgeVert1 = 6; | ||
122 | break; | ||
123 | case 7: | ||
124 | edgeVert0 = 3; | ||
125 | edgeVert1 = 7; | ||
126 | break; | ||
127 | case 8: | ||
128 | edgeVert0 = 4; | ||
129 | edgeVert1 = 5; | ||
130 | break; | ||
131 | case 9: | ||
132 | edgeVert0 = 4; | ||
133 | edgeVert1 = 6; | ||
134 | break; | ||
135 | case 10: | ||
136 | edgeVert0 = 5; | ||
137 | edgeVert1 = 7; | ||
138 | break; | ||
139 | case 11: | ||
140 | edgeVert0 = 6; | ||
141 | edgeVert1 = 7; | ||
142 | break; | ||
143 | default: | ||
144 | throw new BulletException(); | ||
145 | |||
146 | } | ||
147 | |||
148 | GetVertex(edgeVert0, out pa); | ||
149 | GetVertex(edgeVert1, out pb); | ||
150 | } | ||
151 | |||
152 | public override void GetVertex(int i, out Vector3 vtx) | ||
153 | { | ||
154 | Vector3 halfExtents = HalfExtents; | ||
155 | |||
156 | vtx = new Vector3( | ||
157 | halfExtents.X * (1 - (i & 1)) - halfExtents.X * (i & 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)); | ||
160 | } | ||
161 | |||
162 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) | ||
163 | { | ||
164 | //this plane might not be aligned... | ||
165 | Vector4 plane; | ||
166 | GetPlaneEquation(out plane, i); | ||
167 | planeNormal = new Vector3(plane.X, plane.Y, plane.Z); | ||
168 | planeSupport = LocalGetSupportingVertex(-planeNormal); | ||
169 | } | ||
170 | |||
171 | public override bool IsInside(Vector3 pt, float tolerance) | ||
172 | { | ||
173 | Vector3 halfExtents = HalfExtents; | ||
174 | |||
175 | //btScalar minDist = 2*tolerance; | ||
176 | |||
177 | bool result = (pt.X <= ( halfExtents.X + tolerance)) && | ||
178 | (pt.X >= (-halfExtents.X - tolerance)) && | ||
179 | (pt.Y <= ( halfExtents.Y + tolerance)) && | ||
180 | (pt.Y >= (-halfExtents.Y - tolerance)) && | ||
181 | (pt.Z <= ( halfExtents.Z + tolerance)) && | ||
182 | (pt.Z >= (-halfExtents.Z - tolerance)); | ||
183 | |||
184 | return result; | ||
185 | } | ||
186 | |||
187 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | ||
188 | { | ||
189 | Vector3 halfExtents = HalfExtents; | ||
190 | |||
191 | return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X, | ||
192 | vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, | ||
193 | vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); | ||
194 | } | ||
195 | |||
196 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
197 | { | ||
198 | Vector3 halfExtents = HalfExtents; | ||
199 | Vector3 margin = new Vector3(Margin, Margin, Margin); | ||
200 | halfExtents -= margin; | ||
201 | |||
202 | return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X, | ||
203 | vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, | ||
204 | vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); | ||
205 | } | ||
206 | |||
207 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
208 | { | ||
209 | Vector3 halfExtents = HalfExtents; | ||
210 | Vector3 margin = new Vector3(Margin, Margin, Margin); | ||
211 | halfExtents -= margin; | ||
212 | |||
213 | for (int i = 0; i < vectors.Length; i++) | ||
214 | { | ||
215 | Vector3 vec = vectors[i]; | ||
216 | supportVerticesOut[i] = new Vector3(vec.X < 0.0f ? -halfExtents.X : halfExtents.X, | ||
217 | vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, | ||
218 | vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); | ||
219 | } | ||
220 | } | ||
221 | |||
222 | public virtual void GetPlaneEquation(out Vector4 plane, int i) | ||
223 | { | ||
224 | Vector3 halfExtents = HalfExtents; | ||
225 | |||
226 | switch (i) | ||
227 | { | ||
228 | case 0: | ||
229 | plane = new Vector4(1, 0, 0, 0); | ||
230 | plane.W = -halfExtents.X; | ||
231 | break; | ||
232 | case 1: | ||
233 | plane = new Vector4(-1, 0, 0, 0); | ||
234 | plane.W = -halfExtents.X; | ||
235 | break; | ||
236 | case 2: | ||
237 | plane = new Vector4(0, 1, 0, 0); | ||
238 | plane.W = -halfExtents.Y; | ||
239 | break; | ||
240 | case 3: | ||
241 | plane = new Vector4(0, -1, 0, 0); | ||
242 | plane.W = -halfExtents.Y; | ||
243 | break; | ||
244 | case 4: | ||
245 | plane = new Vector4(0, 0, 1, 0); | ||
246 | plane.W = -halfExtents.Z; | ||
247 | break; | ||
248 | case 5: | ||
249 | plane = new Vector4(0, 0, -1, 0); | ||
250 | plane.W = -halfExtents.Z; | ||
251 | break; | ||
252 | default: | ||
253 | throw new BulletException(); | ||
254 | } | ||
255 | } | ||
256 | |||
257 | public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) | ||
258 | { | ||
259 | switch (index) | ||
260 | { | ||
261 | case 0: | ||
262 | penetrationVector = new Vector3(1, 0, 0); | ||
263 | break; | ||
264 | case 1: | ||
265 | penetrationVector = new Vector3(-1, 0, 0); | ||
266 | break; | ||
267 | case 2: | ||
268 | penetrationVector = new Vector3(0, 1, 0); | ||
269 | break; | ||
270 | case 3: | ||
271 | penetrationVector = new Vector3(0, -1, 0); | ||
272 | break; | ||
273 | case 4: | ||
274 | penetrationVector = new Vector3(0, 0, 1); | ||
275 | break; | ||
276 | case 5: | ||
277 | penetrationVector = new Vector3(0, 0, -1); | ||
278 | break; | ||
279 | default: | ||
280 | throw new BulletException(); | ||
281 | } | ||
282 | } | ||
283 | |||
284 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
285 | { | ||
286 | Vector3 halfExtents = HalfExtents; | ||
287 | |||
288 | Matrix abs_b = MathHelper.Absolute(t); | ||
289 | Vector3 center = t.Translation; | ||
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); | ||
292 | Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33); | ||
293 | Vector3 extent = new Vector3(Vector3.Dot(row1, halfExtents), | ||
294 | Vector3.Dot(row2, halfExtents), | ||
295 | Vector3.Dot(row3, halfExtents)); | ||
296 | extent += new Vector3(Margin, Margin, Margin); | ||
297 | |||
298 | aabbMin = center - extent; | ||
299 | aabbMax = center + extent; | ||
300 | } | ||
301 | |||
302 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
303 | { | ||
304 | Vector3 halfExtents = HalfExtents; | ||
305 | |||
306 | float lx = 2f * (halfExtents.X); | ||
307 | float ly = 2f * (halfExtents.Y); | ||
308 | float lz = 2f * (halfExtents.Z); | ||
309 | |||
310 | inertia = new Vector3(); | ||
311 | inertia.X = mass / (12.0f) * (ly * ly + lz * lz); | ||
312 | inertia.Y = mass / (12.0f) * (lx * lx + lz * lz); | ||
313 | inertia.Z = mass / (12.0f) * (lx * lx + ly * ly); | ||
314 | } | ||
315 | } | ||
316 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs new file mode 100644 index 0000000..00247a0 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs | |||
@@ -0,0 +1,83 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | class MyNodeOverlapCallback : INodeOverlapCallback | ||
30 | { | ||
31 | StridingMeshInterface _meshInterface; | ||
32 | ITriangleCallback _callback; | ||
33 | Vector3[] _triangle = new Vector3[3]; | ||
34 | |||
35 | public MyNodeOverlapCallback(ITriangleCallback callback, StridingMeshInterface meshInterface) | ||
36 | { | ||
37 | _meshInterface = meshInterface; | ||
38 | _callback = callback; | ||
39 | } | ||
40 | |||
41 | public void ProcessNode(OptimizedBvhNode node) | ||
42 | { | ||
43 | List<Vector3> verts; | ||
44 | List<int> indicies; | ||
45 | int numtriangles; | ||
46 | |||
47 | _meshInterface.GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, node.SubPart); | ||
48 | Vector3 meshScaling = _meshInterface.Scaling; | ||
49 | |||
50 | for (int j = 0; j < 3; j++) | ||
51 | { | ||
52 | _triangle[j] = verts[indicies[j + node.TriangleIndex * 3]] * meshScaling; | ||
53 | } | ||
54 | |||
55 | _callback.ProcessTriangle(_triangle, node.SubPart, node.TriangleIndex); | ||
56 | _meshInterface.UnLockReadOnlyVertexBase(node.SubPart); | ||
57 | } | ||
58 | } | ||
59 | |||
60 | public class BvhTriangleMeshShape : TriangleMeshShape | ||
61 | { | ||
62 | OptimizedBvh _bvh = new OptimizedBvh(); | ||
63 | |||
64 | public BvhTriangleMeshShape(StridingMeshInterface meshInterface) : base(meshInterface) | ||
65 | { | ||
66 | _bvh.Build(meshInterface); | ||
67 | } | ||
68 | |||
69 | public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) | ||
70 | { | ||
71 | MyNodeOverlapCallback myNodeCallback = new MyNodeOverlapCallback(callback, MeshInterface); | ||
72 | _bvh.ReportAabbOverlappingNodex(myNodeCallback, aabbMin, aabbMax); | ||
73 | } | ||
74 | |||
75 | public override string Name | ||
76 | { | ||
77 | get | ||
78 | { | ||
79 | return "BvhTriangleMesh"; | ||
80 | } | ||
81 | } | ||
82 | } | ||
83 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs new file mode 100644 index 0000000..a9ce3be --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs | |||
@@ -0,0 +1,148 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// CollisionShape provides generic interface for collidable objects | ||
31 | /// </summary> | ||
32 | public abstract class CollisionShape | ||
33 | { | ||
34 | //debugging support | ||
35 | private string _tempDebug; | ||
36 | |||
37 | public abstract string Name { get; } | ||
38 | public string ExtraDebugInfo { get { return _tempDebug; } set { _tempDebug = value; } } | ||
39 | |||
40 | public bool IsPolyhedral | ||
41 | { | ||
42 | get | ||
43 | { | ||
44 | return BroadphaseProxy.IsPolyhedral(ShapeType); | ||
45 | } | ||
46 | } | ||
47 | |||
48 | public bool IsConvex | ||
49 | { | ||
50 | get | ||
51 | { | ||
52 | return BroadphaseProxy.IsConvex(ShapeType); | ||
53 | } | ||
54 | } | ||
55 | public bool IsConcave | ||
56 | { | ||
57 | get | ||
58 | { | ||
59 | return BroadphaseProxy.IsConcave(ShapeType); | ||
60 | } | ||
61 | } | ||
62 | public bool IsCompound | ||
63 | { | ||
64 | get | ||
65 | { | ||
66 | return BroadphaseProxy.IsCompound(ShapeType); | ||
67 | } | ||
68 | } | ||
69 | |||
70 | //isInfinite is used to catch simulation error (aabb check) | ||
71 | public bool IsInfinite | ||
72 | { | ||
73 | get | ||
74 | { | ||
75 | return BroadphaseProxy.IsInfinite(ShapeType); | ||
76 | } | ||
77 | } | ||
78 | |||
79 | public abstract float Margin { get; set; } | ||
80 | public abstract Vector3 LocalScaling { get; set; } | ||
81 | public abstract BroadphaseNativeTypes ShapeType { get; } | ||
82 | |||
83 | |||
84 | public virtual void GetBoundingSphere(out Vector3 center, out float radius) | ||
85 | { | ||
86 | Matrix tr = Matrix.Identity; | ||
87 | Vector3 aabbMin, aabbMax; | ||
88 | |||
89 | GetAabb(tr, out aabbMin, out aabbMax); | ||
90 | |||
91 | radius = (aabbMax - aabbMin).Length() * 0.5f; | ||
92 | center = (aabbMin + aabbMax) * 0.5f; | ||
93 | } | ||
94 | |||
95 | public virtual float GetAngularMotionDisc() | ||
96 | { | ||
97 | Vector3 center; | ||
98 | float disc; | ||
99 | GetBoundingSphere(out center, out disc); | ||
100 | disc += center.Length(); | ||
101 | return disc; | ||
102 | } | ||
103 | |||
104 | //calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) | ||
105 | //result is conservative | ||
106 | public void CalculateTemporalAabb(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, out Vector3 temporalAabbMin, out Vector3 temporalAabbMax) | ||
107 | { | ||
108 | //start with static aabb | ||
109 | GetAabb(currentTransform, out temporalAabbMin, out temporalAabbMax); | ||
110 | |||
111 | float temporalAabbMaxx = temporalAabbMax.X; | ||
112 | float temporalAabbMaxy = temporalAabbMax.Y; | ||
113 | float temporalAabbMaxz = temporalAabbMax.Z; | ||
114 | float temporalAabbMinx = temporalAabbMin.X; | ||
115 | float temporalAabbMiny = temporalAabbMin.Y; | ||
116 | float temporalAabbMinz = temporalAabbMin.Z; | ||
117 | |||
118 | // add linear motion | ||
119 | Vector3 linMotion = linearVelocity * timeStep; | ||
120 | //todo: simd would have a vector max/min operation, instead of per-element access | ||
121 | if (linMotion.X > 0) | ||
122 | temporalAabbMaxx += linMotion.X; | ||
123 | else | ||
124 | temporalAabbMinx += linMotion.X; | ||
125 | if (linMotion.Y > 0) | ||
126 | temporalAabbMaxy += linMotion.Y; | ||
127 | else | ||
128 | temporalAabbMiny += linMotion.Y; | ||
129 | if (linMotion.Z > 0) | ||
130 | temporalAabbMaxz += linMotion.Z; | ||
131 | else | ||
132 | temporalAabbMinz += linMotion.Z; | ||
133 | |||
134 | //add conservative angular motion | ||
135 | float angularMotion = angularVelocity.Length() * GetAngularMotionDisc() * timeStep; | ||
136 | Vector3 angularMotion3d = new Vector3(angularMotion, angularMotion, angularMotion); | ||
137 | temporalAabbMin = new Vector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz); | ||
138 | temporalAabbMax = new Vector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz); | ||
139 | |||
140 | temporalAabbMin -= angularMotion3d; | ||
141 | temporalAabbMax += angularMotion3d; | ||
142 | } | ||
143 | |||
144 | public abstract void GetAabb(Matrix transform, out Vector3 aabbMin, out Vector3 aabbMax); | ||
145 | |||
146 | public abstract void CalculateLocalInertia(float mass, out Vector3 inertia); | ||
147 | } | ||
148 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs new file mode 100644 index 0000000..08aeec8 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs | |||
@@ -0,0 +1,183 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// CompoundShape allows to store multiple other CollisionShapes | ||
31 | /// This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape. | ||
32 | /// </summary> | ||
33 | public class CompoundShape : CollisionShape | ||
34 | { | ||
35 | private List<Matrix> _childTransforms = new List<Matrix>(); | ||
36 | private List<CollisionShape> _childShapes = new List<CollisionShape>(); | ||
37 | private Vector3 _localAabbMin; | ||
38 | private Vector3 _localAabbMax; | ||
39 | |||
40 | private OptimizedBvh _aabbTree; | ||
41 | private float _collisionMargin; | ||
42 | private Vector3 _localScaling; | ||
43 | |||
44 | public CompoundShape() | ||
45 | { | ||
46 | _localAabbMin = new Vector3(1e30f, 1e30f, 1e30f); | ||
47 | _localAabbMax = new Vector3(-1e30f, -1e30f, -1e30f); | ||
48 | _aabbTree = null; | ||
49 | _collisionMargin = 0f; | ||
50 | _localScaling = new Vector3(1f, 1f, 1f); | ||
51 | } | ||
52 | |||
53 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
54 | { | ||
55 | Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin); | ||
56 | Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin); | ||
57 | |||
58 | Matrix abs_b = MathHelper.Absolute(t); | ||
59 | |||
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); | ||
62 | Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33); | ||
63 | |||
64 | Vector3 center = new Vector3(Vector3.Dot(row1, localCenter) + t.Translation.X, | ||
65 | Vector3.Dot(row2, localCenter) + t.Translation.Y, | ||
66 | Vector3.Dot(row3, localCenter) + t.Translation.Z); | ||
67 | |||
68 | Vector3 extent = new Vector3(Vector3.Dot(row1, localHalfExtents), | ||
69 | Vector3.Dot(row2, localHalfExtents), | ||
70 | Vector3.Dot(row3, localHalfExtents)); | ||
71 | |||
72 | aabbMin = center - extent; | ||
73 | aabbMax = center + extent; | ||
74 | } | ||
75 | |||
76 | public override BroadphaseNativeTypes ShapeType | ||
77 | { | ||
78 | get | ||
79 | { | ||
80 | return BroadphaseNativeTypes.Compound; | ||
81 | } | ||
82 | } | ||
83 | |||
84 | public override Vector3 LocalScaling | ||
85 | { | ||
86 | get | ||
87 | { | ||
88 | return _localScaling; | ||
89 | } | ||
90 | set | ||
91 | { | ||
92 | _localScaling = value; | ||
93 | } | ||
94 | } | ||
95 | |||
96 | public override string Name | ||
97 | { | ||
98 | get | ||
99 | { | ||
100 | return "Compound"; | ||
101 | } | ||
102 | } | ||
103 | |||
104 | public override float Margin | ||
105 | { | ||
106 | get | ||
107 | { | ||
108 | return _collisionMargin; | ||
109 | } | ||
110 | set | ||
111 | { | ||
112 | _collisionMargin = value; | ||
113 | } | ||
114 | } | ||
115 | |||
116 | public int ChildShapeCount { get { return _childShapes.Count; } } | ||
117 | //this is optional, but should make collision queries faster, by culling non-overlapping nodes | ||
118 | public OptimizedBvh AabbTree { get { return _aabbTree; } } | ||
119 | |||
120 | public CollisionShape GetChildShape(int index) | ||
121 | { | ||
122 | return _childShapes[index]; | ||
123 | } | ||
124 | |||
125 | public Matrix GetChildTransform(int index) | ||
126 | { | ||
127 | return _childTransforms[index]; | ||
128 | } | ||
129 | |||
130 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
131 | { | ||
132 | //approximation: take the inertia from the aabb for now | ||
133 | Matrix ident = Matrix.Identity; | ||
134 | Vector3 aabbMin, aabbMax; | ||
135 | GetAabb(ident, out aabbMin, out aabbMax); | ||
136 | |||
137 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; | ||
138 | |||
139 | float lx = 2f * (halfExtents.X); | ||
140 | float ly = 2f * (halfExtents.Y); | ||
141 | float lz = 2f * (halfExtents.Z); | ||
142 | |||
143 | inertia = new Vector3(); | ||
144 | inertia.X = mass / (12.0f) * (ly * ly + lz * lz); | ||
145 | inertia.Y = mass / (12.0f) * (lx * lx + lz * lz); | ||
146 | inertia.Z = mass / (12.0f) * (lx * lx + ly * ly); | ||
147 | } | ||
148 | |||
149 | public void AddChildShape(Matrix localTransform, CollisionShape shape) | ||
150 | { | ||
151 | _childTransforms.Add(localTransform); | ||
152 | _childShapes.Add(shape); | ||
153 | |||
154 | //extend the local aabbMin/aabbMax | ||
155 | Vector3 localAabbMin, localAabbMax; | ||
156 | shape.GetAabb(localTransform, out localAabbMin, out localAabbMax); | ||
157 | if (_localAabbMin.X > localAabbMin.X) | ||
158 | { | ||
159 | _localAabbMin.X = localAabbMin.X; | ||
160 | } | ||
161 | if (_localAabbMax.X < localAabbMax.X) | ||
162 | { | ||
163 | _localAabbMax.X = localAabbMax.X; | ||
164 | } | ||
165 | if (_localAabbMin.Y > localAabbMin.Y) | ||
166 | { | ||
167 | _localAabbMin.Y = localAabbMin.Y; | ||
168 | } | ||
169 | if (_localAabbMax.Y < localAabbMax.Y) | ||
170 | { | ||
171 | _localAabbMax.Y = localAabbMax.Y; | ||
172 | } | ||
173 | if (_localAabbMin.Z > localAabbMin.Z) | ||
174 | { | ||
175 | _localAabbMin.Z = localAabbMin.Z; | ||
176 | } | ||
177 | if (_localAabbMax.Z < localAabbMax.Z) | ||
178 | { | ||
179 | _localAabbMax.Z = localAabbMax.Z; | ||
180 | } | ||
181 | } | ||
182 | } | ||
183 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs new file mode 100644 index 0000000..1e773f0 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs | |||
@@ -0,0 +1,55 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public abstract class ConcaveShape : CollisionShape | ||
30 | { | ||
31 | private float _collisionMargin; | ||
32 | |||
33 | public ConcaveShape() { } | ||
34 | |||
35 | public float CollisionMargin | ||
36 | { | ||
37 | get { return _collisionMargin; } | ||
38 | set { _collisionMargin = value; } | ||
39 | } | ||
40 | |||
41 | public override float Margin | ||
42 | { | ||
43 | get | ||
44 | { | ||
45 | return _collisionMargin; | ||
46 | } | ||
47 | set | ||
48 | { | ||
49 | _collisionMargin = value; | ||
50 | } | ||
51 | } | ||
52 | |||
53 | public abstract void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax); | ||
54 | } | ||
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 new file mode 100644 index 0000000..361c277 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs | |||
@@ -0,0 +1,208 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// ConeShape implements a Cone shape, around the X axis | ||
31 | /// </summary> | ||
32 | public class ConeShapeX : ConeShape | ||
33 | { | ||
34 | public ConeShapeX(float radius, float height) | ||
35 | : base(radius, height) | ||
36 | { | ||
37 | ConeUpIndex = 0; | ||
38 | } | ||
39 | } | ||
40 | |||
41 | /// <summary> | ||
42 | /// ConeShape implements a Cone shape, around the Z axis | ||
43 | /// </summary> | ||
44 | public class ConeShapeZ : ConeShape | ||
45 | { | ||
46 | public ConeShapeZ(float radius, float height) | ||
47 | : base(radius, height) | ||
48 | { | ||
49 | ConeUpIndex = 2; | ||
50 | } | ||
51 | } | ||
52 | |||
53 | /// <summary> | ||
54 | /// ConeShape implements a Cone shape, around the Y axis | ||
55 | /// </summary> | ||
56 | public class ConeShape : ConvexShape | ||
57 | { | ||
58 | private float _sinAngle; | ||
59 | private float _radius; | ||
60 | private float _height; | ||
61 | private int[] _coneIndices = new int[3]; | ||
62 | |||
63 | public ConeShape(float radius, float height) | ||
64 | { | ||
65 | _radius = radius; | ||
66 | _height = height; | ||
67 | ConeUpIndex = 1; | ||
68 | _sinAngle = (_radius / (float)Math.Sqrt(_radius * _radius + _height * _height)); | ||
69 | } | ||
70 | |||
71 | public float Radius { get { return _radius; } } | ||
72 | public float Height { get { return _height; } } | ||
73 | |||
74 | public override BroadphaseNativeTypes ShapeType | ||
75 | { | ||
76 | get | ||
77 | { | ||
78 | return BroadphaseNativeTypes.Cone; | ||
79 | } | ||
80 | } | ||
81 | |||
82 | public override string Name | ||
83 | { | ||
84 | get | ||
85 | { | ||
86 | return "Cone"; | ||
87 | } | ||
88 | } | ||
89 | |||
90 | //choose upAxis index | ||
91 | public int ConeUpIndex | ||
92 | { | ||
93 | get { return _coneIndices[1]; } | ||
94 | set | ||
95 | { | ||
96 | switch (value) | ||
97 | { | ||
98 | case 0: | ||
99 | _coneIndices[0] = 1; | ||
100 | _coneIndices[1] = 0; | ||
101 | _coneIndices[2] = 2; | ||
102 | break; | ||
103 | case 1: | ||
104 | _coneIndices[0] = 0; | ||
105 | _coneIndices[1] = 1; | ||
106 | _coneIndices[2] = 2; | ||
107 | break; | ||
108 | case 2: | ||
109 | _coneIndices[0] = 0; | ||
110 | _coneIndices[1] = 2; | ||
111 | _coneIndices[2] = 1; | ||
112 | break; | ||
113 | default: | ||
114 | BulletDebug.Assert(false); | ||
115 | break; | ||
116 | } | ||
117 | } | ||
118 | } | ||
119 | |||
120 | private Vector3 ConeLocalSupport(Vector3 v) | ||
121 | { | ||
122 | float halfHeight = _height * 0.5f; | ||
123 | bool condition; | ||
124 | |||
125 | if (_coneIndices[1] == 0) | ||
126 | condition = v.X > v.Length() * _sinAngle; | ||
127 | else if (_coneIndices[1] == 1) | ||
128 | condition = v.Y > v.Length() * _sinAngle; | ||
129 | else | ||
130 | condition = v.Z > v.Length() * _sinAngle; | ||
131 | |||
132 | if (condition) | ||
133 | { | ||
134 | Vector3 tmp = new Vector3(); | ||
135 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], halfHeight); | ||
136 | return tmp; | ||
137 | } | ||
138 | else | ||
139 | { | ||
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])); | ||
142 | if (s > MathHelper.Epsilon) | ||
143 | { | ||
144 | float d = _radius / s; | ||
145 | Vector3 tmp = new Vector3(); | ||
146 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[0], MathHelper.GetValueByIndex(v, _coneIndices[0]) * d); | ||
147 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); | ||
148 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[2], MathHelper.GetValueByIndex(v, _coneIndices[2]) * d); | ||
149 | return tmp; | ||
150 | } | ||
151 | else | ||
152 | { | ||
153 | Vector3 tmp = new Vector3(); | ||
154 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); | ||
155 | return tmp; | ||
156 | } | ||
157 | } | ||
158 | } | ||
159 | |||
160 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
161 | { | ||
162 | return ConeLocalSupport(vec); | ||
163 | } | ||
164 | |||
165 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
166 | { | ||
167 | for (int i = 0; i < vectors.Length; i++) | ||
168 | supportVerticesOut[i] = ConeLocalSupport(vectors[i]); | ||
169 | } | ||
170 | |||
171 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
172 | { | ||
173 | Matrix identity = Matrix.Identity; | ||
174 | Vector3 aabbMin, aabbMax; | ||
175 | GetAabb(identity, out aabbMin, out aabbMax); | ||
176 | |||
177 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; | ||
178 | |||
179 | float margin = Margin; | ||
180 | |||
181 | float lx = 2f * (halfExtents.X + margin); | ||
182 | float ly = 2f * (halfExtents.Y + margin); | ||
183 | float lz = 2f * (halfExtents.Z + margin); | ||
184 | float x2 = lx * lx; | ||
185 | float y2 = ly * ly; | ||
186 | float z2 = lz * lz; | ||
187 | float scaledmass = mass * 0.08333333f; | ||
188 | |||
189 | inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2)); | ||
190 | } | ||
191 | |||
192 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | ||
193 | { | ||
194 | Vector3 supVertex = ConeLocalSupport(vec); | ||
195 | if (Margin != 0) | ||
196 | { | ||
197 | Vector3 vecnorm = vec; | ||
198 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | ||
199 | { | ||
200 | vecnorm = new Vector3(-1f, -1f, -1f); | ||
201 | } | ||
202 | vecnorm = Vector3.Normalize(vecnorm); | ||
203 | supVertex += Margin * vecnorm; | ||
204 | } | ||
205 | return supVertex; | ||
206 | } | ||
207 | } | ||
208 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs new file mode 100644 index 0000000..eb78533 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs | |||
@@ -0,0 +1,184 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (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. | ||
33 | /// (memory is much slower then the cpu) | ||
34 | /// </summary> | ||
35 | public class ConvexHullShape : PolyhedralConvexShape | ||
36 | { | ||
37 | private List<Vector3> _points = new List<Vector3>(); | ||
38 | |||
39 | public ConvexHullShape() { } | ||
40 | |||
41 | public override int VertexCount | ||
42 | { | ||
43 | get | ||
44 | { | ||
45 | return _points.Count; | ||
46 | } | ||
47 | } | ||
48 | |||
49 | public override int EdgeCount | ||
50 | { | ||
51 | get | ||
52 | { | ||
53 | return _points.Count; | ||
54 | } | ||
55 | } | ||
56 | |||
57 | public override int PlaneCount | ||
58 | { | ||
59 | get | ||
60 | { | ||
61 | return 0; | ||
62 | } | ||
63 | } | ||
64 | |||
65 | public override BroadphaseNativeTypes ShapeType | ||
66 | { | ||
67 | get | ||
68 | { | ||
69 | return BroadphaseNativeTypes.ConvexHull; | ||
70 | } | ||
71 | } | ||
72 | |||
73 | public override string Name | ||
74 | { | ||
75 | get | ||
76 | { | ||
77 | return "Convex"; | ||
78 | } | ||
79 | } | ||
80 | |||
81 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | ||
82 | { | ||
83 | Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); | ||
84 | |||
85 | if (Margin != 0) | ||
86 | { | ||
87 | Vector3 vecnorm = vec; | ||
88 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | ||
89 | { | ||
90 | vecnorm=new Vector3(-1, -1, -1); | ||
91 | } | ||
92 | vecnorm = Vector3.Normalize(vecnorm); | ||
93 | supVertex += Margin * vecnorm; | ||
94 | } | ||
95 | return supVertex; | ||
96 | } | ||
97 | |||
98 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0) | ||
99 | { | ||
100 | Vector3 supVec = new Vector3(); | ||
101 | float newDot, maxDot = -1e30f; | ||
102 | |||
103 | Vector3 vec = vec0; | ||
104 | float lenSqr = vec.LengthSquared(); | ||
105 | if (lenSqr < 0.0001f) | ||
106 | { | ||
107 | vec = new Vector3(1, 0, 0); | ||
108 | } | ||
109 | else | ||
110 | { | ||
111 | float rlen = 1f / (float)Math.Sqrt(lenSqr); | ||
112 | vec *= rlen; | ||
113 | } | ||
114 | |||
115 | for (int i = 0; i < _points.Count; i++) | ||
116 | { | ||
117 | Vector3 vtx = _points[i] * LocalScaling; | ||
118 | |||
119 | newDot = Vector3.Dot(vec, vtx); | ||
120 | if (newDot > maxDot) | ||
121 | { | ||
122 | maxDot = newDot; | ||
123 | supVec = vtx; | ||
124 | } | ||
125 | } | ||
126 | return supVec; | ||
127 | } | ||
128 | |||
129 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
130 | { | ||
131 | float newDot; | ||
132 | //use 'w' component of supportVerticesOut? | ||
133 | /*{ | ||
134 | for (int i = 0; i < numVectors; i++) | ||
135 | { | ||
136 | supportVerticesOut[i][3] = -1e30f; | ||
137 | } | ||
138 | }*/ | ||
139 | #warning Warning! | ||
140 | for (int i = 0; i < _points.Count; i++) | ||
141 | { | ||
142 | Vector3 vtx = _points[i] * LocalScaling; | ||
143 | |||
144 | for (int j = 0; j < vectors.Length; j++) | ||
145 | { | ||
146 | newDot = Vector3.Dot(vectors[j], vtx); | ||
147 | if (newDot > -1e30f) | ||
148 | { | ||
149 | //WARNING: don't swap next lines, the w component would get overwritten! | ||
150 | supportVerticesOut[j] = vtx; | ||
151 | //supportVerticesOut[j][3] = newDot; | ||
152 | #warning Warning! | ||
153 | } | ||
154 | } | ||
155 | } | ||
156 | } | ||
157 | |||
158 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) | ||
159 | { | ||
160 | int index0 = i % _points.Count; | ||
161 | int index1 = (i + 1) % _points.Count; | ||
162 | pa = _points[index0] * LocalScaling; | ||
163 | pb = _points[index1] * LocalScaling; | ||
164 | } | ||
165 | |||
166 | public override void GetVertex(int i, out Vector3 vtx) | ||
167 | { | ||
168 | vtx = _points[i] * LocalScaling; | ||
169 | } | ||
170 | |||
171 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) | ||
172 | { | ||
173 | planeNormal = new Vector3(); | ||
174 | planeSupport = new Vector3(); | ||
175 | BulletDebug.Assert(false); | ||
176 | } | ||
177 | |||
178 | public override bool IsInside(Vector3 pt, float tolerance) | ||
179 | { | ||
180 | BulletDebug.Assert(false); | ||
181 | return false; | ||
182 | } | ||
183 | } | ||
184 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs new file mode 100644 index 0000000..02d678e --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs | |||
@@ -0,0 +1,141 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// ConvexShape is an abstract shape interface. | ||
31 | /// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface. | ||
32 | /// used in combination with GJK or btConvexCast | ||
33 | /// </summary> | ||
34 | public abstract class ConvexShape : CollisionShape | ||
35 | { | ||
36 | private const int _maxPreferredPenetrationDirections = 10; | ||
37 | private const float _convexDistanceMargin = 0.04f; | ||
38 | |||
39 | private Vector3 _localScaling; | ||
40 | private Vector3 _implicitShapeDimensions; | ||
41 | private float _collisionMargin; | ||
42 | |||
43 | public ConvexShape() | ||
44 | : base() | ||
45 | { | ||
46 | _localScaling = Vector3.One; | ||
47 | _collisionMargin = ConvexDistanceMargin; | ||
48 | } | ||
49 | |||
50 | public static int MaxPreferredPenetrationDirections { get { return _maxPreferredPenetrationDirections; } } | ||
51 | public static float ConvexDistanceMargin { get { return _convexDistanceMargin; } } | ||
52 | |||
53 | public Vector3 ImplicitShapeDimensions { get { return _implicitShapeDimensions; } protected set { _implicitShapeDimensions = value; } } | ||
54 | public virtual int PreferredPenetrationDirectionsCount { get { return 0; } } | ||
55 | |||
56 | protected float CollisionMargin { get { return _collisionMargin; } set { _collisionMargin = value; } } | ||
57 | |||
58 | public virtual void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) | ||
59 | { | ||
60 | penetrationVector = new Vector3(); | ||
61 | BulletDebug.Assert(false); | ||
62 | } | ||
63 | |||
64 | public abstract Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec); | ||
65 | //notice that the vectors should be unit length | ||
66 | public abstract void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut); | ||
67 | |||
68 | /// <summary> | ||
69 | /// getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version | ||
70 | /// </summary> | ||
71 | /// <param name="t"></param> | ||
72 | /// <param name="aabbMin"></param> | ||
73 | /// <param name="aabbMax"></param> | ||
74 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
75 | { | ||
76 | GetAabbSlow(t, out aabbMin, out aabbMax); | ||
77 | } | ||
78 | |||
79 | public override Vector3 LocalScaling | ||
80 | { | ||
81 | get | ||
82 | { | ||
83 | return _localScaling; | ||
84 | } | ||
85 | set | ||
86 | { | ||
87 | _localScaling = value; | ||
88 | } | ||
89 | } | ||
90 | |||
91 | public override float Margin | ||
92 | { | ||
93 | get | ||
94 | { | ||
95 | return _collisionMargin; | ||
96 | } | ||
97 | set | ||
98 | { | ||
99 | _collisionMargin = value; | ||
100 | } | ||
101 | } | ||
102 | |||
103 | public virtual Vector3 LocalGetSupportingVertex(Vector3 vec) | ||
104 | { | ||
105 | Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); | ||
106 | |||
107 | if (Margin != 0f) | ||
108 | { | ||
109 | Vector3 vecnorm = vec; | ||
110 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | ||
111 | { | ||
112 | vecnorm = new Vector3(-1f, -1f, -1f); | ||
113 | } | ||
114 | vecnorm.Normalize(); | ||
115 | supVertex += Margin * vecnorm; | ||
116 | } | ||
117 | return supVertex; | ||
118 | } | ||
119 | |||
120 | public virtual void GetAabbSlow(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
121 | { | ||
122 | float margin = Margin; | ||
123 | aabbMax = new Vector3(); | ||
124 | aabbMin = new Vector3(); | ||
125 | |||
126 | for (int i = 0; i < 3; i++) | ||
127 | { | ||
128 | Vector3 vec = new Vector3(0f, 0f, 0f); | ||
129 | MathHelper.SetElement(ref vec, i, 1); | ||
130 | |||
131 | Vector3 sv = LocalGetSupportingVertex(Vector3.TransformNormal(vec, t)); | ||
132 | |||
133 | Vector3 tmp = MathHelper.MatrixToVector(t, sv); | ||
134 | MathHelper.SetElement(ref aabbMax, i, MathHelper.GetElement(tmp, i) + margin); | ||
135 | MathHelper.SetElement(ref vec, i, -1f); | ||
136 | tmp = MathHelper.MatrixToVector(t, LocalGetSupportingVertex(Vector3.TransformNormal(vec, t))); | ||
137 | MathHelper.SetElement(ref aabbMin, i, MathHelper.GetElement(tmp, i) - margin); | ||
138 | } | ||
139 | } | ||
140 | } | ||
141 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs new file mode 100644 index 0000000..0fd3fa7 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs | |||
@@ -0,0 +1,185 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// 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. | ||
32 | /// </summary> | ||
33 | public class ConvexTriangleMeshShape : PolyhedralConvexShape | ||
34 | { | ||
35 | private StridingMeshInterface _stridingMesh; | ||
36 | |||
37 | public ConvexTriangleMeshShape(StridingMeshInterface meshInterface) | ||
38 | { | ||
39 | _stridingMesh = meshInterface; | ||
40 | } | ||
41 | |||
42 | public StridingMeshInterface getStridingMesh() | ||
43 | { | ||
44 | return _stridingMesh; | ||
45 | } | ||
46 | |||
47 | public override int VertexCount | ||
48 | { | ||
49 | get | ||
50 | { | ||
51 | return 0; | ||
52 | } | ||
53 | } | ||
54 | |||
55 | public override int EdgeCount | ||
56 | { | ||
57 | get | ||
58 | { | ||
59 | return 0; | ||
60 | } | ||
61 | } | ||
62 | |||
63 | public override int PlaneCount | ||
64 | { | ||
65 | get | ||
66 | { | ||
67 | return 0; | ||
68 | } | ||
69 | } | ||
70 | |||
71 | public override Vector3 LocalScaling | ||
72 | { | ||
73 | get | ||
74 | { | ||
75 | return base.LocalScaling; | ||
76 | } | ||
77 | set | ||
78 | { | ||
79 | _stridingMesh.Scaling = value; | ||
80 | } | ||
81 | } | ||
82 | |||
83 | public override BroadphaseNativeTypes ShapeType | ||
84 | { | ||
85 | get | ||
86 | { | ||
87 | return BroadphaseNativeTypes.ConvexTriangleMesh; | ||
88 | } | ||
89 | } | ||
90 | |||
91 | public override string Name | ||
92 | { | ||
93 | get | ||
94 | { | ||
95 | return "ConvexTrimesh"; | ||
96 | } | ||
97 | } | ||
98 | |||
99 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) | ||
100 | { | ||
101 | pa = new Vector3(); | ||
102 | pb = new Vector3(); | ||
103 | BulletDebug.Assert(false); | ||
104 | } | ||
105 | |||
106 | public override void GetVertex(int i, out Vector3 vtx) | ||
107 | { | ||
108 | vtx = new Vector3(); | ||
109 | BulletDebug.Assert(false); | ||
110 | } | ||
111 | |||
112 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) | ||
113 | { | ||
114 | planeNormal = new Vector3(); | ||
115 | planeSupport = new Vector3(); | ||
116 | BulletDebug.Assert(false); | ||
117 | } | ||
118 | |||
119 | public override bool IsInside(Vector3 pt, float tolerance) | ||
120 | { | ||
121 | BulletDebug.Assert(false); | ||
122 | return false; | ||
123 | } | ||
124 | |||
125 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | ||
126 | { | ||
127 | Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); | ||
128 | |||
129 | if (Margin != 0) | ||
130 | { | ||
131 | Vector3 vecnorm = vec; | ||
132 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | ||
133 | { | ||
134 | vecnorm = new Vector3(-1, -1, -1); | ||
135 | } | ||
136 | vecnorm = Vector3.Normalize(vecnorm); | ||
137 | supVertex += Margin * vecnorm; | ||
138 | } | ||
139 | return supVertex; | ||
140 | } | ||
141 | |||
142 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0) | ||
143 | { | ||
144 | Vector3 supVec = new Vector3(); | ||
145 | |||
146 | Vector3 vec = vec0; | ||
147 | float lenSqr = vec.LengthSquared(); | ||
148 | if (lenSqr < 0.0001f) | ||
149 | { | ||
150 | vec = new Vector3(1, 0, 0); | ||
151 | } | ||
152 | else | ||
153 | { | ||
154 | float rlen = 1f / (float)Math.Sqrt(lenSqr); | ||
155 | vec *= rlen; | ||
156 | } | ||
157 | |||
158 | LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec); | ||
159 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | ||
160 | _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); | ||
161 | supVec = supportCallback.SupportVertexLocal; | ||
162 | |||
163 | return supVec; | ||
164 | } | ||
165 | |||
166 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
167 | { | ||
168 | //use 'w' component of supportVerticesOut? | ||
169 | /*{ | ||
170 | for (int i = 0; i < numVectors; i++) | ||
171 | { | ||
172 | supportVerticesOut[i][3] = -1e30f; | ||
173 | } | ||
174 | }*/ | ||
175 | for (int j = 0; j < vectors.Length; j++) | ||
176 | { | ||
177 | Vector3 vec = vectors[j]; | ||
178 | LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec); | ||
179 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | ||
180 | _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); | ||
181 | supportVerticesOut[j] = supportCallback.SupportVertexLocal; | ||
182 | } | ||
183 | } | ||
184 | } | ||
185 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs new file mode 100644 index 0000000..9a86eaf --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs | |||
@@ -0,0 +1,136 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// implements cylinder shape interface | ||
31 | /// </summary> | ||
32 | public class CylinderShape : BoxShape | ||
33 | { | ||
34 | public CylinderShape(Vector3 halfExtents) | ||
35 | : base(halfExtents) | ||
36 | { | ||
37 | } | ||
38 | |||
39 | public override BroadphaseNativeTypes ShapeType | ||
40 | { | ||
41 | get | ||
42 | { | ||
43 | return BroadphaseNativeTypes.Cylinder; | ||
44 | } | ||
45 | } | ||
46 | |||
47 | public virtual int UpAxis | ||
48 | { | ||
49 | get | ||
50 | { | ||
51 | return 1; | ||
52 | } | ||
53 | } | ||
54 | |||
55 | public virtual float Radius | ||
56 | { | ||
57 | get | ||
58 | { | ||
59 | return HalfExtents.Z; | ||
60 | } | ||
61 | } | ||
62 | |||
63 | //debugging | ||
64 | public override string Name | ||
65 | { | ||
66 | get | ||
67 | { | ||
68 | return "CylinderY"; | ||
69 | } | ||
70 | } | ||
71 | |||
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) | ||
74 | { | ||
75 | GetAabbSlow(t, out aabbMin, out aabbMax); | ||
76 | } | ||
77 | |||
78 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
79 | { | ||
80 | return CylinderLocalSupportY(HalfExtents, vec); | ||
81 | } | ||
82 | |||
83 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
84 | { | ||
85 | for (int i = 0; i < vectors.Length; i++) | ||
86 | { | ||
87 | supportVerticesOut[i] = CylinderLocalSupportY(HalfExtents, vectors[i]); | ||
88 | } | ||
89 | } | ||
90 | |||
91 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | ||
92 | { | ||
93 | |||
94 | Vector3 supVertex; | ||
95 | supVertex = LocalGetSupportingVertexWithoutMargin(vec); | ||
96 | |||
97 | if (Margin != 0) | ||
98 | { | ||
99 | Vector3 vecnorm = vec; | ||
100 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | ||
101 | { | ||
102 | vecnorm=new Vector3(-1, -1, -1); | ||
103 | } | ||
104 | vecnorm = Vector3.Normalize(vecnorm); | ||
105 | supVertex += Margin * vecnorm; | ||
106 | } | ||
107 | return supVertex; | ||
108 | } | ||
109 | |||
110 | private Vector3 CylinderLocalSupportY(Vector3 halfExtents, Vector3 v) | ||
111 | { | ||
112 | float radius = halfExtents.X; | ||
113 | float halfHeight = halfExtents.Y; | ||
114 | |||
115 | Vector3 tmp = new Vector3(); | ||
116 | float d; | ||
117 | |||
118 | float s = (float)Math.Sqrt(v.X * v.X + v.Z * v.Z); | ||
119 | if (s != 0) | ||
120 | { | ||
121 | d = radius / s; | ||
122 | tmp.X = v.X * d; | ||
123 | tmp.Y = v.Y < 0 ? -halfHeight : halfHeight; | ||
124 | tmp.Z = v.Z * d; | ||
125 | return tmp; | ||
126 | } | ||
127 | else | ||
128 | { | ||
129 | tmp.X = radius; | ||
130 | tmp.Y = v.Y < 0 ? -halfHeight : halfHeight; | ||
131 | tmp.Z = 0; | ||
132 | return tmp; | ||
133 | } | ||
134 | } | ||
135 | } | ||
136 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs new file mode 100644 index 0000000..9dc8e2d --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs | |||
@@ -0,0 +1,100 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class CylinderShapeX : CylinderShape | ||
30 | { | ||
31 | public CylinderShapeX(Vector3 halfExtents) | ||
32 | : base(halfExtents) { } | ||
33 | |||
34 | public override int UpAxis | ||
35 | { | ||
36 | get | ||
37 | { | ||
38 | return 0; | ||
39 | } | ||
40 | } | ||
41 | |||
42 | public override float Radius | ||
43 | { | ||
44 | get | ||
45 | { | ||
46 | return HalfExtents.Y; | ||
47 | } | ||
48 | } | ||
49 | |||
50 | //debugging | ||
51 | public override string Name | ||
52 | { | ||
53 | get | ||
54 | { | ||
55 | return "CylinderX"; | ||
56 | } | ||
57 | } | ||
58 | |||
59 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
60 | { | ||
61 | return CylinderLocalSupportX(HalfExtents, vec); | ||
62 | } | ||
63 | |||
64 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
65 | { | ||
66 | for (int i = 0; i < vectors.Length; i++) | ||
67 | { | ||
68 | supportVerticesOut[i] = CylinderLocalSupportX(HalfExtents, vectors[i]); | ||
69 | } | ||
70 | } | ||
71 | |||
72 | private Vector3 CylinderLocalSupportX(Vector3 halfExtents, Vector3 v) | ||
73 | { | ||
74 | //mapping depends on how cylinder local orientation is | ||
75 | // extents of the cylinder is: X,Y is for radius, and Z for height | ||
76 | float radius = halfExtents.Y; | ||
77 | float halfHeight = halfExtents.X; | ||
78 | |||
79 | Vector3 tmp = new Vector3(); | ||
80 | float d; | ||
81 | |||
82 | float s = (float)Math.Sqrt(v.Y * v.Y + v.Z * v.Z); | ||
83 | if (s != 0) | ||
84 | { | ||
85 | d = radius / s; | ||
86 | tmp.Y = v.Y * d; | ||
87 | tmp.X = v.X < 0 ? -halfHeight : halfHeight; | ||
88 | tmp.Z = v.Z * d; | ||
89 | return tmp; | ||
90 | } | ||
91 | else | ||
92 | { | ||
93 | tmp.Y = radius; | ||
94 | tmp.X = v.X < 0 ? -halfHeight : halfHeight; | ||
95 | tmp.Z = 0; | ||
96 | return tmp; | ||
97 | } | ||
98 | } | ||
99 | } | ||
100 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs new file mode 100644 index 0000000..1913e9c --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs | |||
@@ -0,0 +1,100 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class CylinderShapeZ : CylinderShape | ||
30 | { | ||
31 | public CylinderShapeZ(Vector3 halfExtents) | ||
32 | : base(halfExtents) { } | ||
33 | |||
34 | public override int UpAxis | ||
35 | { | ||
36 | get | ||
37 | { | ||
38 | return 2; | ||
39 | } | ||
40 | } | ||
41 | |||
42 | public override float Radius | ||
43 | { | ||
44 | get | ||
45 | { | ||
46 | return HalfExtents.X; | ||
47 | } | ||
48 | } | ||
49 | |||
50 | //debugging | ||
51 | public override string Name | ||
52 | { | ||
53 | get | ||
54 | { | ||
55 | return "CylinderZ"; | ||
56 | } | ||
57 | } | ||
58 | |||
59 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
60 | { | ||
61 | return CylinderLocalSupportZ(HalfExtents, vec); | ||
62 | } | ||
63 | |||
64 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
65 | { | ||
66 | for (int i = 0; i < vectors.Length; i++) | ||
67 | { | ||
68 | supportVerticesOut[i] = CylinderLocalSupportZ(HalfExtents, vectors[i]); | ||
69 | } | ||
70 | } | ||
71 | |||
72 | Vector3 CylinderLocalSupportZ(Vector3 halfExtents, Vector3 v) | ||
73 | { | ||
74 | //mapping depends on how cylinder local orientation is | ||
75 | // extents of the cylinder is: X,Y is for radius, and Z for height | ||
76 | float radius = halfExtents.X; | ||
77 | float halfHeight = halfExtents.Z; | ||
78 | |||
79 | Vector3 tmp = new Vector3(); | ||
80 | float d; | ||
81 | |||
82 | float s = (float)Math.Sqrt(v.X * v.X + v.Y * v.Y); | ||
83 | if (s != 0) | ||
84 | { | ||
85 | d = radius / s; | ||
86 | tmp.X = v.X * d; | ||
87 | tmp.Z = v.Z < 0 ? -halfHeight : halfHeight; | ||
88 | tmp.Y = v.Y * d; | ||
89 | return tmp; | ||
90 | } | ||
91 | else | ||
92 | { | ||
93 | tmp.X = radius; | ||
94 | tmp.Z = v.Z < 0 ? -halfHeight : halfHeight; | ||
95 | tmp.Y = 0; | ||
96 | return tmp; | ||
97 | } | ||
98 | } | ||
99 | } | ||
100 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs new file mode 100644 index 0000000..b094c39 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs | |||
@@ -0,0 +1,80 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | using System.Diagnostics; | ||
27 | |||
28 | namespace XnaDevRu.BulletX | ||
29 | { | ||
30 | public class EmptyShape : ConcaveShape | ||
31 | { | ||
32 | private Vector3 _localScaling; | ||
33 | |||
34 | public override string Name | ||
35 | { | ||
36 | get | ||
37 | { | ||
38 | return "Empty"; | ||
39 | } | ||
40 | } | ||
41 | |||
42 | public override BroadphaseNativeTypes ShapeType | ||
43 | { | ||
44 | get | ||
45 | { | ||
46 | return BroadphaseNativeTypes.Empty; | ||
47 | } | ||
48 | } | ||
49 | |||
50 | public override Vector3 LocalScaling | ||
51 | { | ||
52 | get | ||
53 | { | ||
54 | return _localScaling; | ||
55 | } | ||
56 | set | ||
57 | { | ||
58 | _localScaling = value; | ||
59 | } | ||
60 | } | ||
61 | |||
62 | public override void ProcessAllTriangles(ITriangleCallback callback, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax) | ||
63 | { | ||
64 | throw new Exception("The method or operation is not implemented."); | ||
65 | } | ||
66 | |||
67 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
68 | { | ||
69 | Vector3 margin = new Vector3(Margin, Margin, Margin); | ||
70 | aabbMin = t.Translation - margin; | ||
71 | aabbMax = t.Translation + margin; | ||
72 | } | ||
73 | |||
74 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
75 | { | ||
76 | inertia = new Vector3(); | ||
77 | BulletDebug.Assert(false); | ||
78 | } | ||
79 | } | ||
80 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs new file mode 100644 index 0000000..5073096 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs | |||
@@ -0,0 +1,55 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class FilteredCallback : ITriangleIndexCallback | ||
30 | { | ||
31 | private ITriangleCallback _callback; | ||
32 | private Vector3 _aabbMin; | ||
33 | private Vector3 _aabbMax; | ||
34 | |||
35 | public FilteredCallback(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) | ||
36 | { | ||
37 | _callback = callback; | ||
38 | _aabbMin = aabbMin; | ||
39 | _aabbMax = aabbMax; | ||
40 | } | ||
41 | |||
42 | public ITriangleCallback TriangleCallback { get { return _callback; } set { _callback = value; } } | ||
43 | public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } } | ||
44 | public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } } | ||
45 | |||
46 | public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) | ||
47 | { | ||
48 | if (MathHelper.TestTriangleAgainstAabb2(triangle, _aabbMin, _aabbMax)) | ||
49 | { | ||
50 | //check aabb in triangle-space, before doing this | ||
51 | _callback.ProcessTriangle(triangle, partId, triangleIndex); | ||
52 | } | ||
53 | } | ||
54 | } | ||
55 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs new file mode 100644 index 0000000..b88d804 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs | |||
@@ -0,0 +1,33 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public interface ITriangleIndexCallback | ||
30 | { | ||
31 | void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex); | ||
32 | } | ||
33 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs new file mode 100644 index 0000000..951707d --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs | |||
@@ -0,0 +1,58 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class LocalSupportVertexCallback : ITriangleIndexCallback | ||
30 | { | ||
31 | private Vector3 _supportVertexLocal; | ||
32 | private float _maxDot; | ||
33 | private Vector3 _supportVecLocal; | ||
34 | |||
35 | public LocalSupportVertexCallback(Vector3 supportVecLocal) | ||
36 | { | ||
37 | _supportVertexLocal = new Vector3(); | ||
38 | _maxDot = -1e30f; | ||
39 | _supportVecLocal = supportVecLocal; | ||
40 | } | ||
41 | |||
42 | public float MaxDot { get { return _maxDot; } set { _maxDot = value; } } | ||
43 | public Vector3 SupportVertexLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } } | ||
44 | |||
45 | public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) | ||
46 | { | ||
47 | for (int i = 0; i < 3; i++) | ||
48 | { | ||
49 | float dot = Vector3.Dot(_supportVecLocal, triangle[i]); | ||
50 | if (dot > _maxDot) | ||
51 | { | ||
52 | _maxDot = dot; | ||
53 | _supportVertexLocal = triangle[i]; | ||
54 | } | ||
55 | } | ||
56 | } | ||
57 | } | ||
58 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs new file mode 100644 index 0000000..6f70cae --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs | |||
@@ -0,0 +1,99 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// MinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes. | ||
31 | /// </summary> | ||
32 | public class MinkowskiSumShape : ConvexShape | ||
33 | { | ||
34 | private Matrix _transformA; | ||
35 | private Matrix _transformB; | ||
36 | private ConvexShape _shapeA; | ||
37 | private ConvexShape _shapeB; | ||
38 | |||
39 | public MinkowskiSumShape(ConvexShape shapeA, ConvexShape shapeB) | ||
40 | { | ||
41 | _shapeA = shapeA; | ||
42 | _shapeB = shapeB; | ||
43 | _transformA = Matrix.Identity; | ||
44 | _transformB = Matrix.Identity; | ||
45 | } | ||
46 | |||
47 | public Matrix TransformA { get { return _transformA; } set { _transformA = value; } } | ||
48 | public Matrix TransformB { get { return _transformB; } set { _transformB = value; } } | ||
49 | public ConvexShape ShapeA { get { return _shapeA; } } | ||
50 | public ConvexShape ShapeB { get { return _shapeB; } } | ||
51 | |||
52 | public override float Margin | ||
53 | { | ||
54 | get | ||
55 | { | ||
56 | return _shapeA.Margin + _shapeB.Margin; | ||
57 | } | ||
58 | set | ||
59 | { | ||
60 | base.Margin = value; | ||
61 | } | ||
62 | } | ||
63 | |||
64 | public override BroadphaseNativeTypes ShapeType | ||
65 | { | ||
66 | get | ||
67 | { | ||
68 | return BroadphaseNativeTypes.MinkowskiDifference; | ||
69 | } | ||
70 | } | ||
71 | |||
72 | public override string Name | ||
73 | { | ||
74 | get | ||
75 | { | ||
76 | return "MinkowskiSum"; | ||
77 | } | ||
78 | } | ||
79 | |||
80 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
81 | { | ||
82 | Vector3 supVertexA = MathHelper.MatrixToVector(_transformA, _shapeA.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformA))); | ||
83 | Vector3 supVertexB = MathHelper.MatrixToVector(_transformB, _shapeB.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformB))); | ||
84 | return supVertexA + supVertexB; | ||
85 | } | ||
86 | |||
87 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
88 | { | ||
89 | for (int i = 0; i < vectors.Length; i++) | ||
90 | supportVerticesOut[i] = LocalGetSupportingVertexWithoutMargin(vectors[i]); | ||
91 | } | ||
92 | |||
93 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
94 | { | ||
95 | inertia = new Vector3(); | ||
96 | BulletDebug.Assert(false); | ||
97 | } | ||
98 | } | ||
99 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs new file mode 100644 index 0000000..9959c01 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs | |||
@@ -0,0 +1,154 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// MultiSphereShape represents implicit convex hull of a collection of spheres (using getSupportingVertex) | ||
31 | /// </summary> | ||
32 | public class MultiSphereShape : ConvexShape | ||
33 | { | ||
34 | private const int _maxNumSpheres = 5; | ||
35 | private Vector3[] _localPositions = new Vector3[MaxNumSpheres]; | ||
36 | private float[] _radi = new float[MaxNumSpheres]; | ||
37 | private Vector3 _inertiaHalfExtents; | ||
38 | |||
39 | private int m_numSpheres; | ||
40 | |||
41 | public MultiSphereShape(Vector3 inertiaHalfExtents, Vector3[] positions, float[] radi, int numSpheres) | ||
42 | { | ||
43 | _inertiaHalfExtents = inertiaHalfExtents; | ||
44 | float startMargin = 1e30f; | ||
45 | |||
46 | m_numSpheres = numSpheres; | ||
47 | for (int i = 0; i < m_numSpheres; i++) | ||
48 | { | ||
49 | _localPositions[i] = positions[i]; | ||
50 | _radi[i] = radi[i]; | ||
51 | if (radi[i] < startMargin) | ||
52 | startMargin = radi[i]; | ||
53 | } | ||
54 | Margin = startMargin; | ||
55 | } | ||
56 | |||
57 | public static int MaxNumSpheres { get { return _maxNumSpheres; } } | ||
58 | |||
59 | public override BroadphaseNativeTypes ShapeType | ||
60 | { | ||
61 | get | ||
62 | { | ||
63 | return BroadphaseNativeTypes.MultiSphere; | ||
64 | } | ||
65 | } | ||
66 | |||
67 | public override string Name | ||
68 | { | ||
69 | get | ||
70 | { | ||
71 | return "MultiSphere"; | ||
72 | } | ||
73 | } | ||
74 | |||
75 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vecA) | ||
76 | { | ||
77 | Vector3 supVec = new Vector3(); | ||
78 | |||
79 | float maxDot = -1e30f; | ||
80 | |||
81 | |||
82 | Vector3 vec = vecA; | ||
83 | float lenSqr = vec.LengthSquared(); | ||
84 | if (lenSqr < 0.0001f) | ||
85 | { | ||
86 | vec = new Vector3(1, 0, 0); | ||
87 | } | ||
88 | else | ||
89 | { | ||
90 | float rlen = 1f / (float)Math.Sqrt(lenSqr); | ||
91 | vec *= rlen; | ||
92 | } | ||
93 | |||
94 | Vector3 vtx; | ||
95 | float newDot; | ||
96 | |||
97 | for (int i = 0; i < m_numSpheres; i++) | ||
98 | { | ||
99 | vtx = _localPositions[i] + vec * LocalScaling * _radi[i] - vec * Margin; | ||
100 | newDot = Vector3.Dot(vec, vtx); | ||
101 | if (newDot > maxDot) | ||
102 | { | ||
103 | maxDot = newDot; | ||
104 | supVec = vtx; | ||
105 | } | ||
106 | } | ||
107 | |||
108 | return supVec; | ||
109 | } | ||
110 | |||
111 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
112 | { | ||
113 | for (int j = 0; j < vectors.Length; j++) | ||
114 | { | ||
115 | float maxDot = -1e30f; | ||
116 | Vector3 vtx; | ||
117 | float newDot; | ||
118 | |||
119 | for (int i = 0; i < m_numSpheres; i++) | ||
120 | { | ||
121 | vtx = _localPositions[i] + vectors[j] * LocalScaling * _radi[i] - vectors[j] * Margin; | ||
122 | newDot = Vector3.Dot(vectors[j], vtx); | ||
123 | if (newDot > maxDot) | ||
124 | { | ||
125 | maxDot = newDot; | ||
126 | supportVerticesOut[j] = vtx; | ||
127 | } | ||
128 | } | ||
129 | } | ||
130 | } | ||
131 | |||
132 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
133 | { | ||
134 | //as an approximation, take the inertia of the box that bounds the spheres | ||
135 | Matrix ident = Matrix.Identity; | ||
136 | Vector3 halfExtents = _inertiaHalfExtents; | ||
137 | |||
138 | float margin = ConvexDistanceMargin; | ||
139 | |||
140 | float lx = 2f * (halfExtents.X + margin); | ||
141 | float ly = 2f * (halfExtents.Y + margin); | ||
142 | float lz = 2f * (halfExtents.Z + margin); | ||
143 | float x2 = lx * lx; | ||
144 | float y2 = ly * ly; | ||
145 | float z2 = lz * lz; | ||
146 | float scaledmass = mass * 0.08333333f; | ||
147 | |||
148 | inertia = new Vector3(); | ||
149 | inertia.X = scaledmass * (y2 + z2); | ||
150 | inertia.Y = scaledmass * (x2 + z2); | ||
151 | inertia.Z = scaledmass * (x2 + y2); | ||
152 | } | ||
153 | } | ||
154 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs new file mode 100644 index 0000000..fac344e --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs | |||
@@ -0,0 +1,32 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public interface INodeOverlapCallback | ||
29 | { | ||
30 | void ProcessNode(OptimizedBvhNode node); | ||
31 | } | ||
32 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs new file mode 100644 index 0000000..c22c990 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs | |||
@@ -0,0 +1,293 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future) | ||
31 | /// </summary> | ||
32 | public class OptimizedBvh | ||
33 | { | ||
34 | private static int _maxIterations = 0; | ||
35 | private OptimizedBvhNode _rootNode; | ||
36 | |||
37 | private OptimizedBvhNode[] _contiguousNodes; | ||
38 | private int _curNodeIndex; | ||
39 | |||
40 | private List<OptimizedBvhNode> _leafNodes = new List<OptimizedBvhNode>(); | ||
41 | |||
42 | public OptimizedBvh() { } | ||
43 | |||
44 | public void Build(StridingMeshInterface triangles) | ||
45 | { | ||
46 | NodeTriangleCallback callback = new NodeTriangleCallback(_leafNodes); | ||
47 | |||
48 | Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); | ||
49 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | ||
50 | |||
51 | triangles.InternalProcessAllTriangles(callback, aabbMin, aabbMax); | ||
52 | |||
53 | //now we have an array of leafnodes in m_leafNodes | ||
54 | _contiguousNodes = new OptimizedBvhNode[2 * _leafNodes.Count]; | ||
55 | for (int i = 0; i < _contiguousNodes.Length; i++) | ||
56 | _contiguousNodes[i] = new OptimizedBvhNode(); | ||
57 | _curNodeIndex = 0; | ||
58 | |||
59 | _rootNode = BuildTree(_leafNodes, 0, _leafNodes.Count); | ||
60 | } | ||
61 | |||
62 | public OptimizedBvhNode BuildTree(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex) | ||
63 | { | ||
64 | OptimizedBvhNode internalNode; | ||
65 | |||
66 | int splitAxis, splitIndex, i; | ||
67 | int numIndices = endIndex - startIndex; | ||
68 | int curIndex = _curNodeIndex; | ||
69 | |||
70 | if (numIndices <= 0) | ||
71 | throw new BulletException(); | ||
72 | |||
73 | if (numIndices == 1) | ||
74 | { | ||
75 | _contiguousNodes[_curNodeIndex++] = leafNodes[startIndex]; | ||
76 | //return new (&m_contiguousNodes[m_curNodeIndex++]) btOptimizedBvhNode(leafNodes[startIndex]); | ||
77 | return leafNodes[startIndex]; | ||
78 | } | ||
79 | |||
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); | ||
82 | |||
83 | splitIndex = SortAndCalculateSplittingIndex(leafNodes, startIndex, endIndex, splitAxis); | ||
84 | |||
85 | internalNode = _contiguousNodes[_curNodeIndex++]; | ||
86 | |||
87 | internalNode.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f); | ||
88 | internalNode.AabbMin = new Vector3(1e30f, 1e30f, 1e30f); | ||
89 | |||
90 | for (i = startIndex; i < endIndex; i++) | ||
91 | { | ||
92 | internalNode.AabbMax = MathHelper.SetMax(internalNode.AabbMax, leafNodes[i].AabbMax); | ||
93 | internalNode.AabbMin = MathHelper.SetMin(internalNode.AabbMin, leafNodes[i].AabbMin); | ||
94 | } | ||
95 | |||
96 | //internalNode->m_escapeIndex; | ||
97 | internalNode.LeftChild = BuildTree(leafNodes, startIndex, splitIndex); | ||
98 | internalNode.RightChild = BuildTree(leafNodes, splitIndex, endIndex); | ||
99 | |||
100 | internalNode.EscapeIndex = _curNodeIndex - curIndex; | ||
101 | return internalNode; | ||
102 | } | ||
103 | |||
104 | public int CalculateSplittingAxis(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex) | ||
105 | { | ||
106 | Vector3 means = new Vector3(); | ||
107 | Vector3 variance = new Vector3(); | ||
108 | int numIndices = endIndex - startIndex; | ||
109 | |||
110 | for (int i = startIndex; i < endIndex; i++) | ||
111 | { | ||
112 | Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); | ||
113 | means += center; | ||
114 | } | ||
115 | means *= (1f / (float)numIndices); | ||
116 | |||
117 | for (int i = startIndex; i < endIndex; i++) | ||
118 | { | ||
119 | Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); | ||
120 | Vector3 diff2 = center - means; | ||
121 | diff2 = diff2 * diff2; | ||
122 | variance += diff2; | ||
123 | } | ||
124 | variance *= (1f / ((float)numIndices - 1)); | ||
125 | |||
126 | return MathHelper.MaxAxis(variance); | ||
127 | } | ||
128 | |||
129 | public int SortAndCalculateSplittingIndex(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex, int splitAxis) | ||
130 | { | ||
131 | int splitIndex = startIndex; | ||
132 | int numIndices = endIndex - startIndex; | ||
133 | float splitValue; | ||
134 | |||
135 | Vector3 means = new Vector3(); | ||
136 | for (int i = startIndex; i < endIndex; i++) | ||
137 | { | ||
138 | Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); | ||
139 | means += center; | ||
140 | } | ||
141 | means *= (1f / (float)numIndices); | ||
142 | |||
143 | if (splitAxis == 0) | ||
144 | splitValue = means.X; | ||
145 | else if (splitAxis == 1) | ||
146 | splitValue = means.Y; | ||
147 | else if (splitAxis == 2) | ||
148 | splitValue = means.Z; | ||
149 | else | ||
150 | throw new ArgumentException(); | ||
151 | |||
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++) | ||
154 | { | ||
155 | Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); | ||
156 | float centerSplit; | ||
157 | |||
158 | if (splitAxis == 0) | ||
159 | centerSplit = means.X; | ||
160 | else if (splitAxis == 1) | ||
161 | centerSplit = means.Y; | ||
162 | else if (splitAxis == 2) | ||
163 | centerSplit = means.Z; | ||
164 | else | ||
165 | throw new ArgumentException(); | ||
166 | |||
167 | if (centerSplit > splitValue) | ||
168 | { | ||
169 | //swap | ||
170 | OptimizedBvhNode tmp = leafNodes[i]; | ||
171 | leafNodes[i] = leafNodes[splitIndex]; | ||
172 | leafNodes[splitIndex] = tmp; | ||
173 | splitIndex++; | ||
174 | } | ||
175 | } | ||
176 | if ((splitIndex == startIndex) || (splitIndex == (endIndex - 1))) | ||
177 | { | ||
178 | splitIndex = startIndex + (numIndices >> 1); | ||
179 | } | ||
180 | return splitIndex; | ||
181 | } | ||
182 | |||
183 | public void WalkTree(OptimizedBvhNode rootNode, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) | ||
184 | { | ||
185 | bool isLeafNode, aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax); | ||
186 | if (aabbOverlap) | ||
187 | { | ||
188 | isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null); | ||
189 | if (isLeafNode) | ||
190 | { | ||
191 | nodeCallback.ProcessNode(rootNode); | ||
192 | } | ||
193 | else | ||
194 | { | ||
195 | WalkTree(rootNode.LeftChild, nodeCallback, aabbMin, aabbMax); | ||
196 | WalkTree(rootNode.RightChild, nodeCallback, aabbMin, aabbMax); | ||
197 | } | ||
198 | } | ||
199 | } | ||
200 | |||
201 | public void WalkStacklessTree(OptimizedBvhNode[] rootNodeArray, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) | ||
202 | { | ||
203 | int escapeIndex, curIndex = 0; | ||
204 | int walkIterations = 0; | ||
205 | bool aabbOverlap, isLeafNode; | ||
206 | int rootNodeIndex = 0; | ||
207 | OptimizedBvhNode rootNode = rootNodeArray[rootNodeIndex]; | ||
208 | |||
209 | while (curIndex < _curNodeIndex) | ||
210 | { | ||
211 | //catch bugs in tree data | ||
212 | if (walkIterations >= _curNodeIndex) | ||
213 | throw new BulletException(); | ||
214 | |||
215 | walkIterations++; | ||
216 | aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax); | ||
217 | isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null); | ||
218 | |||
219 | if (isLeafNode && aabbOverlap) | ||
220 | { | ||
221 | nodeCallback.ProcessNode(rootNode); | ||
222 | } | ||
223 | |||
224 | if (aabbOverlap || isLeafNode) | ||
225 | { | ||
226 | rootNodeIndex++; // this | ||
227 | curIndex++; | ||
228 | if (rootNodeIndex < rootNodeArray.Length) | ||
229 | rootNode = rootNodeArray[rootNodeIndex]; | ||
230 | } | ||
231 | else | ||
232 | { | ||
233 | escapeIndex = rootNode.EscapeIndex; | ||
234 | rootNodeIndex += escapeIndex; // and this | ||
235 | curIndex += escapeIndex; | ||
236 | if (rootNodeIndex < rootNodeArray.Length) | ||
237 | rootNode = rootNodeArray[rootNodeIndex]; | ||
238 | } | ||
239 | |||
240 | } | ||
241 | |||
242 | if (_maxIterations < walkIterations) | ||
243 | _maxIterations = walkIterations; | ||
244 | } | ||
245 | |||
246 | public void ReportAabbOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) | ||
247 | { | ||
248 | //either choose recursive traversal (walkTree) or stackless (walkStacklessTree) | ||
249 | //walkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax); | ||
250 | //WalkStacklessTree(_rootNode, nodeCallback, aabbMin, aabbMax); | ||
251 | WalkStacklessTree(_contiguousNodes, nodeCallback, aabbMin, aabbMax); | ||
252 | } | ||
253 | |||
254 | public void ReportSphereOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) { } | ||
255 | } | ||
256 | |||
257 | public class NodeTriangleCallback : ITriangleIndexCallback | ||
258 | { | ||
259 | private List<OptimizedBvhNode> _triangleNodes; | ||
260 | |||
261 | public NodeTriangleCallback(List<OptimizedBvhNode> triangleNodes) | ||
262 | { | ||
263 | _triangleNodes = triangleNodes; | ||
264 | } | ||
265 | |||
266 | public List<OptimizedBvhNode> TriangleNodes { get { return _triangleNodes; } set { _triangleNodes = value; } } | ||
267 | |||
268 | public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) | ||
269 | { | ||
270 | |||
271 | OptimizedBvhNode node = new OptimizedBvhNode(); | ||
272 | node.AabbMin = new Vector3(1e30f, 1e30f, 1e30f); | ||
273 | node.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f); | ||
274 | |||
275 | node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[0]); | ||
276 | node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[0]); | ||
277 | node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[1]); | ||
278 | node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[1]); | ||
279 | node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[2]); | ||
280 | node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[2]); | ||
281 | |||
282 | node.EscapeIndex = -1; | ||
283 | node.LeftChild = null; | ||
284 | node.RightChild = null; | ||
285 | |||
286 | //for child nodes | ||
287 | node.SubPart = partId; | ||
288 | node.TriangleIndex = triangleIndex; | ||
289 | |||
290 | _triangleNodes.Add(node); | ||
291 | } | ||
292 | } | ||
293 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs new file mode 100644 index 0000000..d43f2fb --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs | |||
@@ -0,0 +1,63 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// OptimizedBvhNode contains both internal and leaf node information. | ||
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) | ||
33 | /// and storing aabbmin/max as quantized integers. | ||
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 | ||
36 | /// </summary> | ||
37 | public class OptimizedBvhNode | ||
38 | { | ||
39 | private Vector3 _aabbMin; | ||
40 | private Vector3 _aabbMax; | ||
41 | |||
42 | //these 2 pointers are obsolete, the stackless traversal just uses the escape index | ||
43 | private OptimizedBvhNode _leftChild; | ||
44 | private OptimizedBvhNode _rightChild; | ||
45 | |||
46 | private int _escapeIndex; | ||
47 | |||
48 | //for child nodes | ||
49 | private int _subPart; | ||
50 | private int _triangleIndex; | ||
51 | |||
52 | public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } } | ||
53 | public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } } | ||
54 | |||
55 | public OptimizedBvhNode LeftChild { get { return _leftChild; } set { _leftChild = value; } } | ||
56 | public OptimizedBvhNode RightChild { get { return _rightChild; } set { _rightChild = value; } } | ||
57 | |||
58 | public int EscapeIndex { get { return _escapeIndex; } set { _escapeIndex = value; } } | ||
59 | |||
60 | public int SubPart { get { return _subPart; } set { _subPart = value; } } | ||
61 | public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } | ||
62 | } | ||
63 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs new file mode 100644 index 0000000..f61371a --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs | |||
@@ -0,0 +1,133 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public abstract class PolyhedralConvexShape : ConvexShape | ||
30 | { | ||
31 | public PolyhedralConvexShape() | ||
32 | { | ||
33 | //m_optionalHull = null; | ||
34 | } | ||
35 | |||
36 | public abstract int VertexCount { get; } | ||
37 | public abstract int EdgeCount { get; } | ||
38 | public abstract int PlaneCount { get; } | ||
39 | |||
40 | public abstract void GetEdge(int i, out Vector3 pointA, out Vector3 pointB); | ||
41 | public abstract void GetVertex(int i, out Vector3 vertex); | ||
42 | public abstract void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i); | ||
43 | // abstract int getIndex(int i); | ||
44 | |||
45 | public abstract bool IsInside(Vector3 point, float tolerance); | ||
46 | |||
47 | // optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp | ||
48 | //public class Hull m_optionalHull; | ||
49 | |||
50 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
51 | { | ||
52 | Vector3 supVec = new Vector3(); | ||
53 | |||
54 | float maxDot = -1e30f; | ||
55 | |||
56 | float lenSqr = vec.LengthSquared(); | ||
57 | if (lenSqr < 0.0001f) | ||
58 | { | ||
59 | vec = new Vector3(1, 0, 0); | ||
60 | } | ||
61 | else | ||
62 | { | ||
63 | float rlen = 1f / (float)Math.Sqrt(lenSqr); | ||
64 | vec *= rlen; | ||
65 | } | ||
66 | |||
67 | Vector3 vtx; | ||
68 | float newDot; | ||
69 | |||
70 | for (int i = 0; i < VertexCount; i++) | ||
71 | { | ||
72 | GetVertex(i, out vtx); | ||
73 | newDot = Vector3.Dot(vec, vtx); | ||
74 | if (newDot > maxDot) | ||
75 | { | ||
76 | maxDot = newDot; | ||
77 | supVec = vtx; | ||
78 | } | ||
79 | } | ||
80 | return supVec; | ||
81 | } | ||
82 | |||
83 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
84 | { | ||
85 | #warning Think about this | ||
86 | /*Vector3 vtx; | ||
87 | float newDot; | ||
88 | |||
89 | for (int i = 0; i < vectors.Length; i++) | ||
90 | { | ||
91 | supportVerticesOut[i][3] = -1e30f; | ||
92 | } | ||
93 | |||
94 | for (int j = 0; j < vectors.Length; j++) | ||
95 | { | ||
96 | Vector3 vec = vectors[j]; | ||
97 | |||
98 | for (int i = 0; i < getNumVertices(); i++) | ||
99 | { | ||
100 | getVertex(i, out vtx); | ||
101 | newDot = Vector3.Dot(vec,vtx); | ||
102 | if (newDot > supportVerticesOut[j][3]) | ||
103 | { | ||
104 | //WARNING: don't swap next lines, the w component would get overwritten! | ||
105 | supportVerticesOut[j] = vtx; | ||
106 | supportVerticesOut[j][3] = newDot; | ||
107 | } | ||
108 | } | ||
109 | }*/ | ||
110 | } | ||
111 | |||
112 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
113 | { | ||
114 | //not yet, return box inertia | ||
115 | float margin = Margin; | ||
116 | |||
117 | Matrix ident = Matrix.Identity; | ||
118 | Vector3 aabbMin, aabbMax; | ||
119 | GetAabb(ident, out aabbMin, out aabbMax); | ||
120 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; | ||
121 | |||
122 | float lx = 2f * (halfExtents.X + margin); | ||
123 | float ly = 2f * (halfExtents.Y + margin); | ||
124 | float lz = 2f * (halfExtents.Z + margin); | ||
125 | float x2 = lx * lx; | ||
126 | float y2 = ly * ly; | ||
127 | float z2 = lz * lz; | ||
128 | float scaledmass = mass * 0.08333333f; | ||
129 | |||
130 | inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2)); | ||
131 | } | ||
132 | } | ||
133 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs new file mode 100644 index 0000000..c660a6f --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs | |||
@@ -0,0 +1,116 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// btSphereShape implements an implicit (getSupportingVertex) Sphere | ||
31 | /// </summary> | ||
32 | public class SphereShape : ConvexShape | ||
33 | { | ||
34 | public SphereShape(float radius) | ||
35 | : base() | ||
36 | { | ||
37 | Vector3 temp = ImplicitShapeDimensions; | ||
38 | temp.X = radius; | ||
39 | ImplicitShapeDimensions = temp; | ||
40 | } | ||
41 | |||
42 | public float Radius { get { return ImplicitShapeDimensions.X; } } | ||
43 | |||
44 | public override BroadphaseNativeTypes ShapeType | ||
45 | { | ||
46 | get | ||
47 | { | ||
48 | return BroadphaseNativeTypes.Sphere; | ||
49 | } | ||
50 | } | ||
51 | |||
52 | public override string Name | ||
53 | { | ||
54 | get | ||
55 | { | ||
56 | return "Sphere"; | ||
57 | } | ||
58 | } | ||
59 | |||
60 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
61 | { | ||
62 | return new Vector3(); | ||
63 | } | ||
64 | |||
65 | /// <summary> | ||
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 | ||
68 | /// </summary> | ||
69 | public override float Margin | ||
70 | { | ||
71 | get | ||
72 | { | ||
73 | return LocalScaling.X * Radius + base.Margin; | ||
74 | } | ||
75 | set | ||
76 | { | ||
77 | base.Margin = value; | ||
78 | } | ||
79 | } | ||
80 | |||
81 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
82 | { | ||
83 | if (supportVerticesOut != null) | ||
84 | for (int i = 0; i < supportVerticesOut.Length; i++) | ||
85 | supportVerticesOut[i] = new Vector3(); | ||
86 | } | ||
87 | |||
88 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
89 | { | ||
90 | float elem = 0.4f * mass * Margin * Margin; | ||
91 | inertia = new Vector3(elem, elem, elem); | ||
92 | } | ||
93 | |||
94 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | ||
95 | { | ||
96 | Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); | ||
97 | |||
98 | Vector3 vecnorm = vec; | ||
99 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | ||
100 | { | ||
101 | vecnorm = new Vector3(-1f, -1f, -1f); | ||
102 | } | ||
103 | vecnorm.Normalize(); | ||
104 | supVertex += Margin * vecnorm; | ||
105 | return supVertex; | ||
106 | } | ||
107 | |||
108 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
109 | { | ||
110 | Vector3 center = t.Translation; | ||
111 | Vector3 extent = new Vector3(Margin, Margin, Margin); | ||
112 | aabbMin = center - extent; | ||
113 | aabbMax = center + extent; | ||
114 | } | ||
115 | } | ||
116 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs new file mode 100644 index 0000000..49604a0 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs | |||
@@ -0,0 +1,124 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class StaticPlaneShape : ConcaveShape | ||
30 | { | ||
31 | private Vector3 _localAabbMin; | ||
32 | private Vector3 _localAabbMax; | ||
33 | |||
34 | private Vector3 _planeNormal; | ||
35 | private float _planeConstant; | ||
36 | private Vector3 _localScaling; | ||
37 | |||
38 | public StaticPlaneShape(Vector3 planeNormal, float planeConstant) | ||
39 | { | ||
40 | _planeNormal = planeNormal; | ||
41 | _planeConstant = planeConstant; | ||
42 | _localScaling = new Vector3(); | ||
43 | } | ||
44 | |||
45 | protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } } | ||
46 | protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } } | ||
47 | |||
48 | protected Vector3 PlaneNormal { get { return _planeNormal; } set { _planeNormal = value; } } | ||
49 | protected float PlaneConstant { get { return _planeConstant; } set { _planeConstant = value; } } | ||
50 | |||
51 | public override BroadphaseNativeTypes ShapeType | ||
52 | { | ||
53 | get | ||
54 | { | ||
55 | return BroadphaseNativeTypes.StaticPlane; | ||
56 | } | ||
57 | } | ||
58 | |||
59 | public override Vector3 LocalScaling | ||
60 | { | ||
61 | get | ||
62 | { | ||
63 | return _localScaling; | ||
64 | } | ||
65 | set | ||
66 | { | ||
67 | _localScaling = value; | ||
68 | } | ||
69 | } | ||
70 | |||
71 | public override string Name | ||
72 | { | ||
73 | get | ||
74 | { | ||
75 | return "StaticPlane"; | ||
76 | } | ||
77 | } | ||
78 | |||
79 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
80 | { | ||
81 | Vector3 infvec = new Vector3(1e30f, 1e30f, 1e30f); | ||
82 | |||
83 | Vector3 center = _planeNormal * _planeConstant; | ||
84 | aabbMin = center + infvec * _planeNormal; | ||
85 | aabbMax = aabbMin; | ||
86 | MathHelper.SetMin(ref aabbMin, center - infvec * _planeNormal); | ||
87 | MathHelper.SetMax(ref aabbMax, center - infvec * _planeNormal); | ||
88 | |||
89 | aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); | ||
90 | aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | ||
91 | } | ||
92 | |||
93 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
94 | { | ||
95 | //moving concave objects not supported | ||
96 | inertia = new Vector3(); | ||
97 | } | ||
98 | |||
99 | public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) { | ||
100 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; | ||
101 | float radius = halfExtents.Length(); | ||
102 | Vector3 center = (aabbMax + aabbMin) * 0.5f; | ||
103 | |||
104 | //this is where the triangles are generated, given AABB and plane equation (normal/constant) | ||
105 | Vector3 tangentDir0 = new Vector3(), tangentDir1 = new Vector3(); | ||
106 | |||
107 | //tangentDir0/tangentDir1 can be precalculated | ||
108 | MathHelper.PlaneSpace1(_planeNormal, ref tangentDir0, ref tangentDir1); | ||
109 | |||
110 | Vector3 projectedCenter = center - (Vector3.Dot(_planeNormal, center) - _planeConstant) * _planeNormal; | ||
111 | |||
112 | Vector3[] triangle = new Vector3[3]; | ||
113 | triangle[0] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; | ||
114 | triangle[1] = projectedCenter + tangentDir0 * radius - tangentDir1 * radius; | ||
115 | triangle[2] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; | ||
116 | callback.ProcessTriangle(triangle, 0, 0); | ||
117 | |||
118 | triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; | ||
119 | triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius; | ||
120 | triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; | ||
121 | callback.ProcessTriangle(triangle, 0, 1); | ||
122 | } | ||
123 | } | ||
124 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs new file mode 100644 index 0000000..2a2ce27 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs | |||
@@ -0,0 +1,115 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// PHY_ScalarType enumerates possible scalar types. | ||
31 | /// See the StridingMeshInterface for its use | ||
32 | /// </summary> | ||
33 | public enum PHY_ScalarType | ||
34 | { | ||
35 | PHY_FLOAT, | ||
36 | PHY_DOUBLE, | ||
37 | PHY_INTEGER, | ||
38 | PHY_SHORT, | ||
39 | PHY_FIXEDPOINT88 | ||
40 | } | ||
41 | |||
42 | /// <summary> | ||
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. | ||
45 | /// </summary> | ||
46 | public abstract class StridingMeshInterface | ||
47 | { | ||
48 | protected Vector3 _scaling; | ||
49 | |||
50 | public StridingMeshInterface() | ||
51 | { | ||
52 | _scaling = new Vector3(1f,1f,1f); | ||
53 | } | ||
54 | |||
55 | public void InternalProcessAllTriangles(ITriangleIndexCallback callback, Vector3 aabbMin, Vector3 aabbMax) | ||
56 | { | ||
57 | int numtotalphysicsverts = 0; | ||
58 | int numtriangles, gfxindex; | ||
59 | int part, graphicssubparts = SubPartsCount(); | ||
60 | Vector3[] triangle = new Vector3[3]; | ||
61 | List<Vector3> verts; | ||
62 | List<int> indicies; | ||
63 | |||
64 | Vector3 meshScaling = Scaling; | ||
65 | |||
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++) | ||
68 | { | ||
69 | GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, part); | ||
70 | numtotalphysicsverts += numtriangles * 3; //upper bound | ||
71 | |||
72 | for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) | ||
73 | { | ||
74 | triangle[0] = verts[indicies[gfxindex * 3 + 0]]; | ||
75 | triangle[1] = verts[indicies[gfxindex * 3 + 1]]; | ||
76 | triangle[2] = verts[indicies[gfxindex * 3 + 2]]; | ||
77 | |||
78 | callback.ProcessTriangleIndex(triangle, part, gfxindex); | ||
79 | } | ||
80 | |||
81 | UnLockReadOnlyVertexBase(part); | ||
82 | } | ||
83 | } | ||
84 | |||
85 | |||
86 | // get read and write access to a subpart of a triangle mesh | ||
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 | ||
89 | // very similar to OpenGL vertexarray support | ||
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); | ||
92 | |||
93 | public abstract void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart); | ||
94 | |||
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 | ||
97 | public abstract void UnLockVertexBase(int subpart); | ||
98 | |||
99 | public abstract void UnLockReadOnlyVertexBase(int subpart); | ||
100 | |||
101 | |||
102 | // getNumSubParts returns the number of seperate subparts | ||
103 | // each subpart has a continuous array of vertices and indices | ||
104 | public abstract int SubPartsCount(); | ||
105 | |||
106 | public abstract void PreallocateVertices(int numverts); | ||
107 | public abstract void PreallocateIndices(int numindices); | ||
108 | |||
109 | public Vector3 Scaling | ||
110 | { | ||
111 | get { return _scaling; } | ||
112 | set { _scaling = value; } | ||
113 | } | ||
114 | } | ||
115 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs new file mode 100644 index 0000000..ffb5813 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs | |||
@@ -0,0 +1,67 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | internal class SupportVertexCallback : ITriangleCallback | ||
30 | { | ||
31 | private Vector3 _supportVertexLocal; | ||
32 | |||
33 | private Matrix _worldTransform; | ||
34 | private float _maxDot; | ||
35 | private Vector3 _supportVecLocal; | ||
36 | |||
37 | public SupportVertexCallback(Vector3 supportVecWorld, Matrix trans) | ||
38 | { | ||
39 | _supportVertexLocal = new Vector3(); | ||
40 | _worldTransform = trans; | ||
41 | _maxDot = -1e30f; | ||
42 | _supportVecLocal = Vector3.TransformNormal(supportVecWorld, _worldTransform); | ||
43 | } | ||
44 | |||
45 | public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } } | ||
46 | public float MaxDot { get { return _maxDot; } set { _maxDot = value; } } | ||
47 | public Vector3 SupportVectorLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } } | ||
48 | |||
49 | public Vector3 SupportVertexLocal { get { return _supportVertexLocal; } } | ||
50 | public Vector3 SupportVertexWorldSpace { get { return MathHelper.MatrixToVector(_worldTransform, _supportVertexLocal); } } | ||
51 | |||
52 | #region ITriangleCallback Members | ||
53 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) | ||
54 | { | ||
55 | for (int i = 0; i < 3; i++) | ||
56 | { | ||
57 | float dot = Vector3.Dot(_supportVecLocal, triangle[i]); | ||
58 | if (dot > _maxDot) | ||
59 | { | ||
60 | _maxDot = dot; | ||
61 | _supportVertexLocal = triangle[i]; | ||
62 | } | ||
63 | } | ||
64 | } | ||
65 | #endregion | ||
66 | } | ||
67 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs new file mode 100644 index 0000000..158e42a --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs | |||
@@ -0,0 +1,80 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class Triangle | ||
30 | { | ||
31 | private Vector3 _vertexA; | ||
32 | private Vector3 _vertexB; | ||
33 | private Vector3 _vertexC; | ||
34 | private int _partId; | ||
35 | private int _triangleIndex; | ||
36 | |||
37 | public Vector3 VertexA { get { return _vertexA; } set { _vertexA = value; } } | ||
38 | public Vector3 VertexB { get { return _vertexB; } set { _vertexB = value; } } | ||
39 | public Vector3 VertexC { get { return _vertexC; } set { _vertexC = value; } } | ||
40 | public int PartId { get { return _partId; } set { _partId = value; } } | ||
41 | public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } | ||
42 | } | ||
43 | |||
44 | /// <summary> | ||
45 | /// example usage of this class: | ||
46 | /// TriangleBuffer triBuf; | ||
47 | /// concaveShape.processAllTriangles(triBuf, out aabbMin, out aabbMax); | ||
48 | /// for (int i = 0; i < triBuf.getNumTriangles(); i++) | ||
49 | /// { | ||
50 | /// Triangle tri = triBuf.getTriangle(i); | ||
51 | /// //do something useful here with the triangle | ||
52 | /// } | ||
53 | /// </summary> | ||
54 | public class TriangleBuffer : ITriangleCallback | ||
55 | { | ||
56 | private List<Triangle> _triangleBuffer = new List<Triangle>(); | ||
57 | |||
58 | public int TriangleCount { get { return _triangleBuffer.Count; } } | ||
59 | public Triangle this[int index] { get { return _triangleBuffer[index]; } } | ||
60 | |||
61 | public void ClearBuffer() | ||
62 | { | ||
63 | _triangleBuffer.Clear(); | ||
64 | } | ||
65 | |||
66 | #region ITriangleCallback Members | ||
67 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) | ||
68 | { | ||
69 | Triangle tri = new Triangle(); | ||
70 | tri.VertexA = triangle[0]; | ||
71 | tri.VertexB = triangle[1]; | ||
72 | tri.VertexC = triangle[2]; | ||
73 | tri.PartId = partID; | ||
74 | tri.TriangleIndex = triangleIndex; | ||
75 | |||
76 | _triangleBuffer.Add(tri); | ||
77 | } | ||
78 | #endregion | ||
79 | } | ||
80 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs new file mode 100644 index 0000000..ac892c2 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs | |||
@@ -0,0 +1,33 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public interface ITriangleCallback | ||
30 | { | ||
31 | void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex); | ||
32 | } | ||
33 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs new file mode 100644 index 0000000..6d0d7b5 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs | |||
@@ -0,0 +1,136 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// 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 | ||
32 | /// </summary> | ||
33 | public struct IndexedMesh | ||
34 | { | ||
35 | private int _numTriangles; | ||
36 | private int[] _triangleIndexBase; | ||
37 | private int _triangleIndexStride; | ||
38 | private int _numVertices; | ||
39 | private Vector3[] _vertexBase; | ||
40 | private int _vertexStride; | ||
41 | |||
42 | public IndexedMesh(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride) | ||
43 | { | ||
44 | _numTriangles = numTriangleIndices; | ||
45 | _triangleIndexBase = triangleIndexBase; | ||
46 | _triangleIndexStride = triangleIndexStride; | ||
47 | _vertexBase = vertexBase; | ||
48 | _numVertices = numVertices; | ||
49 | _vertexStride = vertexStride; | ||
50 | } | ||
51 | |||
52 | public IndexedMesh(int[] triangleIndexBase, Vector3[] vertexBase) | ||
53 | { | ||
54 | _numTriangles = triangleIndexBase.Length; | ||
55 | _triangleIndexBase = triangleIndexBase; | ||
56 | _triangleIndexStride = 32; | ||
57 | _vertexBase = vertexBase; | ||
58 | _numVertices = vertexBase.Length; | ||
59 | _vertexStride = 24; | ||
60 | } | ||
61 | |||
62 | public int TriangleCount { get { return _numTriangles; } set { _numTriangles = value; } } | ||
63 | public int[] TriangleIndexBase { get { return _triangleIndexBase; } set { _triangleIndexBase = value; } } | ||
64 | public int TriangleIndexStride { get { return _triangleIndexStride; } set { _triangleIndexStride = value; } } | ||
65 | public int VertexCount { get { return _numVertices; } set { _numVertices = value; } } | ||
66 | public Vector3[] VertexBase { get { return _vertexBase; } set { _vertexBase = value; } } | ||
67 | public int VertexStride { get { return _vertexStride; } set { _vertexStride = value; } } | ||
68 | } | ||
69 | |||
70 | /// <summary> | ||
71 | /// TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays. | ||
72 | /// Additional meshes can be added using addIndexedMesh | ||
73 | /// </summary> | ||
74 | public class TriangleIndexVertexArray : StridingMeshInterface | ||
75 | { | ||
76 | List<IndexedMesh> _indexedMeshes = new List<IndexedMesh>(); | ||
77 | |||
78 | public TriangleIndexVertexArray() { } | ||
79 | |||
80 | public TriangleIndexVertexArray(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride) | ||
81 | { | ||
82 | IndexedMesh mesh = new IndexedMesh(); | ||
83 | mesh.TriangleCount = numTriangleIndices; | ||
84 | mesh.TriangleIndexBase = triangleIndexBase; | ||
85 | mesh.TriangleIndexStride = triangleIndexStride; | ||
86 | mesh.VertexBase = vertexBase; | ||
87 | mesh.VertexCount = numVertices; | ||
88 | mesh.VertexStride = vertexStride; | ||
89 | |||
90 | AddIndexedMesh(mesh); | ||
91 | } | ||
92 | |||
93 | public TriangleIndexVertexArray(int[] triangleIndexBase, Vector3[] vertexBase) | ||
94 | : this(triangleIndexBase.Length, triangleIndexBase, 32, vertexBase.Length, vertexBase, 24) { } | ||
95 | |||
96 | public void AddIndexedMesh(IndexedMesh indexedMesh) | ||
97 | { | ||
98 | _indexedMeshes.Add(indexedMesh); | ||
99 | } | ||
100 | |||
101 | public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) | ||
102 | { | ||
103 | throw new Exception("The method or operation is not implemented."); | ||
104 | } | ||
105 | |||
106 | public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) | ||
107 | { | ||
108 | throw new Exception("The method or operation is not implemented."); | ||
109 | } | ||
110 | |||
111 | public override void UnLockVertexBase(int subpart) | ||
112 | { | ||
113 | throw new Exception("The method or operation is not implemented."); | ||
114 | } | ||
115 | |||
116 | public override void UnLockReadOnlyVertexBase(int subpart) | ||
117 | { | ||
118 | throw new Exception("The method or operation is not implemented."); | ||
119 | } | ||
120 | |||
121 | public override int SubPartsCount() | ||
122 | { | ||
123 | return _indexedMeshes.Count; | ||
124 | } | ||
125 | |||
126 | public override void PreallocateVertices(int numverts) | ||
127 | { | ||
128 | throw new Exception("The method or operation is not implemented."); | ||
129 | } | ||
130 | |||
131 | public override void PreallocateIndices(int numindices) | ||
132 | { | ||
133 | throw new Exception("The method or operation is not implemented."); | ||
134 | } | ||
135 | } | ||
136 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs new file mode 100644 index 0000000..1426355 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs | |||
@@ -0,0 +1,102 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | using MonoXnaCompactMaths; | ||
27 | |||
28 | namespace XnaDevRu.BulletX | ||
29 | { | ||
30 | class TriangleMesh : StridingMeshInterface | ||
31 | { | ||
32 | int _numTriangles; | ||
33 | List<Vector3> _verts; | ||
34 | |||
35 | public TriangleMesh() | ||
36 | { | ||
37 | _numTriangles = 0; | ||
38 | _verts = new List<Vector3>(); | ||
39 | } | ||
40 | |||
41 | void AddTriangle(Vector3 vertex0, Vector3 vertex1, Vector3 vertex2) | ||
42 | { | ||
43 | _verts.Add(vertex0); | ||
44 | _verts.Add(vertex1); | ||
45 | _verts.Add(vertex2); | ||
46 | _numTriangles++; | ||
47 | } | ||
48 | |||
49 | public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) | ||
50 | { | ||
51 | verts = new List<Vector3>(); | ||
52 | for (int i = 0; i < 3; i++) | ||
53 | { | ||
54 | verts.Add(_verts[subpart * 3 + i]); | ||
55 | } | ||
56 | indicies = new List<int>(); | ||
57 | indicies.Add(0); | ||
58 | indicies.Add(1); | ||
59 | indicies.Add(2); | ||
60 | numfaces = 1; | ||
61 | } | ||
62 | |||
63 | public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) | ||
64 | { | ||
65 | verts = new List<Vector3>(); | ||
66 | for (int i = 0; i < 3; i++) | ||
67 | { | ||
68 | verts.Add(_verts[subpart * 3 + i]); | ||
69 | } | ||
70 | indicies = new List<int>(); | ||
71 | indicies.Add(0); | ||
72 | indicies.Add(1); | ||
73 | indicies.Add(2); | ||
74 | numfaces = 1; | ||
75 | } | ||
76 | |||
77 | public override void UnLockVertexBase(int subpart) | ||
78 | { | ||
79 | |||
80 | } | ||
81 | |||
82 | public override void UnLockReadOnlyVertexBase(int subpart) | ||
83 | { | ||
84 | |||
85 | } | ||
86 | |||
87 | public override int SubPartsCount() | ||
88 | { | ||
89 | return _numTriangles; | ||
90 | } | ||
91 | |||
92 | public override void PreallocateVertices(int numverts) | ||
93 | { | ||
94 | |||
95 | } | ||
96 | |||
97 | public override void PreallocateIndices(int numindices) | ||
98 | { | ||
99 | |||
100 | } | ||
101 | } | ||
102 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs new file mode 100644 index 0000000..bc8f09e --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs | |||
@@ -0,0 +1,160 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles. | ||
31 | /// </summary> | ||
32 | public class TriangleMeshShape : ConcaveShape | ||
33 | { | ||
34 | private StridingMeshInterface _meshInterface; | ||
35 | private Vector3 _localAabbMin; | ||
36 | private Vector3 _localAabbMax; | ||
37 | |||
38 | public TriangleMeshShape(StridingMeshInterface meshInterface) | ||
39 | { | ||
40 | this._meshInterface = meshInterface; | ||
41 | RecalcLocalAabb(); | ||
42 | } | ||
43 | |||
44 | protected StridingMeshInterface MeshInterface { get { return _meshInterface; } set { _meshInterface = value; } } | ||
45 | protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } } | ||
46 | protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } } | ||
47 | |||
48 | public override BroadphaseNativeTypes ShapeType | ||
49 | { | ||
50 | get | ||
51 | { | ||
52 | return BroadphaseNativeTypes.TriangleMesh; | ||
53 | } | ||
54 | } | ||
55 | |||
56 | public override Vector3 LocalScaling | ||
57 | { | ||
58 | get | ||
59 | { | ||
60 | return _meshInterface.Scaling; | ||
61 | } | ||
62 | set | ||
63 | { | ||
64 | _meshInterface.Scaling = value; | ||
65 | } | ||
66 | } | ||
67 | |||
68 | public override string Name | ||
69 | { | ||
70 | get | ||
71 | { | ||
72 | return "TriangleMesh"; | ||
73 | } | ||
74 | } | ||
75 | |||
76 | public void RecalcLocalAabb() | ||
77 | { | ||
78 | { | ||
79 | Vector3 vec = new Vector3(); | ||
80 | vec.X = 1f; | ||
81 | Vector3 tmp = LocalGetSupportingVertex(vec); | ||
82 | _localAabbMax.X = tmp.X + CollisionMargin; | ||
83 | vec.X = -1f; | ||
84 | tmp = LocalGetSupportingVertex(vec); | ||
85 | _localAabbMin.X = tmp.X - CollisionMargin; | ||
86 | } | ||
87 | { | ||
88 | Vector3 vec = new Vector3(); | ||
89 | vec.Y = 1f; | ||
90 | Vector3 tmp = LocalGetSupportingVertex(vec); | ||
91 | _localAabbMax.Y = tmp.Y + CollisionMargin; | ||
92 | vec.Y = -1f; | ||
93 | tmp = LocalGetSupportingVertex(vec); | ||
94 | _localAabbMin.Y = tmp.Y - CollisionMargin; | ||
95 | } | ||
96 | { | ||
97 | Vector3 vec = new Vector3(); | ||
98 | vec.Z = 1f; | ||
99 | Vector3 tmp = LocalGetSupportingVertex(vec); | ||
100 | _localAabbMax.Z = tmp.Z + CollisionMargin; | ||
101 | vec.Z = -1f; | ||
102 | tmp = LocalGetSupportingVertex(vec); | ||
103 | _localAabbMin.Z = tmp.Z - CollisionMargin; | ||
104 | } | ||
105 | } | ||
106 | |||
107 | public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) | ||
108 | { | ||
109 | LocalProcessAllTriangles(callback, aabbMax, aabbMax); | ||
110 | } | ||
111 | |||
112 | protected void LocalProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) | ||
113 | { | ||
114 | FilteredCallback filterCallback = new FilteredCallback(callback, aabbMin, aabbMax); | ||
115 | _meshInterface.InternalProcessAllTriangles(filterCallback, aabbMin, aabbMax); | ||
116 | } | ||
117 | |||
118 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
119 | { | ||
120 | Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin); | ||
121 | Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin); | ||
122 | |||
123 | Matrix abs_b = MathHelper.Absolute(t); | ||
124 | |||
125 | Vector3 center = MathHelper.MatrixToVector(t, localCenter); | ||
126 | |||
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), | ||
129 | Vector3.Dot(new Vector3(abs_b.M31, abs_b.M32, abs_b.M33), localHalfExtents)); | ||
130 | extent += new Vector3(Margin, Margin, Margin); | ||
131 | |||
132 | aabbMin = center - extent; | ||
133 | aabbMax = center + extent; | ||
134 | } | ||
135 | |||
136 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
137 | { | ||
138 | inertia = new Vector3(); | ||
139 | //moving concave objects not supported | ||
140 | BulletDebug.Assert(false); | ||
141 | } | ||
142 | |||
143 | public virtual Vector3 LocalGetSupportingVertex(Vector3 vec) | ||
144 | { | ||
145 | Vector3 supportVertex; | ||
146 | Matrix ident = Matrix.Identity; | ||
147 | SupportVertexCallback supportCallback = new SupportVertexCallback(vec, ident); | ||
148 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | ||
149 | LocalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); | ||
150 | supportVertex = supportCallback.SupportVertexLocal; | ||
151 | return supportVertex; | ||
152 | } | ||
153 | |||
154 | public virtual Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
155 | { | ||
156 | BulletDebug.Assert(false); | ||
157 | return LocalGetSupportingVertex(vec); | ||
158 | } | ||
159 | } | ||
160 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs new file mode 100644 index 0000000..59ffad0 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs | |||
@@ -0,0 +1,187 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class TriangleShape : PolyhedralConvexShape | ||
30 | { | ||
31 | private Vector3[] _vertices = new Vector3[3]; | ||
32 | |||
33 | public TriangleShape(Vector3 pointA, Vector3 pointB, Vector3 pointC) | ||
34 | { | ||
35 | _vertices[0] = pointA; | ||
36 | _vertices[1] = pointB; | ||
37 | _vertices[2] = pointC; | ||
38 | } | ||
39 | |||
40 | public override int PreferredPenetrationDirectionsCount | ||
41 | { | ||
42 | get | ||
43 | { | ||
44 | return 2; | ||
45 | } | ||
46 | } | ||
47 | |||
48 | public Vector3[] Vertices | ||
49 | { | ||
50 | get | ||
51 | { | ||
52 | return _vertices; | ||
53 | } | ||
54 | } | ||
55 | |||
56 | public override int VertexCount | ||
57 | { | ||
58 | get | ||
59 | { | ||
60 | return 3; | ||
61 | } | ||
62 | } | ||
63 | |||
64 | public override int EdgeCount | ||
65 | { | ||
66 | get | ||
67 | { | ||
68 | return 3; | ||
69 | } | ||
70 | } | ||
71 | |||
72 | public override int PlaneCount | ||
73 | { | ||
74 | get | ||
75 | { | ||
76 | return 1; | ||
77 | } | ||
78 | } | ||
79 | |||
80 | public override BroadphaseNativeTypes ShapeType | ||
81 | { | ||
82 | get | ||
83 | { | ||
84 | return BroadphaseNativeTypes.Triangle; | ||
85 | } | ||
86 | } | ||
87 | |||
88 | public override string Name | ||
89 | { | ||
90 | get | ||
91 | { | ||
92 | return "Triangle"; | ||
93 | } | ||
94 | } | ||
95 | |||
96 | public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) | ||
97 | { | ||
98 | CalculateNormal(out penetrationVector); | ||
99 | if (index != 0) | ||
100 | penetrationVector *= -1f; | ||
101 | } | ||
102 | |||
103 | public virtual void GetPlaneEquation(int i, out Vector3 planeNormal, out Vector3 planeSupport) | ||
104 | { | ||
105 | CalculateNormal(out planeNormal); | ||
106 | planeSupport = _vertices[0]; | ||
107 | } | ||
108 | |||
109 | public void CalculateNormal(out Vector3 normal) | ||
110 | { | ||
111 | normal = Vector3.Normalize(Vector3.Cross(_vertices[1] - _vertices[0], _vertices[2] - _vertices[0])); | ||
112 | } | ||
113 | |||
114 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
115 | { | ||
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)]; | ||
118 | } | ||
119 | |||
120 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
121 | { | ||
122 | for (int i = 0; i < vectors.Length; i++) | ||
123 | { | ||
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])); | ||
126 | supportVerticesOut[i] = _vertices[MathHelper.MaxAxis(dots)]; | ||
127 | } | ||
128 | } | ||
129 | |||
130 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
131 | { | ||
132 | inertia = new Vector3(); | ||
133 | BulletDebug.Assert(false); | ||
134 | } | ||
135 | |||
136 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) | ||
137 | { | ||
138 | GetVertex(i, out pa); | ||
139 | GetVertex((i + 1) % 3, out pb); | ||
140 | } | ||
141 | |||
142 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
143 | { | ||
144 | GetAabbSlow(t, out aabbMin, out aabbMax); | ||
145 | } | ||
146 | |||
147 | public override void GetVertex(int i, out Vector3 vtx) | ||
148 | { | ||
149 | vtx = _vertices[i]; | ||
150 | } | ||
151 | |||
152 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) | ||
153 | { | ||
154 | GetPlaneEquation(i, out planeNormal, out planeSupport); | ||
155 | } | ||
156 | |||
157 | public override bool IsInside(Vector3 pt, float tolerance) | ||
158 | { | ||
159 | Vector3 normal; | ||
160 | CalculateNormal(out normal); | ||
161 | //distance to plane | ||
162 | float dist = Vector3.Dot(pt, normal); | ||
163 | float planeconst = Vector3.Dot(_vertices[0], normal); | ||
164 | dist -= planeconst; | ||
165 | if (dist >= -tolerance && dist <= tolerance) | ||
166 | { | ||
167 | //inside check on edge-planes | ||
168 | int i; | ||
169 | for (i = 0; i < 3; i++) | ||
170 | { | ||
171 | Vector3 pa, pb; | ||
172 | GetEdge(i, out pa, out pb); | ||
173 | Vector3 edge = pb - pa; | ||
174 | Vector3 edgeNormal = Vector3.Cross(edge, normal); | ||
175 | edgeNormal = Vector3.Normalize(edgeNormal); | ||
176 | float distance = Vector3.Dot(pt, edgeNormal); | ||
177 | float edgeConst = Vector3.Dot(pa, edgeNormal); | ||
178 | distance -= edgeConst; | ||
179 | if (distance < -tolerance) | ||
180 | return false; | ||
181 | } | ||
182 | return true; | ||
183 | } | ||
184 | return false; | ||
185 | } | ||
186 | } | ||
187 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs new file mode 100644 index 0000000..497f220 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs | |||
@@ -0,0 +1,199 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// ContinuousConvexCollision implements angular and linear time of impact for convex objects. | ||
31 | /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis). | ||
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 | ||
34 | /// </summary> | ||
35 | public class ContinuousConvexCollision : IConvexCast | ||
36 | { | ||
37 | /// <summary> | ||
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. | ||
40 | /// </summary> | ||
41 | private const int MaxIterations = 1000; | ||
42 | |||
43 | private ISimplexSolver _simplexSolver; | ||
44 | private IConvexPenetrationDepthSolver _penetrationDepthSolver; | ||
45 | private ConvexShape _convexA; | ||
46 | private ConvexShape _convexB; | ||
47 | |||
48 | public ContinuousConvexCollision(ConvexShape convexA, ConvexShape convexB, | ||
49 | ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) | ||
50 | { | ||
51 | _simplexSolver = simplexSolver; | ||
52 | _penetrationDepthSolver = penetrationDepthSolver; | ||
53 | _convexA = convexA; | ||
54 | _convexB = convexB; | ||
55 | } | ||
56 | |||
57 | public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) | ||
58 | { | ||
59 | _simplexSolver.Reset(); | ||
60 | |||
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(); | ||
63 | TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA); | ||
64 | TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB); | ||
65 | |||
66 | float boundingRadiusA = _convexA.GetAngularMotionDisc(); | ||
67 | float boundingRadiusB = _convexB.GetAngularMotionDisc(); | ||
68 | |||
69 | float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA + | ||
70 | angVelB.Length() * boundingRadiusB; | ||
71 | |||
72 | float radius = 0.001f; | ||
73 | |||
74 | float lambda = 0f; | ||
75 | Vector3 v = new Vector3(1f, 0f, 0f); | ||
76 | |||
77 | int maxIter = MaxIterations; | ||
78 | |||
79 | Vector3 n = new Vector3(); | ||
80 | bool hasResult = false; | ||
81 | Vector3 c; | ||
82 | |||
83 | float lastLambda = lambda; | ||
84 | //float epsilon = 0.001f; | ||
85 | |||
86 | int numIter = 0; | ||
87 | //first solution, using GJK | ||
88 | |||
89 | |||
90 | Matrix identityTrans = Matrix.Identity; | ||
91 | |||
92 | SphereShape raySphere = new SphereShape(0f); | ||
93 | raySphere.Margin=0f; | ||
94 | |||
95 | |||
96 | //result.drawCoordSystem(sphereTr); | ||
97 | |||
98 | PointCollector pointCollector1 = new PointCollector(); | ||
99 | |||
100 | GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver); | ||
101 | GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | ||
102 | |||
103 | //we don't use margins during CCD | ||
104 | gjk.setIgnoreMargin(true); | ||
105 | |||
106 | input.TransformA = fromA; | ||
107 | input.TransformB = fromB; | ||
108 | |||
109 | DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1; | ||
110 | gjk.GetClosestPoints(input, r, null); | ||
111 | |||
112 | hasResult = pointCollector1.HasResult; | ||
113 | c = pointCollector1.PointInWorld; | ||
114 | |||
115 | if (hasResult) | ||
116 | { | ||
117 | float dist; | ||
118 | dist = pointCollector1.Distance; | ||
119 | n = pointCollector1.NormalOnBInWorld; | ||
120 | |||
121 | //not close enough | ||
122 | while (dist > radius) | ||
123 | { | ||
124 | numIter++; | ||
125 | if (numIter > maxIter) | ||
126 | return false; //todo: report a failure | ||
127 | |||
128 | float dLambda = 0f; | ||
129 | |||
130 | //calculate safe moving fraction from distance / (linear+rotational velocity) | ||
131 | |||
132 | //float clippedDist = GEN_min(angularConservativeRadius,dist); | ||
133 | //float clippedDist = dist; | ||
134 | |||
135 | float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n); | ||
136 | |||
137 | dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity); | ||
138 | |||
139 | lambda = lambda + dLambda; | ||
140 | |||
141 | if (lambda > 1f) return false; | ||
142 | if (lambda < 0f) return false; | ||
143 | |||
144 | //todo: next check with relative epsilon | ||
145 | if (lambda <= lastLambda) | ||
146 | break; | ||
147 | lastLambda = lambda; | ||
148 | |||
149 | |||
150 | //interpolate to next lambda | ||
151 | Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans; | ||
152 | |||
153 | TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA); | ||
154 | TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB); | ||
155 | |||
156 | relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA); | ||
157 | |||
158 | result.DebugDraw(lambda); | ||
159 | |||
160 | PointCollector pointCollector = new PointCollector(); | ||
161 | gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver); | ||
162 | input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | ||
163 | input.TransformA = interpolatedTransA; | ||
164 | input.TransformB = interpolatedTransB; | ||
165 | |||
166 | // !!!!!!!!!! | ||
167 | r = (DiscreteCollisionDetectorInterface.Result)pointCollector1; | ||
168 | gjk.GetClosestPoints(input, r, null); | ||
169 | |||
170 | if (pointCollector.HasResult) | ||
171 | { | ||
172 | if (pointCollector.Distance < 0f) | ||
173 | { | ||
174 | //degenerate ?! | ||
175 | result.Fraction = lastLambda; | ||
176 | result.Normal = n; | ||
177 | return true; | ||
178 | } | ||
179 | c = pointCollector.PointInWorld; | ||
180 | |||
181 | dist = pointCollector.Distance; | ||
182 | } | ||
183 | else | ||
184 | { | ||
185 | //?? | ||
186 | return false; | ||
187 | } | ||
188 | |||
189 | } | ||
190 | |||
191 | result.Fraction = lambda; | ||
192 | result.Normal = n; | ||
193 | return true; | ||
194 | } | ||
195 | |||
196 | return false; | ||
197 | } | ||
198 | } | ||
199 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs new file mode 100644 index 0000000..9e88a7e --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs | |||
@@ -0,0 +1,73 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | |||
30 | /// <summary> | ||
31 | /// CastResult stores the closest result | ||
32 | /// alternatively, add a callback method to decide about closest/all results | ||
33 | /// </summary> | ||
34 | public class CastResult | ||
35 | { | ||
36 | private Vector3 _normal; | ||
37 | private float _fraction; | ||
38 | private Matrix _hitTransformA; | ||
39 | private Matrix _hitTransformB; | ||
40 | private IDebugDraw _debugDrawer; | ||
41 | |||
42 | public CastResult() | ||
43 | { | ||
44 | _fraction = 1e30f; | ||
45 | } | ||
46 | |||
47 | public Vector3 Normal { get { return _normal; } set { _normal = value; } } | ||
48 | public float Fraction { get { return _fraction; } set { _fraction = value; } } | ||
49 | public Matrix HitTransformA { get { return _hitTransformA; } set { _hitTransformA = value; } } | ||
50 | public Matrix HitTransformB { get { return _hitTransformB; } set { _hitTransformB = value; } } | ||
51 | public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } } | ||
52 | |||
53 | public virtual void DebugDraw(float fraction) { } | ||
54 | public virtual void DrawCoordSystem(Matrix trans) { } | ||
55 | } | ||
56 | |||
57 | /// <summary> | ||
58 | /// ConvexCast is an interface for Casting | ||
59 | /// </summary> | ||
60 | public interface IConvexCast | ||
61 | { | ||
62 | /// <summary> | ||
63 | /// cast a convex against another convex object | ||
64 | /// </summary> | ||
65 | /// <param name="fromA"></param> | ||
66 | /// <param name="toA"></param> | ||
67 | /// <param name="fromB"></param> | ||
68 | /// <param name="toB"></param> | ||
69 | /// <param name="result"></param> | ||
70 | /// <returns></returns> | ||
71 | bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result); | ||
72 | } | ||
73 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs new file mode 100644 index 0000000..fae2557 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs | |||
@@ -0,0 +1,117 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public abstract class DiscreteCollisionDetectorInterface | ||
30 | { | ||
31 | public abstract class Result | ||
32 | { | ||
33 | public abstract void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB); | ||
34 | public abstract void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth); | ||
35 | } | ||
36 | |||
37 | public class ClosestPointInput | ||
38 | { | ||
39 | private float _maximumDistanceSquared; | ||
40 | private Matrix _transformA, _transformB; | ||
41 | |||
42 | #region Properties | ||
43 | public Matrix TransformB | ||
44 | { | ||
45 | get { return _transformB; } | ||
46 | set { _transformB = value; } | ||
47 | } | ||
48 | |||
49 | public Matrix TransformA | ||
50 | { | ||
51 | get { return _transformA; } | ||
52 | set { _transformA = value; } | ||
53 | } | ||
54 | |||
55 | public float MaximumDistanceSquared | ||
56 | { | ||
57 | get { return _maximumDistanceSquared; } | ||
58 | set { _maximumDistanceSquared = value; } | ||
59 | } | ||
60 | #endregion | ||
61 | |||
62 | public ClosestPointInput() | ||
63 | { | ||
64 | _maximumDistanceSquared = 1e30f; | ||
65 | } | ||
66 | } | ||
67 | |||
68 | public abstract void GetClosestPoints(ClosestPointInput input, Result output, IDebugDraw debugDraw); | ||
69 | } | ||
70 | |||
71 | public class StorageResult : DiscreteCollisionDetectorInterface.Result | ||
72 | { | ||
73 | private Vector3 _closestPointInB; | ||
74 | private Vector3 _normalOnSurfaceB; | ||
75 | private float _distance; //negative means penetration ! | ||
76 | |||
77 | #region Properties | ||
78 | |||
79 | public float Distance | ||
80 | { | ||
81 | get { return _distance; } | ||
82 | set { _distance = value; } | ||
83 | } | ||
84 | public Vector3 NormalOnSurfaceB | ||
85 | { | ||
86 | get { return _normalOnSurfaceB; } | ||
87 | set { _normalOnSurfaceB = value; } | ||
88 | } | ||
89 | public Vector3 ClosestPointInB | ||
90 | { | ||
91 | get { return _closestPointInB; } | ||
92 | set { _closestPointInB = value; } | ||
93 | } | ||
94 | |||
95 | #endregion | ||
96 | |||
97 | public StorageResult() | ||
98 | { | ||
99 | _distance = 1e30f; | ||
100 | } | ||
101 | |||
102 | public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) | ||
103 | { | ||
104 | if (depth < _distance) | ||
105 | { | ||
106 | _normalOnSurfaceB = normalOnBInWorld; | ||
107 | _closestPointInB = pointInWorld; | ||
108 | _distance = depth; | ||
109 | } | ||
110 | } | ||
111 | |||
112 | public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1) | ||
113 | { | ||
114 | throw new Exception("The method or operation is not implemented."); | ||
115 | } | ||
116 | } | ||
117 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs new file mode 100644 index 0000000..4ee9599 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs | |||
@@ -0,0 +1,176 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// GjkConvexCast performs a raycast on a convex object using support mapping. | ||
31 | /// </summary> | ||
32 | public class GjkConvexCast : IConvexCast | ||
33 | { | ||
34 | private VoronoiSimplexSolver _simplexSolver; | ||
35 | private ConvexShape _convexA, _convexB; | ||
36 | |||
37 | public GjkConvexCast(ConvexShape convexShapeA, ConvexShape convexShapeB, VoronoiSimplexSolver solver) | ||
38 | { | ||
39 | _simplexSolver = solver; | ||
40 | |||
41 | _convexA = convexShapeA; | ||
42 | _convexB = convexShapeB; | ||
43 | } | ||
44 | |||
45 | #region IConvexCast Members | ||
46 | |||
47 | /// <summary> | ||
48 | /// cast a convex against another convex object | ||
49 | /// </summary> | ||
50 | /// <param name="fromA"></param> | ||
51 | /// <param name="toA"></param> | ||
52 | /// <param name="fromB"></param> | ||
53 | /// <param name="toB"></param> | ||
54 | /// <param name="result"></param> | ||
55 | /// <returns></returns> | ||
56 | public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) | ||
57 | { | ||
58 | MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB); | ||
59 | |||
60 | Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB; | ||
61 | Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB; | ||
62 | |||
63 | Matrix transformA = fromA; | ||
64 | Matrix transformB = fromB; | ||
65 | |||
66 | transformA.Translation = new Vector3(0, 0, 0); | ||
67 | transformB.Translation = new Vector3(0, 0, 0); | ||
68 | |||
69 | combined.TransformA = transformA; | ||
70 | combined.TransformB = transformB; | ||
71 | |||
72 | float radius = 0.01f; | ||
73 | float lambda = 0; | ||
74 | |||
75 | Vector3 s = rayFromLocalA.Translation; | ||
76 | Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation; | ||
77 | Vector3 x = s; | ||
78 | Vector3 n = new Vector3(); | ||
79 | Vector3 c = new Vector3(); | ||
80 | |||
81 | bool hasResult = false; | ||
82 | float lastLambda = lambda; | ||
83 | |||
84 | IConvexPenetrationDepthSolver penSolver = null; | ||
85 | Matrix identityTransform = Matrix.Identity; | ||
86 | |||
87 | SphereShape raySphere = new SphereShape(0.0f); | ||
88 | raySphere.Margin=0.0f; | ||
89 | |||
90 | Matrix sphereTransform = Matrix.Identity; | ||
91 | sphereTransform.Translation = rayFromLocalA.Translation; | ||
92 | |||
93 | result.DrawCoordSystem(sphereTransform); | ||
94 | |||
95 | { | ||
96 | PointCollector pointCollector = new PointCollector(); | ||
97 | GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver); | ||
98 | |||
99 | GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | ||
100 | input.TransformA = sphereTransform; | ||
101 | input.TransformB = identityTransform; | ||
102 | |||
103 | gjk.GetClosestPoints(input, pointCollector, null); | ||
104 | |||
105 | hasResult = pointCollector.HasResult; | ||
106 | |||
107 | c = pointCollector.PointInWorld; | ||
108 | n = pointCollector.NormalOnBInWorld; | ||
109 | } | ||
110 | |||
111 | if (hasResult) | ||
112 | { | ||
113 | float dist = (c - x).Length(); | ||
114 | |||
115 | if (dist < radius) | ||
116 | { | ||
117 | lastLambda = 1.0f; | ||
118 | } | ||
119 | |||
120 | while (dist > radius) | ||
121 | { | ||
122 | n = x - c; | ||
123 | float dot = Vector3.Dot(n, r); | ||
124 | |||
125 | if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false; | ||
126 | |||
127 | lambda = lambda - Vector3.Distance(n, n) / dot; | ||
128 | if (lambda <= lastLambda) break; | ||
129 | |||
130 | lastLambda = lambda; | ||
131 | |||
132 | x = s + lambda * r; | ||
133 | |||
134 | sphereTransform.Translation = x; | ||
135 | result.DrawCoordSystem(sphereTransform); | ||
136 | PointCollector pointCollector = new PointCollector(); | ||
137 | |||
138 | GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver); | ||
139 | GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | ||
140 | input.TransformA = sphereTransform; | ||
141 | input.TransformB = identityTransform; | ||
142 | |||
143 | gjk.GetClosestPoints(input, pointCollector, null); | ||
144 | |||
145 | if (pointCollector.HasResult) | ||
146 | { | ||
147 | if (pointCollector.Distance < 0.0f) | ||
148 | { | ||
149 | result.Fraction = lastLambda; | ||
150 | result.Normal = n; | ||
151 | return true; | ||
152 | } | ||
153 | |||
154 | c = pointCollector.PointInWorld; | ||
155 | dist = (c - x).Length(); | ||
156 | } | ||
157 | else | ||
158 | { | ||
159 | return false; | ||
160 | } | ||
161 | } | ||
162 | |||
163 | if (lastLambda < 1.0f) | ||
164 | { | ||
165 | result.Fraction = lastLambda; | ||
166 | result.Normal = n; | ||
167 | return true; | ||
168 | } | ||
169 | } | ||
170 | |||
171 | return false; | ||
172 | } | ||
173 | |||
174 | #endregion | ||
175 | } | ||
176 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs new file mode 100644 index 0000000..50ae7ab --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs | |||
@@ -0,0 +1,633 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// GJK-EPA collision solver by Nathanael Presson | ||
31 | /// Nov.2006 | ||
32 | /// </summary> | ||
33 | public class GjkEpa | ||
34 | { | ||
35 | //private static readonly int _precision = 1 /* U(sizeof(F) == 4)*/; | ||
36 | |||
37 | private static readonly float _infinity = MathHelper.Infinity; | ||
38 | //private static readonly float _pi = (float)Math.PI; | ||
39 | private static readonly float _twoPi = (float)(Math.PI * 2); | ||
40 | |||
41 | private static readonly int _gjkMaxIterations = 128; | ||
42 | private static readonly int _gjkHashSize = 1 << 6; | ||
43 | private static readonly int _gjkHashMask = _gjkHashSize - 1; | ||
44 | private static readonly float _gjkInSimplexEpsilon = 0.0001f; | ||
45 | private static readonly float _gjkSquareInSimplexEpsilon = _gjkInSimplexEpsilon * _gjkInSimplexEpsilon; | ||
46 | |||
47 | private static readonly int _epaMaxIterations = 256; | ||
48 | private static readonly float _epaInFaceEpsilon = 0.01f; | ||
49 | private static readonly float _epaAccuracy = 0.001f; | ||
50 | |||
51 | public static float EpaAccuracy { get { return _epaAccuracy; } } | ||
52 | |||
53 | private static float Abs(float v) { return (v < 0 ? -v : v); } | ||
54 | private static float Sign(float v) { return (v < 0 ? -1 : 1); } | ||
55 | |||
56 | static void Swap<T>(ref T a, ref T b) | ||
57 | { | ||
58 | T t = a; | ||
59 | a = b; | ||
60 | b = t; | ||
61 | } | ||
62 | |||
63 | public class Gjk | ||
64 | { | ||
65 | public class MinkowskiVertice | ||
66 | { | ||
67 | private Vector3 _vertice; /* Minkowski vertice */ | ||
68 | private Vector3 _ray; /* Ray */ | ||
69 | |||
70 | public Vector3 Vertice { get { return _vertice; } set { _vertice = value; } } | ||
71 | public Vector3 Ray { get { return _ray; } set { _ray = value; } } | ||
72 | } | ||
73 | public class He | ||
74 | { | ||
75 | private Vector3 _ray; | ||
76 | private He _next; | ||
77 | |||
78 | public He Next { get { return _next; } set { _next = value; } } | ||
79 | public Vector3 Ray { get { return _ray; } set { _ray = value; } } | ||
80 | } | ||
81 | |||
82 | private He[] _table = new He[_gjkHashSize]; | ||
83 | private Matrix[] _wrotations = new Matrix[2]; | ||
84 | private Vector3[] _positions = new Vector3[2]; | ||
85 | private ConvexShape[] _shapes = new ConvexShape[2]; | ||
86 | private MinkowskiVertice[] _simplex = new MinkowskiVertice[5]; | ||
87 | private Vector3 _ray; | ||
88 | private int _order; | ||
89 | private int _iterations; | ||
90 | private float _margin; | ||
91 | private bool _failed; | ||
92 | |||
93 | public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, | ||
94 | Matrix wrotationB, Vector3 positionB, ConvexShape shapeB) | ||
95 | : this(wrotationA, positionA, shapeA, wrotationB, positionB, shapeB, 0) { } | ||
96 | |||
97 | public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, | ||
98 | Matrix wrotationB, Vector3 positionB, ConvexShape shapeB, | ||
99 | float pmargin) | ||
100 | { | ||
101 | for (int i = 0; i < _simplex.Length; i++) | ||
102 | _simplex[i] = new MinkowskiVertice(); | ||
103 | |||
104 | for (int i = 0; i < _wrotations.Length; i++) | ||
105 | _wrotations[i] = new Matrix(); | ||
106 | |||
107 | for (int i = 0; i < _positions.Length; i++) | ||
108 | _positions[i] = new Vector3(); | ||
109 | |||
110 | _wrotations[0] = wrotationA; _positions[0] = positionA; | ||
111 | _shapes[0] = shapeA; | ||
112 | _wrotations[0].Translation = Vector3.Zero; | ||
113 | _wrotations[1] = wrotationB; _positions[1] = positionB; | ||
114 | _shapes[1] = shapeB; | ||
115 | _wrotations[1].Translation = Vector3.Zero; | ||
116 | //sablock = sa->BeginBlock(); | ||
117 | _margin = pmargin; | ||
118 | _failed = false; | ||
119 | } | ||
120 | |||
121 | public bool Failed { get { return _failed; } } | ||
122 | public int Iterations { get { return _iterations; } } | ||
123 | public int Order { get { return _order; } } | ||
124 | public MinkowskiVertice[] Simplex { get { return _simplex; } } | ||
125 | |||
126 | public int Hash(Vector3 v) | ||
127 | { | ||
128 | int h = ((int)(v.X * 15461) ^ (int)(v.Y * 83003) ^ (int)(v.Z * 15473)); | ||
129 | return (h * 169639) & _gjkHashMask; | ||
130 | } | ||
131 | |||
132 | public bool FetchSupport() | ||
133 | { | ||
134 | int h = Hash(_ray); | ||
135 | He e = _table[h]; | ||
136 | while (e != null) | ||
137 | { | ||
138 | if (e.Ray == _ray) | ||
139 | { | ||
140 | --_order; | ||
141 | return (false); | ||
142 | } | ||
143 | else e = e.Next; | ||
144 | } | ||
145 | e = new He(); | ||
146 | e.Ray = _ray; | ||
147 | e.Next = _table[h]; | ||
148 | _table[h] = e; | ||
149 | Support(_ray, ref _simplex[++_order]); | ||
150 | return (Vector3.Dot(_ray, _simplex[_order].Vertice) > 0); | ||
151 | } | ||
152 | |||
153 | public Vector3 LocalSupport(Vector3 d, int i) | ||
154 | { | ||
155 | Matrix m = _wrotations[i]; | ||
156 | m.Translation = Vector3.Zero; | ||
157 | Vector3 vtx = Vector3.TransformNormal(d, m); | ||
158 | Vector3 result = MathHelper.MatrixToVector(_wrotations[i], _shapes[i].LocalGetSupportingVertex(vtx)); | ||
159 | return (result + _positions[i]); | ||
160 | } | ||
161 | |||
162 | public void Support(Vector3 d, ref MinkowskiVertice v) | ||
163 | { | ||
164 | v.Ray = d; | ||
165 | v.Vertice = LocalSupport(d, 0) - LocalSupport(-d, 1) + d * _margin; | ||
166 | } | ||
167 | |||
168 | public bool SolveSimplex2(Vector3 ao, Vector3 ab) | ||
169 | { | ||
170 | if (Vector3.Dot(ab, ao) >= 0) | ||
171 | { | ||
172 | Vector3 cabo = Vector3.Cross(ab, ao); | ||
173 | if (cabo.LengthSquared() > _gjkSquareInSimplexEpsilon) | ||
174 | { _ray = Vector3.Cross(cabo, ab); } | ||
175 | else | ||
176 | { return true; } | ||
177 | } | ||
178 | else | ||
179 | { | ||
180 | _order = 0; | ||
181 | _simplex[0].Ray = _simplex[1].Ray; | ||
182 | _simplex[0].Vertice = _simplex[1].Vertice; | ||
183 | |||
184 | _ray = ao; | ||
185 | } | ||
186 | return false; | ||
187 | } | ||
188 | |||
189 | public bool SolveSimplex3(Vector3 ao, Vector3 ab, Vector3 ac) | ||
190 | { | ||
191 | return (SolveSimplex3a(ao, ab, ac, Vector3.Cross(ab, ac))); | ||
192 | } | ||
193 | |||
194 | public bool SolveSimplex3a(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 cabc) | ||
195 | { | ||
196 | if ((Vector3.Dot(Vector3.Cross(cabc, ab), ao)) < -_gjkInSimplexEpsilon) | ||
197 | { | ||
198 | _order = 1; | ||
199 | _simplex[0].Vertice = _simplex[1].Vertice; | ||
200 | _simplex[0].Ray = _simplex[1].Ray; | ||
201 | |||
202 | _simplex[1].Vertice = _simplex[2].Vertice; | ||
203 | _simplex[1].Ray = _simplex[2].Ray; | ||
204 | |||
205 | return (SolveSimplex2(ao, ab)); | ||
206 | } | ||
207 | else if (Vector3.Dot(Vector3.Cross(cabc, ac), ao) > +_gjkInSimplexEpsilon) | ||
208 | { | ||
209 | _order = 1; | ||
210 | _simplex[1].Vertice = _simplex[2].Vertice; | ||
211 | _simplex[1].Ray = _simplex[2].Ray; | ||
212 | |||
213 | return (SolveSimplex2(ao, ac)); | ||
214 | } | ||
215 | else | ||
216 | { | ||
217 | float d = Vector3.Dot(cabc, ao); | ||
218 | if (Abs(d) > _gjkInSimplexEpsilon) | ||
219 | { | ||
220 | if (d > 0) | ||
221 | { _ray = cabc; } | ||
222 | else | ||
223 | { _ray = -cabc; Swap<MinkowskiVertice>(ref _simplex[0], ref _simplex[1]); } | ||
224 | return (false); | ||
225 | } | ||
226 | else return (true); | ||
227 | } | ||
228 | } | ||
229 | |||
230 | public bool SolveSimplex4(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 ad) | ||
231 | { | ||
232 | Vector3 crs; | ||
233 | if (Vector3.Dot((crs = Vector3.Cross(ab, ac)), ao) > _gjkInSimplexEpsilon) | ||
234 | { | ||
235 | _order = 2; | ||
236 | _simplex[0].Vertice = _simplex[1].Vertice; | ||
237 | _simplex[0].Ray = _simplex[1].Ray; | ||
238 | |||
239 | _simplex[1].Vertice = _simplex[2].Vertice; | ||
240 | _simplex[1].Ray = _simplex[2].Ray; | ||
241 | |||
242 | _simplex[2].Vertice = _simplex[3].Vertice; | ||
243 | _simplex[2].Ray = _simplex[3].Ray; | ||
244 | |||
245 | return (SolveSimplex3a(ao, ab, ac, crs)); | ||
246 | } | ||
247 | else if (Vector3.Dot((crs = Vector3.Cross(ac, ad)), ao) > _gjkInSimplexEpsilon) | ||
248 | { | ||
249 | _order = 2; | ||
250 | _simplex[2].Vertice = _simplex[3].Vertice; | ||
251 | _simplex[2].Ray = _simplex[3].Ray; | ||
252 | |||
253 | return (SolveSimplex3a(ao, ac, ad, crs)); | ||
254 | } | ||
255 | else if (Vector3.Dot((crs = Vector3.Cross(ad, ab)), ao) > _gjkInSimplexEpsilon) | ||
256 | { | ||
257 | _order = 2; | ||
258 | |||
259 | _simplex[1].Vertice = _simplex[0].Vertice; | ||
260 | _simplex[1].Ray = _simplex[0].Ray; | ||
261 | |||
262 | _simplex[0].Vertice = _simplex[2].Vertice; | ||
263 | _simplex[0].Ray = _simplex[2].Ray; | ||
264 | |||
265 | _simplex[2].Vertice = _simplex[3].Vertice; | ||
266 | _simplex[2].Ray = _simplex[3].Ray; | ||
267 | |||
268 | return (SolveSimplex3a(ao, ad, ab, crs)); | ||
269 | } | ||
270 | else return (true); | ||
271 | } | ||
272 | |||
273 | public bool SearchOrigin() | ||
274 | { | ||
275 | return SearchOrigin(new Vector3(1, 0, 0)); | ||
276 | } | ||
277 | |||
278 | public bool SearchOrigin(Vector3 initray) | ||
279 | { | ||
280 | _iterations = 0; | ||
281 | unchecked | ||
282 | { | ||
283 | _order = (int)(-1); | ||
284 | } | ||
285 | _failed = false; | ||
286 | _ray = Vector3.Normalize(initray); | ||
287 | |||
288 | //ClearMemory(table, sizeof(void*) * GJK_hashsize); | ||
289 | for (int i = 0; i < _table.Length; i++) | ||
290 | _table[i] = null; | ||
291 | FetchSupport(); | ||
292 | _ray = -_simplex[0].Vertice; | ||
293 | for (; _iterations < _gjkMaxIterations; ++_iterations) | ||
294 | { | ||
295 | float rl = _ray.Length(); | ||
296 | _ray /= rl > 0 ? rl : 1; | ||
297 | if (FetchSupport()) | ||
298 | { | ||
299 | bool found = (false); | ||
300 | switch (_order) | ||
301 | { | ||
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; | ||
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 | } | ||
306 | if (found) return (true); | ||
307 | } | ||
308 | else return (false); | ||
309 | } | ||
310 | _failed = true; | ||
311 | return (false); | ||
312 | } | ||
313 | |||
314 | public bool EncloseOrigin() | ||
315 | { | ||
316 | switch (_order) | ||
317 | { | ||
318 | /* Point */ | ||
319 | case 0: break; | ||
320 | /* Line */ | ||
321 | case 1: | ||
322 | Vector3 ab = _simplex[1].Vertice - _simplex[0].Vertice; | ||
323 | Vector3[] b ={ Vector3.Cross(ab, new Vector3(1, 0, 0)), | ||
324 | Vector3.Cross(ab, new Vector3(0, 1, 0)), | ||
325 | Vector3.Cross(ab, new Vector3(0, 0, 1)) }; | ||
326 | float[] m ={ b[0].LengthSquared(), b[1].LengthSquared(), b[2].LengthSquared() }; | ||
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]; | ||
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); | ||
331 | Support(Vector3.Normalize(w), ref _simplex[3]); w = Vector3.TransformNormal(w, r); | ||
332 | _order = 4; | ||
333 | return true; | ||
334 | /* Triangle */ | ||
335 | case 2: | ||
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]); | ||
338 | Support(-n, ref _simplex[4]); | ||
339 | _order = 4; | ||
340 | return true; | ||
341 | /* Tetrahedron */ | ||
342 | case 3: return (true); | ||
343 | /* Hexahedron */ | ||
344 | case 4: return (true); | ||
345 | } | ||
346 | return (false); | ||
347 | } | ||
348 | } | ||
349 | |||
350 | public class Epa | ||
351 | { | ||
352 | public class Face | ||
353 | { | ||
354 | public Gjk.MinkowskiVertice[] _vertices = new Gjk.MinkowskiVertice[3]; | ||
355 | public Face[] _faces = new Face[3]; | ||
356 | public int[] _e = new int[3]; | ||
357 | public Vector3 _n; | ||
358 | public float _d; | ||
359 | public int _mark; | ||
360 | public Face _prev; | ||
361 | public Face _next; | ||
362 | } | ||
363 | |||
364 | private Gjk _gjk; | ||
365 | private Face _root; | ||
366 | private int _nfaces; | ||
367 | private int _iterations; | ||
368 | private Vector3[,] _features = new Vector3[2, 3]; | ||
369 | private Vector3[] _nearest = new Vector3[2]; | ||
370 | private Vector3 _normal; | ||
371 | private float _depth; | ||
372 | private bool _failed; | ||
373 | |||
374 | public Epa(Gjk gjk) | ||
375 | { | ||
376 | this._gjk = gjk; | ||
377 | } | ||
378 | |||
379 | public bool Failed { get { return _failed; } } | ||
380 | public int Iterations { get { return _iterations; } } | ||
381 | public Vector3 Normal { get { return _normal; } } | ||
382 | public Vector3[] Nearest { get { return _nearest; } } | ||
383 | |||
384 | public Vector3 GetCoordinates(Face face) | ||
385 | { | ||
386 | Vector3 o = face._n * -face._d; | ||
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(), | ||
389 | Vector3.Cross(face._vertices[2].Vertice - o, face._vertices[0].Vertice - o).Length()}; | ||
390 | float sm = a[0] + a[1] + a[2]; | ||
391 | return (new Vector3(a[1], a[2], a[0]) / (sm > 0 ? sm : 1)); | ||
392 | } | ||
393 | |||
394 | public Face FindBest() | ||
395 | { | ||
396 | Face bf = null; | ||
397 | if (_root != null) | ||
398 | { | ||
399 | Face cf = _root; | ||
400 | float bd = _infinity; | ||
401 | do | ||
402 | { | ||
403 | if (cf._d < bd) { bd = cf._d; bf = cf; } | ||
404 | } while (null != (cf = cf._next)); | ||
405 | } | ||
406 | return bf; | ||
407 | } | ||
408 | |||
409 | public bool Set(ref Face f, Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) | ||
410 | { | ||
411 | Vector3 nrm = Vector3.Cross(b.Vertice - a.Vertice, c.Vertice - a.Vertice); | ||
412 | float len = nrm.Length(); | ||
413 | bool valid = (Vector3.Dot(Vector3.Cross(a.Vertice, b.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); | ||
416 | f._vertices[0] = a; | ||
417 | f._vertices[1] = b; | ||
418 | f._vertices[2] = c; | ||
419 | f._mark = 0; | ||
420 | f._n = nrm / (len > 0 ? len : _infinity); | ||
421 | f._d = Max(0, -Vector3.Dot(f._n, a.Vertice)); | ||
422 | return valid; | ||
423 | } | ||
424 | |||
425 | public Face NewFace(Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) | ||
426 | { | ||
427 | Face pf = new Face(); | ||
428 | if (Set(ref pf, a, b, c)) | ||
429 | { | ||
430 | if (_root != null) _root._prev = pf; | ||
431 | pf._prev = null; | ||
432 | pf._next = _root; | ||
433 | _root = pf; | ||
434 | ++_nfaces; | ||
435 | } | ||
436 | else | ||
437 | { | ||
438 | pf._prev = pf._next = null; | ||
439 | } | ||
440 | return (pf); | ||
441 | } | ||
442 | |||
443 | public void Detach(ref Face face) | ||
444 | { | ||
445 | if (face._prev != null || face._next != null) | ||
446 | { | ||
447 | --_nfaces; | ||
448 | if (face == _root) | ||
449 | { | ||
450 | _root = face._next; | ||
451 | _root._prev = null; | ||
452 | } | ||
453 | else | ||
454 | { | ||
455 | if (face._next == null) | ||
456 | { | ||
457 | face._prev._next = null; | ||
458 | } | ||
459 | else | ||
460 | { | ||
461 | face._prev._next = face._next; | ||
462 | face._next._prev = face._prev; | ||
463 | } | ||
464 | } | ||
465 | face._prev = face._next = null; | ||
466 | } | ||
467 | } | ||
468 | |||
469 | public void Link(ref Face f0, int e0, ref Face f1, int e1) | ||
470 | { | ||
471 | f0._faces[e0] = f1; f1._e[e1] = e0; | ||
472 | f1._faces[e1] = f0; f0._e[e0] = e1; | ||
473 | } | ||
474 | |||
475 | public Gjk.MinkowskiVertice Support(Vector3 w) | ||
476 | { | ||
477 | Gjk.MinkowskiVertice v = new Gjk.MinkowskiVertice(); | ||
478 | _gjk.Support(w, ref v); | ||
479 | return v; | ||
480 | } | ||
481 | |||
482 | private static int[] mod3 ={ 0, 1, 2, 0, 1 }; | ||
483 | |||
484 | public int BuildHorizon(int markid, Gjk.MinkowskiVertice w, ref Face f, int e, ref Face cf, ref Face ff) | ||
485 | { | ||
486 | int ne = (0); | ||
487 | if (f._mark != markid) | ||
488 | { | ||
489 | int e1 = (mod3[e + 1]); | ||
490 | if ((Vector3.Dot(f._n, w.Vertice) + f._d) > 0) | ||
491 | { | ||
492 | Face nf = NewFace(f._vertices[e1], f._vertices[e], w); | ||
493 | Link(ref nf, 0, ref f, e); | ||
494 | if (cf != null) Link(ref cf, 1, ref nf, 2); else ff = nf; | ||
495 | cf = nf; ne = 1; | ||
496 | } | ||
497 | else | ||
498 | { | ||
499 | int e2 = (mod3[e + 2]); | ||
500 | Detach(ref f); | ||
501 | f._mark = markid; | ||
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); | ||
504 | } | ||
505 | } | ||
506 | return (ne); | ||
507 | } | ||
508 | |||
509 | public float EvaluatePD() | ||
510 | { | ||
511 | return EvaluatePD(_epaAccuracy); | ||
512 | } | ||
513 | |||
514 | private int[,] fidx; | ||
515 | private int[,] eidx; | ||
516 | |||
517 | public float EvaluatePD(float accuracy) | ||
518 | { | ||
519 | //Block* sablock = sa->BeginBlock(); | ||
520 | Face bestface = null; | ||
521 | int markid = 1; | ||
522 | _depth = -_infinity; | ||
523 | _normal = new Vector3(); | ||
524 | _root = null; | ||
525 | _nfaces = 0; | ||
526 | _iterations = 0; | ||
527 | _failed = false; | ||
528 | /* Prepare hull */ | ||
529 | if (_gjk.EncloseOrigin()) | ||
530 | { | ||
531 | int nfidx = 0; | ||
532 | int neidx = 0; | ||
533 | Gjk.MinkowskiVertice[] basemkv = new Gjk.MinkowskiVertice[5]; | ||
534 | Face[] basefaces = new Face[6]; | ||
535 | switch (_gjk.Order) | ||
536 | { | ||
537 | /* Tetrahedron */ | ||
538 | case 3: | ||
539 | { | ||
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 } }; | ||
542 | nfidx = 4; neidx = 6; | ||
543 | } break; | ||
544 | /* Hexahedron */ | ||
545 | case 4: | ||
546 | { | ||
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 } }; | ||
549 | nfidx = 6; neidx = 9; | ||
550 | } break; | ||
551 | } | ||
552 | int i; | ||
553 | |||
554 | for (i = 0; i <= _gjk.Order; ++i) | ||
555 | { | ||
556 | //basemkv[i] = (GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv)); | ||
557 | basemkv[i] = new Gjk.MinkowskiVertice(); | ||
558 | basemkv[i].Vertice = _gjk.Simplex[i].Vertice; | ||
559 | basemkv[i].Ray = _gjk.Simplex[i].Ray; | ||
560 | } | ||
561 | for (i = 0; i < nfidx; ++i) | ||
562 | { | ||
563 | basefaces[i] = NewFace(basemkv[fidx[i, 0]], basemkv[fidx[i, 1]], basemkv[fidx[i, 2]]); | ||
564 | } | ||
565 | for (i = 0; i < neidx; ++i) | ||
566 | { | ||
567 | Link(ref basefaces[eidx[i, 0]], eidx[i, 1], ref basefaces[eidx[i, 2]], eidx[i, 3]); | ||
568 | } | ||
569 | } | ||
570 | if (0 == _nfaces) | ||
571 | { | ||
572 | return _depth; | ||
573 | } | ||
574 | /* Expand hull */ | ||
575 | for (; _iterations < _epaMaxIterations; ++_iterations) | ||
576 | { | ||
577 | Face bf = FindBest(); | ||
578 | if (bf != null) | ||
579 | { | ||
580 | Gjk.MinkowskiVertice w = Support(-bf._n); | ||
581 | float d = Vector3.Dot(bf._n, w.Vertice) + bf._d; | ||
582 | bestface = bf; | ||
583 | if (d < -accuracy) | ||
584 | { | ||
585 | Face cf = null; | ||
586 | Face ff = null; | ||
587 | int nf = 0; | ||
588 | Detach(ref bf); | ||
589 | bf._mark = ++markid; | ||
590 | for (int i = 0; i < 3; ++i) | ||
591 | { | ||
592 | nf += BuildHorizon(markid, w, ref bf._faces[i], bf._e[i], ref cf, ref ff); | ||
593 | } | ||
594 | if (nf <= 2) { break; } | ||
595 | Link(ref cf, 1, ref ff, 2); | ||
596 | } | ||
597 | else break; | ||
598 | } | ||
599 | else break; | ||
600 | } | ||
601 | /* Extract contact */ | ||
602 | if (bestface != null) | ||
603 | { | ||
604 | Vector3 b = GetCoordinates(bestface); | ||
605 | _normal = bestface._n; | ||
606 | _depth = Max(0, bestface._d); | ||
607 | for (int i = 0; i < 2; ++i) | ||
608 | { | ||
609 | float s = i != 0 ? -1 : 1; | ||
610 | for (int j = 0; j < 3; ++j) | ||
611 | { | ||
612 | _features[i, j] = _gjk.LocalSupport(s * bestface._vertices[j].Ray, i); | ||
613 | } | ||
614 | } | ||
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; | ||
617 | } | ||
618 | else _failed = true; | ||
619 | return _depth; | ||
620 | } | ||
621 | |||
622 | private float Max(float a, float b) | ||
623 | { | ||
624 | return (a > b ? a : b); | ||
625 | } | ||
626 | |||
627 | private float Min(float a, float b) | ||
628 | { | ||
629 | return (a < b ? a : b); | ||
630 | } | ||
631 | } | ||
632 | } | ||
633 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs new file mode 100644 index 0000000..739b4fc --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs | |||
@@ -0,0 +1,56 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to | ||
31 | /// calculate the penetration depth between two convex shapes. | ||
32 | /// </summary> | ||
33 | public class GjkEpaPenetrationDepthSolver : IConvexPenetrationDepthSolver | ||
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) | ||
36 | { | ||
37 | float radialmargin = 0; | ||
38 | |||
39 | GjkEpaSolver.Results results; | ||
40 | if (GjkEpaSolver.Collide(convexA, transformA, | ||
41 | convexB, transformB, | ||
42 | radialmargin, out results)) | ||
43 | { | ||
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); | ||
46 | ptrA = results.Witnesses[0]; | ||
47 | ptrB = results.Witnesses[1]; | ||
48 | return true; | ||
49 | } | ||
50 | ptrA = new Vector3(); | ||
51 | ptrB = new Vector3(); | ||
52 | |||
53 | return false; | ||
54 | } | ||
55 | } | ||
56 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs new file mode 100644 index 0000000..aa9d61e --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs | |||
@@ -0,0 +1,101 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// GjkEpaSolver contributed under zlib by Nathanael Presson | ||
31 | /// </summary> | ||
32 | public class GjkEpaSolver | ||
33 | { | ||
34 | public struct Results | ||
35 | { | ||
36 | public enum Status | ||
37 | { | ||
38 | Separated, /* Shapes doesnt penetrate */ | ||
39 | Penetrating, /* Shapes are penetrating */ | ||
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 */ | ||
42 | } | ||
43 | |||
44 | private Vector3[] _witnesses; | ||
45 | private Vector3 _normal; | ||
46 | private float _depth; | ||
47 | private int _epaIterations; | ||
48 | private int _gjkIterations; | ||
49 | private Status _status; | ||
50 | |||
51 | public Vector3[] Witnesses { get { return _witnesses; } set { _witnesses = value; } } | ||
52 | public Vector3 Normal { get { return _normal; } set { _normal = value; } } | ||
53 | public float Depth { get { return _depth; } set { _depth = value; } } | ||
54 | public int EpaIterations { get { return _epaIterations; } set { _epaIterations = value; } } | ||
55 | public int GjkIterations { get { return _gjkIterations; } set { _gjkIterations = value; } } | ||
56 | public Status ResultStatus { get { return _status; } set { _status = value; } } | ||
57 | } | ||
58 | |||
59 | public static bool Collide(ConvexShape shapeA, Matrix wtrsA, | ||
60 | ConvexShape shapeB, Matrix wtrsB, | ||
61 | float radialmargin, | ||
62 | out Results results) | ||
63 | { | ||
64 | /* Initialize */ | ||
65 | results = new Results(); | ||
66 | results.Witnesses = new Vector3[2]; | ||
67 | results.Witnesses[0] = | ||
68 | results.Witnesses[1] = | ||
69 | results.Normal = new Vector3(); | ||
70 | results.Depth = 0; | ||
71 | results.ResultStatus = Results.Status.Separated; | ||
72 | results.EpaIterations = 0; | ||
73 | results.GjkIterations = 0; | ||
74 | /* Use GJK to locate origin */ | ||
75 | GjkEpa.Gjk gjk = new GjkEpa.Gjk(wtrsA, wtrsA.Translation, shapeA, | ||
76 | wtrsB, wtrsB.Translation, shapeB, | ||
77 | radialmargin + GjkEpa.EpaAccuracy); | ||
78 | bool collide = gjk.SearchOrigin(); | ||
79 | results.GjkIterations = gjk.Iterations + 1; | ||
80 | if (collide) | ||
81 | { | ||
82 | /* Then EPA for penetration depth */ | ||
83 | GjkEpa.Epa epa = new GjkEpa.Epa(gjk); | ||
84 | float pd = epa.EvaluatePD(); | ||
85 | results.EpaIterations = epa.Iterations + 1; | ||
86 | if (pd > 0) | ||
87 | { | ||
88 | results.ResultStatus = Results.Status.Penetrating; | ||
89 | results.Normal = epa.Normal; | ||
90 | results.Depth = pd; | ||
91 | results.Witnesses[0] = epa.Nearest[0]; | ||
92 | results.Witnesses[1] = epa.Nearest[1]; | ||
93 | return true; | ||
94 | } | ||
95 | else { if (epa.Failed) results.ResultStatus = Results.Status.EpaFailed; } | ||
96 | } | ||
97 | else { if (gjk.Failed) results.ResultStatus = Results.Status.GjkFailed; } | ||
98 | return false; | ||
99 | } | ||
100 | } | ||
101 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs new file mode 100644 index 0000000..0831ff5 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs | |||
@@ -0,0 +1,343 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class GjkPairDetector : DiscreteCollisionDetectorInterface | ||
30 | { | ||
31 | private Vector3 _cachedSeparatingAxis; | ||
32 | private IConvexPenetrationDepthSolver _penetrationDepthSolver; | ||
33 | private ISimplexSolver _simplexSolver; | ||
34 | private ConvexShape _minkowskiA, _minkowskiB; | ||
35 | private bool _ignoreMargin; | ||
36 | |||
37 | private int _lastUsedMethod; | ||
38 | private int _currentIteration; | ||
39 | private int _degenerateSimplex; | ||
40 | private int _catchDegeneracies; | ||
41 | |||
42 | private static int _numDeepPenetrationChecks = 0; | ||
43 | private static int _numGjkChecks = 0; | ||
44 | |||
45 | private const float RelativeError2 = 1.0e-6f; | ||
46 | |||
47 | #region Properties | ||
48 | public int LastUsedMethod | ||
49 | { | ||
50 | get { return _lastUsedMethod; } | ||
51 | set { _lastUsedMethod = value; } | ||
52 | } | ||
53 | |||
54 | public int CurrentIteration | ||
55 | { | ||
56 | get { return _currentIteration; } | ||
57 | set { _currentIteration = value; } | ||
58 | } | ||
59 | |||
60 | public int DegenerateSimplex | ||
61 | { | ||
62 | get { return _degenerateSimplex; } | ||
63 | set { _degenerateSimplex = value; } | ||
64 | } | ||
65 | |||
66 | public int CatchDegeneracies | ||
67 | { | ||
68 | get { return _catchDegeneracies; } | ||
69 | set { _catchDegeneracies = value; } | ||
70 | } | ||
71 | |||
72 | public static int DeepPenetrationChecksCount { get { return _numDeepPenetrationChecks; } } | ||
73 | public static int GjkChecksCount { get { return _numGjkChecks; } } | ||
74 | #endregion | ||
75 | |||
76 | public GjkPairDetector(ConvexShape objectA, ConvexShape objectB, | ||
77 | ISimplexSolver simplexSolver, | ||
78 | IConvexPenetrationDepthSolver penetrationDepthSolver) | ||
79 | { | ||
80 | _cachedSeparatingAxis = new Vector3(0, 0, 1); | ||
81 | |||
82 | _penetrationDepthSolver = penetrationDepthSolver; | ||
83 | _simplexSolver = simplexSolver; | ||
84 | _minkowskiA = objectA; | ||
85 | _minkowskiB = objectB; | ||
86 | _ignoreMargin = false; | ||
87 | _lastUsedMethod = -1; | ||
88 | _catchDegeneracies = 1; | ||
89 | } | ||
90 | |||
91 | public void setMinkowskiA(ConvexShape minkA) | ||
92 | { | ||
93 | _minkowskiA = minkA; | ||
94 | } | ||
95 | |||
96 | public void setMinkowskiB(ConvexShape minkB) | ||
97 | { | ||
98 | _minkowskiB = minkB; | ||
99 | } | ||
100 | public void setCachedSeperatingAxis(Vector3 seperatingAxis) | ||
101 | { | ||
102 | _cachedSeparatingAxis = seperatingAxis; | ||
103 | } | ||
104 | |||
105 | public void setPenetrationDepthSolver(IConvexPenetrationDepthSolver penetrationDepthSolver) | ||
106 | { | ||
107 | this._penetrationDepthSolver = penetrationDepthSolver; | ||
108 | } | ||
109 | |||
110 | public void setIgnoreMargin(bool ignoreMargin) | ||
111 | { | ||
112 | this._ignoreMargin = ignoreMargin; | ||
113 | } | ||
114 | |||
115 | public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw) | ||
116 | { | ||
117 | float distance = 0; | ||
118 | |||
119 | Vector3 normalInB = new Vector3(); | ||
120 | Vector3 pointOnA = new Vector3(), pointOnB = new Vector3(); | ||
121 | |||
122 | Matrix localTransA = input.TransformA; | ||
123 | Matrix localTransB = input.TransformB; | ||
124 | |||
125 | Vector3 positionOffset = (localTransA.Translation + localTransB.Translation) * 0.5f; | ||
126 | localTransA.Translation -= positionOffset; | ||
127 | localTransB.Translation -= positionOffset; | ||
128 | |||
129 | float marginA = _minkowskiA.Margin; | ||
130 | float marginB = _minkowskiB.Margin; | ||
131 | |||
132 | _numGjkChecks++; | ||
133 | |||
134 | if (_ignoreMargin) | ||
135 | { | ||
136 | marginA = 0; | ||
137 | marginB = 0; | ||
138 | } | ||
139 | |||
140 | _currentIteration = 0; | ||
141 | |||
142 | int gjkMaxIter = 1000; | ||
143 | _cachedSeparatingAxis = new Vector3(0, 1, 0); | ||
144 | |||
145 | bool isValid = false; | ||
146 | bool checkSimplex = false; | ||
147 | bool checkPenetration = true; | ||
148 | _degenerateSimplex = 0; | ||
149 | |||
150 | _lastUsedMethod = -1; | ||
151 | |||
152 | { | ||
153 | float squaredDistance = MathHelper.Infinity; | ||
154 | float delta = 0; | ||
155 | |||
156 | float margin = marginA + marginB; | ||
157 | |||
158 | _simplexSolver.Reset(); | ||
159 | |||
160 | while (true) | ||
161 | { | ||
162 | Matrix transABasis = input.TransformA; | ||
163 | transABasis.Translation = Vector3.Zero; | ||
164 | |||
165 | Matrix transBBasis = input.TransformB; | ||
166 | transBBasis.Translation = Vector3.Zero; | ||
167 | |||
168 | Vector3 seperatingAxisInA = Vector3.TransformNormal(-_cachedSeparatingAxis, transABasis); | ||
169 | Vector3 seperatingAxisInB = Vector3.TransformNormal(_cachedSeparatingAxis, transBBasis); | ||
170 | |||
171 | Vector3 pInA = _minkowskiA.LocalGetSupportingVertexWithoutMargin(seperatingAxisInA); | ||
172 | Vector3 qInB = _minkowskiB.LocalGetSupportingVertexWithoutMargin(seperatingAxisInB); | ||
173 | Vector3 pWorld = MathHelper.MatrixToVector(localTransA, pInA); | ||
174 | Vector3 qWorld = MathHelper.MatrixToVector(localTransB, qInB); | ||
175 | |||
176 | Vector3 w = pWorld - qWorld; | ||
177 | delta = Vector3.Dot(_cachedSeparatingAxis, w); | ||
178 | |||
179 | if ((delta > 0.0) && (delta * delta > squaredDistance * input.MaximumDistanceSquared)) | ||
180 | { | ||
181 | checkPenetration = false; | ||
182 | break; | ||
183 | } | ||
184 | |||
185 | if (_simplexSolver.InSimplex(w)) | ||
186 | { | ||
187 | _degenerateSimplex = 1; | ||
188 | checkSimplex = true; | ||
189 | break; | ||
190 | } | ||
191 | |||
192 | float f0 = squaredDistance - delta; | ||
193 | float f1 = squaredDistance * RelativeError2; | ||
194 | |||
195 | if (f0 <= f1) | ||
196 | { | ||
197 | if (f0 <= 0.0f) | ||
198 | { | ||
199 | _degenerateSimplex = 2; | ||
200 | } | ||
201 | |||
202 | checkSimplex = true; | ||
203 | break; | ||
204 | } | ||
205 | |||
206 | _simplexSolver.AddVertex(w, pWorld, qWorld); | ||
207 | |||
208 | if (!_simplexSolver.Closest(out _cachedSeparatingAxis)) | ||
209 | { | ||
210 | _degenerateSimplex = 3; | ||
211 | checkSimplex = true; | ||
212 | break; | ||
213 | } | ||
214 | |||
215 | float previouseSquaredDistance = squaredDistance; | ||
216 | squaredDistance = _cachedSeparatingAxis.LengthSquared(); | ||
217 | |||
218 | if (previouseSquaredDistance - squaredDistance <= MathHelper.Epsilon * previouseSquaredDistance) | ||
219 | { | ||
220 | _simplexSolver.BackupClosest(out _cachedSeparatingAxis); | ||
221 | checkSimplex = true; | ||
222 | break; | ||
223 | } | ||
224 | |||
225 | if (_currentIteration++ > gjkMaxIter) | ||
226 | { | ||
227 | #if DEBUG | ||
228 | Console.WriteLine("GjkPairDetector maxIter exceeded: {0}", _currentIteration); | ||
229 | Console.WriteLine("sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4}, shapeTypeB={5}", | ||
230 | _cachedSeparatingAxis.X, | ||
231 | _cachedSeparatingAxis.Y, | ||
232 | _cachedSeparatingAxis.Z, | ||
233 | squaredDistance, | ||
234 | _minkowskiA.ShapeType, | ||
235 | _minkowskiB.ShapeType | ||
236 | ); | ||
237 | #endif | ||
238 | break; | ||
239 | } | ||
240 | |||
241 | bool check = (!_simplexSolver.FullSimplex); | ||
242 | |||
243 | if (!check) | ||
244 | { | ||
245 | _simplexSolver.BackupClosest(out _cachedSeparatingAxis); | ||
246 | break; | ||
247 | } | ||
248 | } | ||
249 | |||
250 | if (checkSimplex) | ||
251 | { | ||
252 | _simplexSolver.ComputePoints(out pointOnA, out pointOnB); | ||
253 | normalInB = pointOnA - pointOnB; | ||
254 | float lenSqr = _cachedSeparatingAxis.LengthSquared(); | ||
255 | |||
256 | if (lenSqr < 0.0001f) | ||
257 | { | ||
258 | _degenerateSimplex = 5; | ||
259 | } | ||
260 | |||
261 | if (lenSqr > MathHelper.Epsilon * MathHelper.Epsilon) | ||
262 | { | ||
263 | float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr); | ||
264 | normalInB *= rlen; | ||
265 | float s = (float)Math.Sqrt((float)squaredDistance); | ||
266 | |||
267 | BulletDebug.Assert(s > 0); | ||
268 | pointOnA -= _cachedSeparatingAxis * (marginA / s); | ||
269 | pointOnB += _cachedSeparatingAxis * (marginB / s); | ||
270 | distance = ((1 / rlen) - margin); | ||
271 | |||
272 | isValid = true; | ||
273 | |||
274 | _lastUsedMethod = 1; | ||
275 | } | ||
276 | else | ||
277 | { | ||
278 | _lastUsedMethod = 2; | ||
279 | } | ||
280 | } | ||
281 | |||
282 | bool catchDegeneratePenetrationCase = | ||
283 | (_catchDegeneracies != 0 && _penetrationDepthSolver != null && _degenerateSimplex != 0 && ((distance + margin) < 0.01f)); | ||
284 | |||
285 | if (checkPenetration && (!isValid || catchDegeneratePenetrationCase)) | ||
286 | { | ||
287 | #warning Check this | ||
288 | if (_penetrationDepthSolver != null) | ||
289 | { | ||
290 | Vector3 tmpPointOnA, tmpPointOnB; | ||
291 | |||
292 | _numDeepPenetrationChecks++; | ||
293 | |||
294 | bool isValid2 = _penetrationDepthSolver.CalculatePenetrationDepth( | ||
295 | _simplexSolver, _minkowskiA, _minkowskiB, localTransA, localTransB, | ||
296 | _cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB, | ||
297 | debugDraw | ||
298 | ); | ||
299 | |||
300 | if (isValid2) | ||
301 | { | ||
302 | Vector3 tmpNormalInB = tmpPointOnB - tmpPointOnA; | ||
303 | float lengSqr = tmpNormalInB.LengthSquared(); | ||
304 | |||
305 | if (lengSqr > (MathHelper.Epsilon * MathHelper.Epsilon)) | ||
306 | { | ||
307 | tmpNormalInB /= (float)Math.Sqrt((float)lengSqr); | ||
308 | float distance2 = -(tmpPointOnA - tmpPointOnB).Length(); | ||
309 | |||
310 | if (!isValid || (distance2 < distance)) | ||
311 | { | ||
312 | distance = distance2; | ||
313 | pointOnA = tmpPointOnA; | ||
314 | pointOnB = tmpPointOnB; | ||
315 | normalInB = tmpNormalInB; | ||
316 | isValid = true; | ||
317 | _lastUsedMethod = 3; | ||
318 | } | ||
319 | else | ||
320 | { | ||
321 | |||
322 | } | ||
323 | } | ||
324 | else | ||
325 | { | ||
326 | _lastUsedMethod = 4; | ||
327 | } | ||
328 | } | ||
329 | else | ||
330 | { | ||
331 | _lastUsedMethod = 5; | ||
332 | } | ||
333 | } | ||
334 | } | ||
335 | |||
336 | if (isValid) | ||
337 | { | ||
338 | output.AddContactPoint(normalInB, pointOnB + positionOffset, distance); | ||
339 | } | ||
340 | } | ||
341 | } | ||
342 | } | ||
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 new file mode 100644 index 0000000..59fd486 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs | |||
@@ -0,0 +1,42 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// IConvexPenetrationDepthSolver provides an interface for penetration depth calculation. | ||
31 | /// </summary> | ||
32 | public interface IConvexPenetrationDepthSolver | ||
33 | { | ||
34 | bool CalculatePenetrationDepth( | ||
35 | ISimplexSolver simplexSolver, | ||
36 | ConvexShape convexA, ConvexShape convexB, | ||
37 | Matrix transformA, Matrix transformB, | ||
38 | Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, | ||
39 | IDebugDraw debugDraw//, StackAlloc stackAlloc | ||
40 | ); | ||
41 | } | ||
42 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs new file mode 100644 index 0000000..71b1eaa --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs | |||
@@ -0,0 +1,48 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// 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 | ||
31 | /// voronoi regions or barycentric coordinates | ||
32 | public interface ISimplexSolver | ||
33 | { | ||
34 | void Reset(); | ||
35 | void AddVertex(Vector3 w, Vector3 p, Vector3 q); | ||
36 | bool Closest(out Vector3 v); | ||
37 | |||
38 | int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf); | ||
39 | bool InSimplex(Vector3 w); | ||
40 | void BackupClosest(out Vector3 v); | ||
41 | void ComputePoints(out Vector3 pA, out Vector3 pB); | ||
42 | |||
43 | int NumVertices { get;} | ||
44 | bool EmptySimplex { get;} | ||
45 | float MaxVertex { get;} | ||
46 | bool FullSimplex { get;} | ||
47 | } | ||
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 new file mode 100644 index 0000000..751a9df --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs | |||
@@ -0,0 +1,78 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class ManifoldPoint | ||
30 | { | ||
31 | private Vector3 _localPointA; | ||
32 | private Vector3 _localPointB; | ||
33 | private Vector3 _positionWorldOnB; | ||
34 | private Vector3 _positionWorldOnA; | ||
35 | private Vector3 _normalWorldOnB; | ||
36 | |||
37 | private float _distance; | ||
38 | private float _combinedFriction; | ||
39 | private float _combinedRestitution; | ||
40 | |||
41 | private object _userPersistentData; | ||
42 | |||
43 | private int _lifeTime;//lifetime of the contactpoint in frames | ||
44 | |||
45 | public ManifoldPoint() | ||
46 | : this(new Vector3(), new Vector3(), new Vector3(), 0f) | ||
47 | { | ||
48 | } | ||
49 | |||
50 | public ManifoldPoint(Vector3 pointA, Vector3 pointB, | ||
51 | Vector3 normal, | ||
52 | float distance) | ||
53 | { | ||
54 | _localPointA = pointA; | ||
55 | _localPointB = pointB; | ||
56 | _normalWorldOnB = normal; | ||
57 | _distance = distance; | ||
58 | _positionWorldOnA = new Vector3(); | ||
59 | _positionWorldOnB = new Vector3(); | ||
60 | } | ||
61 | |||
62 | public float Distance { get { return _distance; } set { _distance = value; } } | ||
63 | public int LifeTime { get { return _lifeTime; } set { _lifeTime = value; } } | ||
64 | |||
65 | public Vector3 PositionWorldOnA { get { return _positionWorldOnA; } set { _positionWorldOnA = value; } } | ||
66 | public Vector3 PositionWorldOnB { get { return _positionWorldOnB; } set { _positionWorldOnB = value; } } | ||
67 | |||
68 | public Vector3 LocalPointA { get { return _localPointA; } set { _localPointA = value; } } | ||
69 | public Vector3 LocalPointB { get { return _localPointB; } set { _localPointB = value; } } | ||
70 | |||
71 | public Vector3 NormalWorldOnB { get { return _normalWorldOnB; } } | ||
72 | |||
73 | public float CombinedFriction { get { return _combinedFriction; } set { _combinedFriction = value; } } | ||
74 | public float CombinedRestitution { get { return _combinedRestitution; } set { _combinedRestitution = value; } } | ||
75 | |||
76 | public object UserPersistentData { get { return _userPersistentData; } set { _userPersistentData = value; } } | ||
77 | } | ||
78 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs new file mode 100644 index 0000000..81b05b8 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs | |||
@@ -0,0 +1,246 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// 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. | ||
32 | /// </summary> | ||
33 | public class MinkowskiPenetrationDepthSolver : IConvexPenetrationDepthSolver | ||
34 | { | ||
35 | private const int UnitSpherePointsCount = 42; | ||
36 | |||
37 | private static Vector3[] penetrationDirections = | ||
38 | { | ||
39 | new Vector3(0.000000f , -0.000000f,-1.000000f), | ||
40 | new Vector3(0.723608f , -0.525725f,-0.447219f), | ||
41 | new Vector3(-0.276388f , -0.850649f,-0.447219f), | ||
42 | new Vector3(-0.894426f , -0.000000f,-0.447216f), | ||
43 | new Vector3(-0.276388f , 0.850649f,-0.447220f), | ||
44 | new Vector3(0.723608f , 0.525725f,-0.447219f), | ||
45 | new Vector3(0.276388f , -0.850649f,0.447220f), | ||
46 | 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), | ||
49 | new Vector3(0.894426f , 0.000000f,0.447216f), | ||
50 | new Vector3(-0.000000f , 0.000000f,1.000000f), | ||
51 | new Vector3(0.425323f , -0.309011f,-0.850654f), | ||
52 | new Vector3(-0.162456f , -0.499995f,-0.850654f), | ||
53 | new Vector3(0.262869f , -0.809012f,-0.525738f), | ||
54 | new Vector3(0.425323f , 0.309011f,-0.850654f), | ||
55 | new Vector3(0.850648f , -0.000000f,-0.525736f), | ||
56 | new Vector3(-0.525730f , -0.000000f,-0.850652f), | ||
57 | new Vector3(-0.688190f , -0.499997f,-0.525736f), | ||
58 | new Vector3(-0.162456f , 0.499995f,-0.850654f), | ||
59 | new Vector3(-0.688190f , 0.499997f,-0.525736f), | ||
60 | new Vector3(0.262869f , 0.809012f,-0.525738f), | ||
61 | 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), | ||
64 | new Vector3(0.000000f , -1.000000f,0.000000f), | ||
65 | new Vector3(-0.587786f , -0.809017f,0.000000f), | ||
66 | 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), | ||
69 | new Vector3(-0.000000f , 1.000000f,-0.000000f), | ||
70 | new Vector3(0.587786f , 0.809017f,-0.000000f), | ||
71 | new Vector3(0.688190f , -0.499997f,0.525736f), | ||
72 | new Vector3(-0.262869f , -0.809012f,0.525738f), | ||
73 | new Vector3(-0.850648f , 0.000000f,0.525736f), | ||
74 | new Vector3(-0.262869f , 0.809012f,0.525738f), | ||
75 | new Vector3(0.688190f , 0.499997f,0.525736f), | ||
76 | new Vector3(0.525730f , 0.000000f,0.850652f), | ||
77 | new Vector3(0.162456f , -0.499995f,0.850654f), | ||
78 | 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) | ||
81 | }; | ||
82 | |||
83 | private class IntermediateResult : DiscreteCollisionDetectorInterface.Result | ||
84 | { | ||
85 | private Vector3 _normalOnBInWorld; | ||
86 | private Vector3 _pointInWorld; | ||
87 | private float _depth; | ||
88 | private bool _hasResult; | ||
89 | |||
90 | public IntermediateResult() | ||
91 | { | ||
92 | _hasResult = false; | ||
93 | } | ||
94 | |||
95 | public bool HasResult { get { return _hasResult; } } | ||
96 | public float Depth { get { return _depth; } } | ||
97 | public Vector3 PointInWorld { get { return _pointInWorld; } } | ||
98 | |||
99 | public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1) | ||
100 | { | ||
101 | } | ||
102 | |||
103 | public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) | ||
104 | { | ||
105 | _normalOnBInWorld = normalOnBInWorld; | ||
106 | _pointInWorld = pointInWorld; | ||
107 | _depth = depth; | ||
108 | _hasResult = true; | ||
109 | } | ||
110 | } | ||
111 | |||
112 | #region IConvexPenetrationDepthSolver Members | ||
113 | public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, | ||
114 | ConvexShape convexA, ConvexShape convexB, | ||
115 | Matrix transformA, Matrix transformB, | ||
116 | Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw) | ||
117 | { | ||
118 | pa = new Vector3(); | ||
119 | pb = new Vector3(); | ||
120 | //just take fixed number of orientation, and sample the penetration depth in that direction | ||
121 | float minProj = 1e30f; | ||
122 | Vector3 minNorm = new Vector3(); | ||
123 | Vector3 minA = new Vector3(), minB = new Vector3(); | ||
124 | Vector3 seperatingAxisInA, seperatingAxisInB; | ||
125 | Vector3 pInA, qInB, pWorld, qWorld, w; | ||
126 | |||
127 | Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; | ||
128 | Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; | ||
129 | Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; | ||
130 | Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; | ||
131 | |||
132 | int numSampleDirections = UnitSpherePointsCount; | ||
133 | |||
134 | for (int i = 0; i < numSampleDirections; i++) | ||
135 | { | ||
136 | Vector3 norm = penetrationDirections[i]; | ||
137 | seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA); | ||
138 | seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB); | ||
139 | } | ||
140 | |||
141 | { | ||
142 | int numPDA = convexA.PreferredPenetrationDirectionsCount; | ||
143 | if (numPDA != 0) | ||
144 | { | ||
145 | for (int i = 0; i < numPDA; i++) | ||
146 | { | ||
147 | Vector3 norm; | ||
148 | convexA.GetPreferredPenetrationDirection(i, out norm); | ||
149 | norm = Vector3.TransformNormal(norm, transformA); | ||
150 | penetrationDirections[numSampleDirections] = norm; | ||
151 | seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); | ||
152 | seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); | ||
153 | numSampleDirections++; | ||
154 | } | ||
155 | } | ||
156 | } | ||
157 | |||
158 | { | ||
159 | int numPDB = convexB.PreferredPenetrationDirectionsCount; | ||
160 | if (numPDB != 0) | ||
161 | { | ||
162 | for (int i = 0; i < numPDB; i++) | ||
163 | { | ||
164 | Vector3 norm; | ||
165 | convexB.GetPreferredPenetrationDirection(i, out norm); | ||
166 | norm = Vector3.TransformNormal(norm, transformB); | ||
167 | penetrationDirections[numSampleDirections] = norm; | ||
168 | seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); | ||
169 | seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); | ||
170 | numSampleDirections++; | ||
171 | } | ||
172 | } | ||
173 | } | ||
174 | |||
175 | convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections); | ||
176 | convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections); | ||
177 | |||
178 | for (int i = 0; i < numSampleDirections; i++) | ||
179 | { | ||
180 | Vector3 norm = penetrationDirections[i]; | ||
181 | seperatingAxisInA = seperatingAxisInABatch[i]; | ||
182 | seperatingAxisInB = seperatingAxisInBBatch[i]; | ||
183 | |||
184 | pInA = supportVerticesABatch[i]; | ||
185 | qInB = supportVerticesBBatch[i]; | ||
186 | |||
187 | pWorld = MathHelper.MatrixToVector(transformA, pInA); | ||
188 | qWorld = MathHelper.MatrixToVector(transformB, qInB); | ||
189 | w = qWorld - pWorld; | ||
190 | float delta = Vector3.Dot(norm, w); | ||
191 | //find smallest delta | ||
192 | if (delta < minProj) | ||
193 | { | ||
194 | minProj = delta; | ||
195 | minNorm = norm; | ||
196 | minA = pWorld; | ||
197 | minB = qWorld; | ||
198 | } | ||
199 | } | ||
200 | |||
201 | //add the margins | ||
202 | minA += minNorm * convexA.Margin; | ||
203 | minB -= minNorm * convexB.Margin; | ||
204 | //no penetration | ||
205 | if (minProj < 0) | ||
206 | return false; | ||
207 | |||
208 | minProj += (convexA.Margin + convexB.Margin); | ||
209 | |||
210 | GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null); | ||
211 | |||
212 | float offsetDist = minProj; | ||
213 | Vector3 offset = minNorm * offsetDist; | ||
214 | |||
215 | GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | ||
216 | |||
217 | Vector3 newOrg = transformA.Translation + offset; | ||
218 | |||
219 | Matrix displacedTrans = transformA; | ||
220 | displacedTrans.Translation = newOrg; | ||
221 | |||
222 | input.TransformA = displacedTrans; | ||
223 | input.TransformB = transformB; | ||
224 | input.MaximumDistanceSquared = 1e30f;//minProj; | ||
225 | |||
226 | IntermediateResult res = new IntermediateResult(); | ||
227 | gjkdet.GetClosestPoints(input, res, debugDraw); | ||
228 | |||
229 | float correctedMinNorm = minProj - res.Depth; | ||
230 | |||
231 | //the penetration depth is over-estimated, relax it | ||
232 | float penetration_relaxation = 1; | ||
233 | minNorm *= penetration_relaxation; | ||
234 | |||
235 | if (res.HasResult) | ||
236 | { | ||
237 | |||
238 | pa = res.PointInWorld - minNorm * correctedMinNorm; | ||
239 | pb = res.PointInWorld; | ||
240 | } | ||
241 | |||
242 | return res.HasResult; | ||
243 | } | ||
244 | #endregion | ||
245 | } | ||
246 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs new file mode 100644 index 0000000..47cfe9a --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs | |||
@@ -0,0 +1,272 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public delegate bool ContactDestroyedCallback(object userPersistentData); | ||
30 | |||
31 | public class PersistentManifold | ||
32 | { | ||
33 | private static ContactDestroyedCallback _contactDestroyedCallback = null; | ||
34 | private static float _contactBreakingThreshold = 0.02f; | ||
35 | |||
36 | private ManifoldPoint[] _pointCache = new ManifoldPoint[4]; | ||
37 | |||
38 | // this two body pointers can point to the physics rigidbody class. | ||
39 | // object will allow any rigidbody class | ||
40 | private object _bodyA; | ||
41 | private object _bodyB; | ||
42 | private int _cachedPoints; | ||
43 | |||
44 | public PersistentManifold(object bodyA, object bodyB) | ||
45 | { | ||
46 | _bodyA = bodyA; | ||
47 | _bodyB = bodyB; | ||
48 | _cachedPoints = 0; | ||
49 | } | ||
50 | |||
51 | public object BodyA { get { return _bodyA; } } | ||
52 | public object BodyB { get { return _bodyB; } } | ||
53 | |||
54 | public int ContactsCount { get { return _cachedPoints; } } | ||
55 | |||
56 | public static ContactDestroyedCallback ContactDestroyedCallback { get { return _contactDestroyedCallback; } set { _contactDestroyedCallback = value; } } | ||
57 | public static float ContactBreakingThreshold { get { return _contactBreakingThreshold; } } | ||
58 | |||
59 | public void SetBodies(object bodyA, object bodyB) | ||
60 | { | ||
61 | _bodyA = bodyA; | ||
62 | _bodyB = bodyB; | ||
63 | } | ||
64 | |||
65 | public ManifoldPoint GetContactPoint(int index) | ||
66 | { | ||
67 | if (index >= _cachedPoints) | ||
68 | throw new ArgumentOutOfRangeException("index", "index must be smaller than cachedPoints"); | ||
69 | |||
70 | return _pointCache[index]; | ||
71 | } | ||
72 | |||
73 | public int GetCacheEntry(ManifoldPoint newPoint) | ||
74 | { | ||
75 | float shortestDist = ContactBreakingThreshold * ContactBreakingThreshold; | ||
76 | int size = ContactsCount; | ||
77 | int nearestPoint = -1; | ||
78 | for (int i = 0; i < size; i++) | ||
79 | { | ||
80 | ManifoldPoint mp = _pointCache[i]; | ||
81 | |||
82 | Vector3 diffA = mp.LocalPointA - newPoint.LocalPointA; | ||
83 | float distToManiPoint = Vector3.Dot(diffA, diffA); | ||
84 | if (distToManiPoint < shortestDist) | ||
85 | { | ||
86 | shortestDist = distToManiPoint; | ||
87 | nearestPoint = i; | ||
88 | } | ||
89 | } | ||
90 | return nearestPoint; | ||
91 | } | ||
92 | |||
93 | public void AddManifoldPoint(ManifoldPoint newPoint) | ||
94 | { | ||
95 | if (!ValidContactDistance(newPoint)) | ||
96 | throw new BulletException(); | ||
97 | |||
98 | int insertIndex = ContactsCount; | ||
99 | if (insertIndex == 4) | ||
100 | { | ||
101 | //sort cache so best points come first, based on area | ||
102 | insertIndex = SortCachedPoints(newPoint); | ||
103 | } | ||
104 | else | ||
105 | { | ||
106 | _cachedPoints++; | ||
107 | } | ||
108 | ReplaceContactPoint(newPoint, insertIndex); | ||
109 | } | ||
110 | |||
111 | public void RemoveContactPoint(int index) | ||
112 | { | ||
113 | ClearUserCache(_pointCache[index]); | ||
114 | |||
115 | int lastUsedIndex = ContactsCount - 1; | ||
116 | _pointCache[index] = _pointCache[lastUsedIndex]; | ||
117 | //get rid of duplicated userPersistentData pointer | ||
118 | _pointCache[lastUsedIndex].UserPersistentData = null; | ||
119 | _cachedPoints--; | ||
120 | } | ||
121 | |||
122 | public void ReplaceContactPoint(ManifoldPoint newPoint, int insertIndex) | ||
123 | { | ||
124 | BulletDebug.Assert(ValidContactDistance(newPoint)); | ||
125 | |||
126 | if (_pointCache[insertIndex] != null) | ||
127 | { | ||
128 | int lifeTime = _pointCache[insertIndex].LifeTime; | ||
129 | BulletDebug.Assert(lifeTime >= 0); | ||
130 | object cache = _pointCache[insertIndex].UserPersistentData; | ||
131 | |||
132 | _pointCache[insertIndex] = newPoint; | ||
133 | |||
134 | _pointCache[insertIndex].UserPersistentData = cache; | ||
135 | _pointCache[insertIndex].LifeTime = lifeTime; | ||
136 | } | ||
137 | else | ||
138 | { | ||
139 | _pointCache[insertIndex] = newPoint; | ||
140 | } | ||
141 | |||
142 | //ClearUserCache(_pointCache[insertIndex]); | ||
143 | //_pointCache[insertIndex] = newPoint; | ||
144 | } | ||
145 | |||
146 | public bool ValidContactDistance(ManifoldPoint pt) | ||
147 | { | ||
148 | return pt.Distance <= ContactBreakingThreshold; | ||
149 | } | ||
150 | |||
151 | // calculated new worldspace coordinates and depth, and reject points that exceed the collision margin | ||
152 | public void RefreshContactPoints(Matrix trA, Matrix trB) | ||
153 | { | ||
154 | // first refresh worldspace positions and distance | ||
155 | for (int i = ContactsCount - 1; i >= 0; i--) | ||
156 | { | ||
157 | ManifoldPoint manifoldPoint = _pointCache[i]; | ||
158 | manifoldPoint.PositionWorldOnA = MathHelper.MatrixToVector(trA,manifoldPoint.LocalPointA); | ||
159 | manifoldPoint.PositionWorldOnB = MathHelper.MatrixToVector(trB, manifoldPoint.LocalPointB); | ||
160 | manifoldPoint.Distance = Vector3.Dot(manifoldPoint.PositionWorldOnA - manifoldPoint.PositionWorldOnB, manifoldPoint.NormalWorldOnB); | ||
161 | manifoldPoint.LifeTime++; | ||
162 | } | ||
163 | |||
164 | // then | ||
165 | float distance2d; | ||
166 | Vector3 projectedDifference, projectedPoint; | ||
167 | for (int i = ContactsCount - 1; i >= 0; i--) | ||
168 | { | ||
169 | |||
170 | ManifoldPoint manifoldPoint = _pointCache[i]; | ||
171 | //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction) | ||
172 | if (!ValidContactDistance(manifoldPoint)) | ||
173 | { | ||
174 | RemoveContactPoint(i); | ||
175 | } | ||
176 | else | ||
177 | { | ||
178 | //contact also becomes invalid when relative movement orthogonal to normal exceeds margin | ||
179 | projectedPoint = manifoldPoint.PositionWorldOnA - manifoldPoint.NormalWorldOnB * manifoldPoint.Distance; | ||
180 | projectedDifference = manifoldPoint.PositionWorldOnB - projectedPoint; | ||
181 | distance2d = Vector3.Dot(projectedDifference, projectedDifference); | ||
182 | if (distance2d > ContactBreakingThreshold * ContactBreakingThreshold) | ||
183 | { | ||
184 | RemoveContactPoint(i); | ||
185 | } | ||
186 | } | ||
187 | } | ||
188 | } | ||
189 | |||
190 | public void ClearManifold() | ||
191 | { | ||
192 | for (int i = 0; i < _cachedPoints; i++) | ||
193 | { | ||
194 | ClearUserCache(_pointCache[i]); | ||
195 | } | ||
196 | _cachedPoints = 0; | ||
197 | } | ||
198 | |||
199 | private void ClearUserCache(ManifoldPoint pt) | ||
200 | { | ||
201 | if (pt != null) | ||
202 | { | ||
203 | object oldPtr = pt.UserPersistentData; | ||
204 | |||
205 | if (oldPtr != null) | ||
206 | { | ||
207 | if (pt.UserPersistentData != null && _contactDestroyedCallback != null) | ||
208 | { | ||
209 | _contactDestroyedCallback(pt.UserPersistentData); | ||
210 | pt.UserPersistentData = null; | ||
211 | } | ||
212 | } | ||
213 | } | ||
214 | } | ||
215 | |||
216 | // sort cached points so most isolated points come first | ||
217 | private int SortCachedPoints(ManifoldPoint pt) | ||
218 | { | ||
219 | //calculate 4 possible cases areas, and take biggest area | ||
220 | //also need to keep 'deepest' | ||
221 | |||
222 | int maxPenetrationIndex = -1; | ||
223 | float maxPenetration = pt.Distance; | ||
224 | for (int i = 0; i < 4; i++) | ||
225 | { | ||
226 | if (_pointCache[i].Distance < maxPenetration) | ||
227 | { | ||
228 | maxPenetrationIndex = i; | ||
229 | maxPenetration = _pointCache[i].Distance; | ||
230 | } | ||
231 | } | ||
232 | |||
233 | float res0 = 0, res1 = 0, res2 = 0, res3 = 0; | ||
234 | if (maxPenetrationIndex != 0) | ||
235 | { | ||
236 | Vector3 a0 = pt.LocalPointA - _pointCache[1].LocalPointA; | ||
237 | Vector3 b0 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; | ||
238 | Vector3 cross = Vector3.Cross(a0, b0); | ||
239 | res0 = cross.LengthSquared(); | ||
240 | } | ||
241 | if (maxPenetrationIndex != 1) | ||
242 | { | ||
243 | Vector3 a1 = pt.LocalPointA - _pointCache[0].LocalPointA; | ||
244 | Vector3 b1 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; | ||
245 | Vector3 cross = Vector3.Cross(a1, b1); | ||
246 | res1 = cross.LengthSquared(); | ||
247 | } | ||
248 | |||
249 | if (maxPenetrationIndex != 2) | ||
250 | { | ||
251 | Vector3 a2 = pt.LocalPointA - _pointCache[0].LocalPointA; | ||
252 | Vector3 b2 = _pointCache[3].LocalPointA - _pointCache[1].LocalPointA; | ||
253 | Vector3 cross = Vector3.Cross(a2, b2); | ||
254 | res2 = cross.LengthSquared(); | ||
255 | } | ||
256 | |||
257 | if (maxPenetrationIndex != 3) | ||
258 | { | ||
259 | Vector3 a3 = pt.LocalPointA - _pointCache[0].LocalPointA; | ||
260 | Vector3 b3 = _pointCache[2].LocalPointA - _pointCache[1].LocalPointA; | ||
261 | Vector3 cross = Vector3.Cross(a3, b3); | ||
262 | res3 = cross.LengthSquared(); | ||
263 | } | ||
264 | |||
265 | Vector4 maxvec = new Vector4(res0, res1, res2, res3); | ||
266 | int biggestarea = MathHelper.ClosestAxis(maxvec); | ||
267 | return biggestarea; | ||
268 | } | ||
269 | |||
270 | private int FindContactPoint(ManifoldPoint unUsed, int numUnused, ManifoldPoint pt) { return 0; } | ||
271 | } | ||
272 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs new file mode 100644 index 0000000..4e0b0a0 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs | |||
@@ -0,0 +1,64 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class PointCollector : DiscreteCollisionDetectorInterface.Result | ||
30 | { | ||
31 | private Vector3 _normalOnBInWorld; | ||
32 | private Vector3 _pointInWorld; | ||
33 | private float _distance; //negative means penetration | ||
34 | private bool _hasResult; | ||
35 | |||
36 | public PointCollector() | ||
37 | { | ||
38 | _distance = 1e30f; | ||
39 | _hasResult = false; | ||
40 | } | ||
41 | |||
42 | public Vector3 NormalOnBInWorld { get { return _normalOnBInWorld; } } | ||
43 | public Vector3 PointInWorld { get { return _pointInWorld; } } | ||
44 | public float Distance { get { return _distance; } } | ||
45 | public bool HasResult { get { return _hasResult; } } | ||
46 | |||
47 | public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB) | ||
48 | { | ||
49 | //?? | ||
50 | } | ||
51 | |||
52 | public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) | ||
53 | { | ||
54 | if (depth < _distance) | ||
55 | { | ||
56 | _hasResult = true; | ||
57 | _normalOnBInWorld = normalOnBInWorld; | ||
58 | _pointInWorld = pointInWorld; | ||
59 | //negative means penetration | ||
60 | _distance = depth; | ||
61 | } | ||
62 | } | ||
63 | } | ||
64 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs new file mode 100644 index 0000000..6d320c4 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs | |||
@@ -0,0 +1,142 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// SubsimplexConvexCast implements Gino van den Bergens' paper | ||
31 | /// "Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection" | ||
32 | /// GJK based Ray Cast, optimized version | ||
33 | /// Objects should not start in overlap, otherwise results are not defined. | ||
34 | /// </summary> | ||
35 | public class SubsimplexConvexCast : IConvexCast | ||
36 | { | ||
37 | private ISimplexSolver _simplexSolver; | ||
38 | private ConvexShape _convexA; | ||
39 | private ConvexShape _convexB; | ||
40 | |||
41 | /// <summary> | ||
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 | ||
44 | /// </summary> | ||
45 | private const int MaxIterations = 32; | ||
46 | |||
47 | public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolver simplexSolver) | ||
48 | { | ||
49 | _simplexSolver = simplexSolver; | ||
50 | _convexA = shapeA; | ||
51 | _convexB = shapeB; | ||
52 | } | ||
53 | |||
54 | #region IConvexCast Members | ||
55 | /// <summary> | ||
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. | ||
58 | /// </summary> | ||
59 | /// <param name="fromA"></param> | ||
60 | /// <param name="toA"></param> | ||
61 | /// <param name="fromB"></param> | ||
62 | /// <param name="toB"></param> | ||
63 | /// <param name="result"></param> | ||
64 | /// <returns></returns> | ||
65 | public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) | ||
66 | { | ||
67 | MinkowskiSumShape convex = new MinkowskiSumShape(_convexA, _convexB); | ||
68 | |||
69 | Matrix rayFromLocalA; | ||
70 | Matrix rayToLocalA; | ||
71 | |||
72 | rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB; | ||
73 | rayToLocalA = MathHelper.InvertMatrix(toA) * toB; | ||
74 | |||
75 | _simplexSolver.Reset(); | ||
76 | |||
77 | convex.TransformB = rayFromLocalA; | ||
78 | |||
79 | float lambda = 0; | ||
80 | //todo: need to verify this: | ||
81 | //because of minkowski difference, we need the inverse direction | ||
82 | |||
83 | Vector3 s = -rayFromLocalA.Translation; | ||
84 | Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation); | ||
85 | Vector3 x = s; | ||
86 | Vector3 v; | ||
87 | Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(r); | ||
88 | |||
89 | v = x - arbitraryPoint; | ||
90 | |||
91 | int maxIter = MaxIterations; | ||
92 | |||
93 | Vector3 n = new Vector3(); | ||
94 | float lastLambda = lambda; | ||
95 | |||
96 | float dist2 = v.LengthSquared(); | ||
97 | float epsilon = 0.0001f; | ||
98 | |||
99 | Vector3 w, p; | ||
100 | float VdotR; | ||
101 | |||
102 | while ((dist2 > epsilon) && (maxIter-- != 0)) | ||
103 | { | ||
104 | p = convex.LocalGetSupportingVertex(v); | ||
105 | w = x - p; | ||
106 | |||
107 | float VdotW = Vector3.Dot(v, w); | ||
108 | |||
109 | if (VdotW > 0) | ||
110 | { | ||
111 | VdotR = Vector3.Dot(v, r); | ||
112 | |||
113 | if (VdotR >= -(MathHelper.Epsilon * MathHelper.Epsilon)) | ||
114 | return false; | ||
115 | else | ||
116 | { | ||
117 | lambda = lambda - VdotW / VdotR; | ||
118 | x = s + lambda * r; | ||
119 | _simplexSolver.Reset(); | ||
120 | //check next line | ||
121 | w = x - p; | ||
122 | lastLambda = lambda; | ||
123 | n = v; | ||
124 | } | ||
125 | } | ||
126 | _simplexSolver.AddVertex(w, x, p); | ||
127 | if (_simplexSolver.Closest(out v)) | ||
128 | { | ||
129 | dist2 = v.LengthSquared(); | ||
130 | } | ||
131 | else | ||
132 | { | ||
133 | dist2 = 0f; | ||
134 | } | ||
135 | } | ||
136 | result.Fraction = lambda; | ||
137 | result.Normal = n; | ||
138 | return true; | ||
139 | } | ||
140 | #endregion | ||
141 | } | ||
142 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs new file mode 100644 index 0000000..c127460 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs | |||
@@ -0,0 +1,115 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public abstract class TriangleRaycastCallback : ITriangleCallback | ||
30 | { | ||
31 | private Vector3 _from; | ||
32 | private Vector3 _to; | ||
33 | private float _hitFraction; | ||
34 | |||
35 | public TriangleRaycastCallback(Vector3 from, Vector3 to) | ||
36 | { | ||
37 | _from = from; | ||
38 | _to = to; | ||
39 | _hitFraction = 1; | ||
40 | } | ||
41 | |||
42 | public Vector3 From { get { return _from; } set { _from = value; } } | ||
43 | public Vector3 To { get { return _to; } set { _to = value; } } | ||
44 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } | ||
45 | |||
46 | public abstract float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex); | ||
47 | |||
48 | #region ITriangleCallback Members | ||
49 | |||
50 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) | ||
51 | { | ||
52 | Vector3 vertA = triangle[0]; | ||
53 | Vector3 vertB = triangle[1]; | ||
54 | Vector3 vertC = triangle[2]; | ||
55 | |||
56 | Vector3 vBA = vertB - vertA; | ||
57 | Vector3 vCA = vertC - vertA; | ||
58 | |||
59 | Vector3 triangleNormal = Vector3.Cross(vBA, vCA); | ||
60 | |||
61 | float dist = Vector3.Dot(vertA, triangleNormal); | ||
62 | float distA = Vector3.Dot(triangleNormal, _from); | ||
63 | distA -= dist; | ||
64 | float distB = Vector3.Dot(triangleNormal, _to); | ||
65 | distB -= dist; | ||
66 | |||
67 | if (distA * distB >= 0.0f) | ||
68 | { | ||
69 | return; // same sign | ||
70 | } | ||
71 | |||
72 | float projLength = distA - distB; | ||
73 | float distance = (distA) / (projLength); | ||
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, | ||
76 | // in case the ray hits exacly on the edge of the triangle. | ||
77 | // It must be scaled for the triangle size. | ||
78 | |||
79 | if (distance < _hitFraction) | ||
80 | { | ||
81 | float edgeTolerance = triangleNormal.LengthSquared(); | ||
82 | edgeTolerance *= -0.0001f; | ||
83 | Vector3 point = new Vector3(); | ||
84 | MathHelper.SetInterpolate3(_from, _to, distance, ref point); | ||
85 | |||
86 | Vector3 vertexAPoint = vertA - point; | ||
87 | Vector3 vertexBPoint = vertB - point; | ||
88 | Vector3 contactPointA = Vector3.Cross(vertexAPoint, vertexBPoint); | ||
89 | |||
90 | if (Vector3.Dot(contactPointA, triangleNormal) >= edgeTolerance) | ||
91 | { | ||
92 | Vector3 vertexCPoint = vertC - point; | ||
93 | Vector3 contactPointB = Vector3.Cross(vertexBPoint, vertexCPoint); | ||
94 | if (Vector3.Dot(contactPointB, triangleNormal) >= edgeTolerance) | ||
95 | { | ||
96 | Vector3 contactPointC = Vector3.Cross(vertexCPoint, vertexAPoint); | ||
97 | |||
98 | if (Vector3.Dot(contactPointC, triangleNormal) >= edgeTolerance) | ||
99 | { | ||
100 | if (distA > 0) | ||
101 | { | ||
102 | _hitFraction = ReportHit(triangleNormal, distance, partID, triangleIndex); | ||
103 | } | ||
104 | else | ||
105 | { | ||
106 | _hitFraction = ReportHit(-triangleNormal, distance, partID, triangleIndex); | ||
107 | } | ||
108 | } | ||
109 | } | ||
110 | } | ||
111 | } | ||
112 | } | ||
113 | #endregion | ||
114 | } | ||
115 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs new file mode 100644 index 0000000..16f3dab --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs | |||
@@ -0,0 +1,643 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class UsageBitfield | ||
30 | { | ||
31 | private bool _usedVertexA, _usedVertexB, _usedVertexC, _usedVertexD; | ||
32 | |||
33 | public bool UsedVertexA { get { return _usedVertexA; } set { _usedVertexA = value; } } | ||
34 | public bool UsedVertexB { get { return _usedVertexB; } set { _usedVertexB = value; } } | ||
35 | public bool UsedVertexC { get { return _usedVertexC; } set { _usedVertexC = value; } } | ||
36 | public bool UsedVertexD { get { return _usedVertexD; } set { _usedVertexD = value; } } | ||
37 | |||
38 | public void Reset() | ||
39 | { | ||
40 | _usedVertexA = _usedVertexB = _usedVertexC = _usedVertexD = false; | ||
41 | } | ||
42 | } | ||
43 | |||
44 | public class SubSimplexClosestResult | ||
45 | { | ||
46 | private Vector3 _closestPointOnSimplex; | ||
47 | |||
48 | //MASK for m_usedVertices | ||
49 | //stores the simplex vertex-usage, using the MASK, | ||
50 | // if m_usedVertices & MASK then the related vertex is used | ||
51 | private UsageBitfield _usedVertices = new UsageBitfield(); | ||
52 | private float[] _barycentricCoords = new float[4]; | ||
53 | private bool _degenerate; | ||
54 | |||
55 | public Vector3 ClosestPointOnSimplex { get { return _closestPointOnSimplex; } set { _closestPointOnSimplex = value; } } | ||
56 | public UsageBitfield UsedVertices { get { return _usedVertices; } set { _usedVertices = value; } } | ||
57 | public float[] BarycentricCoords { get { return _barycentricCoords; } set { _barycentricCoords = value; } } | ||
58 | public bool Degenerate { get { return _degenerate; } set { _degenerate = value; } } | ||
59 | |||
60 | public void Reset() | ||
61 | { | ||
62 | _degenerate = false; | ||
63 | SetBarycentricCoordinates(); | ||
64 | _usedVertices.Reset(); | ||
65 | } | ||
66 | |||
67 | public bool IsValid | ||
68 | { | ||
69 | get | ||
70 | { | ||
71 | return (_barycentricCoords[0] >= 0f) && | ||
72 | (_barycentricCoords[1] >= 0f) && | ||
73 | (_barycentricCoords[2] >= 0f) && | ||
74 | (_barycentricCoords[3] >= 0f); | ||
75 | } | ||
76 | } | ||
77 | |||
78 | public void SetBarycentricCoordinates() | ||
79 | { | ||
80 | SetBarycentricCoordinates(0f, 0f, 0f, 0f); | ||
81 | } | ||
82 | |||
83 | public void SetBarycentricCoordinates(float a, float b, float c, float d) | ||
84 | { | ||
85 | _barycentricCoords[0] = a; | ||
86 | _barycentricCoords[1] = b; | ||
87 | _barycentricCoords[2] = c; | ||
88 | _barycentricCoords[3] = d; | ||
89 | } | ||
90 | } | ||
91 | |||
92 | /// VoronoiSimplexSolver is an implementation of the closest point distance | ||
93 | /// algorithm from a 1-4 points simplex to the origin. | ||
94 | /// Can be used with GJK, as an alternative to Johnson distance algorithm. | ||
95 | public class VoronoiSimplexSolver : ISimplexSolver | ||
96 | { | ||
97 | private const int VertexA = 0, VertexB = 1, VertexC = 2, VertexD = 3; | ||
98 | |||
99 | private const int VoronoiSimplexMaxVerts = 5; | ||
100 | private const bool CatchDegenerateTetrahedron = true; | ||
101 | |||
102 | private int _numVertices; | ||
103 | |||
104 | private Vector3[] _simplexVectorW = new Vector3[VoronoiSimplexMaxVerts]; | ||
105 | private Vector3[] _simplexPointsP = new Vector3[VoronoiSimplexMaxVerts]; | ||
106 | private Vector3[] _simplexPointsQ = new Vector3[VoronoiSimplexMaxVerts]; | ||
107 | |||
108 | private Vector3 _cachedPA; | ||
109 | private Vector3 _cachedPB; | ||
110 | private Vector3 _cachedV; | ||
111 | private Vector3 _lastW; | ||
112 | private bool _cachedValidClosest; | ||
113 | |||
114 | private SubSimplexClosestResult _cachedBC = new SubSimplexClosestResult(); | ||
115 | |||
116 | private bool _needsUpdate; | ||
117 | |||
118 | #region ISimplexSolver Members | ||
119 | |||
120 | public bool FullSimplex | ||
121 | { | ||
122 | get | ||
123 | { | ||
124 | return _numVertices == 4; | ||
125 | } | ||
126 | } | ||
127 | |||
128 | public int NumVertices | ||
129 | { | ||
130 | get | ||
131 | { | ||
132 | return _numVertices; | ||
133 | } | ||
134 | } | ||
135 | |||
136 | public void Reset() | ||
137 | { | ||
138 | _cachedValidClosest = false; | ||
139 | _numVertices = 0; | ||
140 | _needsUpdate = true; | ||
141 | _lastW = new Vector3(1e30f, 1e30f, 1e30f); | ||
142 | _cachedBC.Reset(); | ||
143 | } | ||
144 | |||
145 | public void AddVertex(Vector3 w, Vector3 p, Vector3 q) | ||
146 | { | ||
147 | _lastW = w; | ||
148 | _needsUpdate = true; | ||
149 | |||
150 | _simplexVectorW[_numVertices] = w; | ||
151 | _simplexPointsP[_numVertices] = p; | ||
152 | _simplexPointsQ[_numVertices] = q; | ||
153 | |||
154 | _numVertices++; | ||
155 | } | ||
156 | |||
157 | //return/calculate the closest vertex | ||
158 | public bool Closest(out Vector3 v) | ||
159 | { | ||
160 | bool succes = UpdateClosestVectorAndPoints(); | ||
161 | v = _cachedV; | ||
162 | return succes; | ||
163 | } | ||
164 | |||
165 | public float MaxVertex | ||
166 | { | ||
167 | get | ||
168 | { | ||
169 | int numverts = NumVertices; | ||
170 | float maxV = 0f, curLen2; | ||
171 | for (int i = 0; i < numverts; i++) | ||
172 | { | ||
173 | curLen2 = _simplexVectorW[i].LengthSquared(); | ||
174 | if (maxV < curLen2) maxV = curLen2; | ||
175 | } | ||
176 | return maxV; | ||
177 | } | ||
178 | } | ||
179 | |||
180 | //return the current simplex | ||
181 | public int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf) | ||
182 | { | ||
183 | int numverts = NumVertices; | ||
184 | pBuf = new Vector3[numverts]; | ||
185 | qBuf = new Vector3[numverts]; | ||
186 | yBuf = new Vector3[numverts]; | ||
187 | for (int i = 0; i < numverts; i++) | ||
188 | { | ||
189 | yBuf[i] = _simplexVectorW[i]; | ||
190 | pBuf[i] = _simplexPointsP[i]; | ||
191 | qBuf[i] = _simplexPointsQ[i]; | ||
192 | } | ||
193 | return numverts; | ||
194 | } | ||
195 | |||
196 | public bool InSimplex(Vector3 w) | ||
197 | { | ||
198 | //check in case lastW is already removed | ||
199 | if (w == _lastW) return true; | ||
200 | |||
201 | //w is in the current (reduced) simplex | ||
202 | int numverts = NumVertices; | ||
203 | for (int i = 0; i < numverts; i++) | ||
204 | if (_simplexVectorW[i] == w) return true; | ||
205 | |||
206 | return false; | ||
207 | } | ||
208 | |||
209 | public void BackupClosest(out Vector3 v) | ||
210 | { | ||
211 | v = _cachedV; | ||
212 | } | ||
213 | |||
214 | public bool EmptySimplex | ||
215 | { | ||
216 | get | ||
217 | { | ||
218 | return NumVertices == 0; | ||
219 | } | ||
220 | } | ||
221 | |||
222 | public void ComputePoints(out Vector3 p1, out Vector3 p2) | ||
223 | { | ||
224 | UpdateClosestVectorAndPoints(); | ||
225 | p1 = _cachedPA; | ||
226 | p2 = _cachedPB; | ||
227 | } | ||
228 | |||
229 | #endregion | ||
230 | |||
231 | public void RemoveVertex(int index) | ||
232 | { | ||
233 | BulletDebug.Assert(_numVertices > 0); | ||
234 | _numVertices--; | ||
235 | _simplexVectorW[index] = _simplexVectorW[_numVertices]; | ||
236 | _simplexPointsP[index] = _simplexPointsP[_numVertices]; | ||
237 | _simplexPointsQ[index] = _simplexPointsQ[_numVertices]; | ||
238 | } | ||
239 | |||
240 | public void ReduceVertices(UsageBitfield usedVerts) | ||
241 | { | ||
242 | if ((NumVertices >= 4) && (!usedVerts.UsedVertexD)) RemoveVertex(3); | ||
243 | if ((NumVertices >= 3) && (!usedVerts.UsedVertexC)) RemoveVertex(2); | ||
244 | if ((NumVertices >= 2) && (!usedVerts.UsedVertexB)) RemoveVertex(1); | ||
245 | if ((NumVertices >= 1) && (!usedVerts.UsedVertexA)) RemoveVertex(0); | ||
246 | } | ||
247 | |||
248 | public bool UpdateClosestVectorAndPoints() | ||
249 | { | ||
250 | if (_needsUpdate) | ||
251 | { | ||
252 | _cachedBC.Reset(); | ||
253 | _needsUpdate = false; | ||
254 | |||
255 | Vector3 p, a, b, c, d; | ||
256 | switch (NumVertices) | ||
257 | { | ||
258 | case 0: | ||
259 | _cachedValidClosest = false; | ||
260 | break; | ||
261 | case 1: | ||
262 | _cachedPA = _simplexPointsP[0]; | ||
263 | _cachedPB = _simplexPointsQ[0]; | ||
264 | _cachedV = _cachedPA - _cachedPB; | ||
265 | _cachedBC.Reset(); | ||
266 | _cachedBC.SetBarycentricCoordinates(1f, 0f, 0f, 0f); | ||
267 | _cachedValidClosest = _cachedBC.IsValid; | ||
268 | break; | ||
269 | case 2: | ||
270 | //closest point origin from line segment | ||
271 | Vector3 from = _simplexVectorW[0]; | ||
272 | Vector3 to = _simplexVectorW[1]; | ||
273 | Vector3 nearest; | ||
274 | |||
275 | Vector3 diff = -from; | ||
276 | Vector3 v = to - from; | ||
277 | float t = Vector3.Dot(v, diff); | ||
278 | |||
279 | if (t > 0) | ||
280 | { | ||
281 | float dotVV = v.LengthSquared(); | ||
282 | if (t < dotVV) | ||
283 | { | ||
284 | t /= dotVV; | ||
285 | diff -= t * v; | ||
286 | _cachedBC.UsedVertices.UsedVertexA = true; | ||
287 | _cachedBC.UsedVertices.UsedVertexB = true; | ||
288 | } | ||
289 | else | ||
290 | { | ||
291 | t = 1; | ||
292 | diff -= v; | ||
293 | //reduce to 1 point | ||
294 | _cachedBC.UsedVertices.UsedVertexB = true; | ||
295 | } | ||
296 | } | ||
297 | else | ||
298 | { | ||
299 | t = 0; | ||
300 | //reduce to 1 point | ||
301 | _cachedBC.UsedVertices.UsedVertexA = true; | ||
302 | } | ||
303 | |||
304 | _cachedBC.SetBarycentricCoordinates(1 - t, t, 0, 0); | ||
305 | nearest = from + t * v; | ||
306 | |||
307 | _cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]); | ||
308 | _cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]); | ||
309 | _cachedV = _cachedPA - _cachedPB; | ||
310 | |||
311 | ReduceVertices(_cachedBC.UsedVertices); | ||
312 | |||
313 | _cachedValidClosest = _cachedBC.IsValid; | ||
314 | break; | ||
315 | case 3: | ||
316 | //closest point origin from triangle | ||
317 | p = new Vector3(); | ||
318 | a = _simplexVectorW[0]; | ||
319 | b = _simplexVectorW[1]; | ||
320 | c = _simplexVectorW[2]; | ||
321 | |||
322 | ClosestPtPointTriangle(p, a, b, c, ref _cachedBC); | ||
323 | _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] + | ||
324 | _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] + | ||
325 | _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] + | ||
326 | _simplexPointsP[3] * _cachedBC.BarycentricCoords[3]; | ||
327 | |||
328 | _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] + | ||
329 | _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] + | ||
330 | _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] + | ||
331 | _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3]; | ||
332 | |||
333 | _cachedV = _cachedPA - _cachedPB; | ||
334 | |||
335 | ReduceVertices(_cachedBC.UsedVertices); | ||
336 | _cachedValidClosest = _cachedBC.IsValid; | ||
337 | break; | ||
338 | case 4: | ||
339 | p = new Vector3(); | ||
340 | a = _simplexVectorW[0]; | ||
341 | b = _simplexVectorW[1]; | ||
342 | c = _simplexVectorW[2]; | ||
343 | d = _simplexVectorW[3]; | ||
344 | |||
345 | bool hasSeperation = ClosestPtPointTetrahedron(p, a, b, c, d, ref _cachedBC); | ||
346 | |||
347 | if (hasSeperation) | ||
348 | { | ||
349 | _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] + | ||
350 | _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] + | ||
351 | _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] + | ||
352 | _simplexPointsP[3] * _cachedBC.BarycentricCoords[3]; | ||
353 | |||
354 | _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] + | ||
355 | _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] + | ||
356 | _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] + | ||
357 | _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3]; | ||
358 | |||
359 | _cachedV = _cachedPA - _cachedPB; | ||
360 | ReduceVertices(_cachedBC.UsedVertices); | ||
361 | } | ||
362 | else | ||
363 | { | ||
364 | if (_cachedBC.Degenerate) | ||
365 | { | ||
366 | _cachedValidClosest = false; | ||
367 | } | ||
368 | else | ||
369 | { | ||
370 | _cachedValidClosest = true; | ||
371 | //degenerate case == false, penetration = true + zero | ||
372 | _cachedV.X = _cachedV.Y = _cachedV.Z = 0f; | ||
373 | } | ||
374 | break; // !!!!!!!!!!!! proverit na vsakiy sluchai | ||
375 | } | ||
376 | |||
377 | _cachedValidClosest = _cachedBC.IsValid; | ||
378 | |||
379 | //closest point origin from tetrahedron | ||
380 | break; | ||
381 | default: | ||
382 | _cachedValidClosest = false; | ||
383 | break; | ||
384 | } | ||
385 | } | ||
386 | |||
387 | return _cachedValidClosest; | ||
388 | } | ||
389 | |||
390 | public bool ClosestPtPointTriangle(Vector3 p, Vector3 a, Vector3 b, Vector3 c, | ||
391 | ref SubSimplexClosestResult result) | ||
392 | { | ||
393 | result.UsedVertices.Reset(); | ||
394 | |||
395 | float v, w; | ||
396 | |||
397 | // Check if P in vertex region outside A | ||
398 | Vector3 ab = b - a; | ||
399 | Vector3 ac = c - a; | ||
400 | Vector3 ap = p - a; | ||
401 | float d1 = Vector3.Dot(ab, ap); | ||
402 | float d2 = Vector3.Dot(ac, ap); | ||
403 | if (d1 <= 0f && d2 <= 0f) | ||
404 | { | ||
405 | result.ClosestPointOnSimplex = a; | ||
406 | result.UsedVertices.UsedVertexA = true; | ||
407 | result.SetBarycentricCoordinates(1, 0, 0, 0); | ||
408 | return true; // a; // barycentric coordinates (1,0,0) | ||
409 | } | ||
410 | |||
411 | // Check if P in vertex region outside B | ||
412 | Vector3 bp = p - b; | ||
413 | float d3 = Vector3.Dot(ab, bp); | ||
414 | float d4 = Vector3.Dot(ac, bp); | ||
415 | if (d3 >= 0f && d4 <= d3) | ||
416 | { | ||
417 | result.ClosestPointOnSimplex = b; | ||
418 | result.UsedVertices.UsedVertexB = true; | ||
419 | result.SetBarycentricCoordinates(0, 1, 0, 0); | ||
420 | |||
421 | return true; // b; // barycentric coordinates (0,1,0) | ||
422 | } | ||
423 | // Check if P in edge region of AB, if so return projection of P onto AB | ||
424 | float vc = d1 * d4 - d3 * d2; | ||
425 | if (vc <= 0f && d1 >= 0f && d3 <= 0f) | ||
426 | { | ||
427 | v = d1 / (d1 - d3); | ||
428 | result.ClosestPointOnSimplex = a + v * ab; | ||
429 | result.UsedVertices.UsedVertexA = true; | ||
430 | result.UsedVertices.UsedVertexB = true; | ||
431 | result.SetBarycentricCoordinates(1 - v, v, 0, 0); | ||
432 | return true; | ||
433 | //return a + v * ab; // barycentric coordinates (1-v,v,0) | ||
434 | } | ||
435 | |||
436 | // Check if P in vertex region outside C | ||
437 | Vector3 cp = p - c; | ||
438 | float d5 = Vector3.Dot(ab, cp); | ||
439 | float d6 = Vector3.Dot(ac, cp); | ||
440 | if (d6 >= 0f && d5 <= d6) | ||
441 | { | ||
442 | result.ClosestPointOnSimplex = c; | ||
443 | result.UsedVertices.UsedVertexC = true; | ||
444 | result.SetBarycentricCoordinates(0, 0, 1, 0); | ||
445 | return true;//c; // barycentric coordinates (0,0,1) | ||
446 | } | ||
447 | |||
448 | // Check if P in edge region of AC, if so return projection of P onto AC | ||
449 | float vb = d5 * d2 - d1 * d6; | ||
450 | if (vb <= 0f && d2 >= 0f && d6 <= 0f) | ||
451 | { | ||
452 | w = d2 / (d2 - d6); | ||
453 | result.ClosestPointOnSimplex = a + w * ac; | ||
454 | result.UsedVertices.UsedVertexA = true; | ||
455 | result.UsedVertices.UsedVertexC = true; | ||
456 | result.SetBarycentricCoordinates(1 - w, 0, w, 0); | ||
457 | return true; | ||
458 | //return a + w * ac; // barycentric coordinates (1-w,0,w) | ||
459 | } | ||
460 | |||
461 | // Check if P in edge region of BC, if so return projection of P onto BC | ||
462 | float va = d3 * d6 - d5 * d4; | ||
463 | if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f) | ||
464 | { | ||
465 | w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); | ||
466 | |||
467 | result.ClosestPointOnSimplex = b + w * (c - b); | ||
468 | result.UsedVertices.UsedVertexB = true; | ||
469 | result.UsedVertices.UsedVertexC = true; | ||
470 | result.SetBarycentricCoordinates(0, 1 - w, w, 0); | ||
471 | return true; | ||
472 | // return b + w * (c - b); // barycentric coordinates (0,1-w,w) | ||
473 | } | ||
474 | |||
475 | // P inside face region. Compute Q through its barycentric coordinates (u,v,w) | ||
476 | float denom = 1.0f / (va + vb + vc); | ||
477 | v = vb * denom; | ||
478 | w = vc * denom; | ||
479 | |||
480 | result.ClosestPointOnSimplex = a + ab * v + ac * w; | ||
481 | result.UsedVertices.UsedVertexA = true; | ||
482 | result.UsedVertices.UsedVertexB = true; | ||
483 | result.UsedVertices.UsedVertexC = true; | ||
484 | result.SetBarycentricCoordinates(1 - v - w, v, w, 0); | ||
485 | |||
486 | return true; | ||
487 | } | ||
488 | |||
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) | ||
491 | { | ||
492 | Vector3 normal = Vector3.Cross(b - a, c - a); | ||
493 | |||
494 | float signp = Vector3.Dot(p - a, normal); // [AP AB AC] | ||
495 | float signd = Vector3.Dot(d - a, normal); // [AD AB AC] | ||
496 | |||
497 | if (CatchDegenerateTetrahedron) | ||
498 | if (signd * signd < (1e-4f * 1e-4f)) return -1; | ||
499 | |||
500 | // Points on opposite sides if expression signs are opposite | ||
501 | return signp * signd < 0f ? 1 : 0; | ||
502 | } | ||
503 | |||
504 | public bool ClosestPtPointTetrahedron(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d, | ||
505 | ref SubSimplexClosestResult finalResult) | ||
506 | { | ||
507 | SubSimplexClosestResult tempResult = new SubSimplexClosestResult(); | ||
508 | |||
509 | // Start out assuming point inside all halfspaces, so closest to itself | ||
510 | finalResult.ClosestPointOnSimplex = p; | ||
511 | finalResult.UsedVertices.Reset(); | ||
512 | finalResult.UsedVertices.UsedVertexA = true; | ||
513 | finalResult.UsedVertices.UsedVertexB = true; | ||
514 | finalResult.UsedVertices.UsedVertexC = true; | ||
515 | finalResult.UsedVertices.UsedVertexD = true; | ||
516 | |||
517 | int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d); | ||
518 | int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b); | ||
519 | int pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c); | ||
520 | int pointOutsideBDC = PointOutsideOfPlane(p, b, d, c, a); | ||
521 | |||
522 | if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) | ||
523 | { | ||
524 | finalResult.Degenerate = true; | ||
525 | return false; | ||
526 | } | ||
527 | |||
528 | if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0) | ||
529 | return false; | ||
530 | |||
531 | float bestSqDist = float.MaxValue; | ||
532 | // If point outside face abc then compute closest point on abc | ||
533 | if (pointOutsideABC != 0) | ||
534 | { | ||
535 | ClosestPtPointTriangle(p, a, b, c, ref tempResult); | ||
536 | Vector3 q = tempResult.ClosestPointOnSimplex; | ||
537 | |||
538 | float sqDist = ((Vector3)(q - p)).LengthSquared(); | ||
539 | // Update best closest point if (squared) distance is less than current best | ||
540 | if (sqDist < bestSqDist) | ||
541 | { | ||
542 | bestSqDist = sqDist; | ||
543 | finalResult.ClosestPointOnSimplex = q; | ||
544 | //convert result bitmask! | ||
545 | finalResult.UsedVertices.Reset(); | ||
546 | finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; | ||
547 | finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexB; | ||
548 | finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC; | ||
549 | finalResult.SetBarycentricCoordinates( | ||
550 | tempResult.BarycentricCoords[VertexA], | ||
551 | tempResult.BarycentricCoords[VertexB], | ||
552 | tempResult.BarycentricCoords[VertexC], | ||
553 | 0); | ||
554 | } | ||
555 | } | ||
556 | |||
557 | // Repeat test for face acd | ||
558 | if (pointOutsideACD != 0) | ||
559 | { | ||
560 | ClosestPtPointTriangle(p, a, c, d, ref tempResult); | ||
561 | Vector3 q = tempResult.ClosestPointOnSimplex; | ||
562 | //convert result bitmask! | ||
563 | |||
564 | float sqDist = ((Vector3)(q - p)).LengthSquared(); | ||
565 | if (sqDist < bestSqDist) | ||
566 | { | ||
567 | bestSqDist = sqDist; | ||
568 | finalResult.ClosestPointOnSimplex = q; | ||
569 | finalResult.UsedVertices.Reset(); | ||
570 | finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; | ||
571 | finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexB; | ||
572 | finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexC; | ||
573 | finalResult.SetBarycentricCoordinates( | ||
574 | tempResult.BarycentricCoords[VertexA], | ||
575 | 0, | ||
576 | tempResult.BarycentricCoords[VertexB], | ||
577 | tempResult.BarycentricCoords[VertexC]); | ||
578 | } | ||
579 | } | ||
580 | // Repeat test for face adb | ||
581 | |||
582 | if (pointOutsideADB != 0) | ||
583 | { | ||
584 | ClosestPtPointTriangle(p, a, d, b, ref tempResult); | ||
585 | Vector3 q = tempResult.ClosestPointOnSimplex; | ||
586 | //convert result bitmask! | ||
587 | |||
588 | float sqDist = ((Vector3)(q - p)).LengthSquared(); | ||
589 | if (sqDist < bestSqDist) | ||
590 | { | ||
591 | bestSqDist = sqDist; | ||
592 | finalResult.ClosestPointOnSimplex = q; | ||
593 | finalResult.UsedVertices.Reset(); | ||
594 | finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; | ||
595 | finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB; | ||
596 | finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexC; | ||
597 | finalResult.SetBarycentricCoordinates( | ||
598 | tempResult.BarycentricCoords[VertexA], | ||
599 | tempResult.BarycentricCoords[VertexC], | ||
600 | 0, | ||
601 | tempResult.BarycentricCoords[VertexB]); | ||
602 | |||
603 | } | ||
604 | } | ||
605 | // Repeat test for face bdc | ||
606 | |||
607 | if (pointOutsideBDC != 0) | ||
608 | { | ||
609 | ClosestPtPointTriangle(p, b, d, c, ref tempResult); | ||
610 | Vector3 q = tempResult.ClosestPointOnSimplex; | ||
611 | //convert result bitmask! | ||
612 | float sqDist = ((Vector3)(q - p)).LengthSquared(); | ||
613 | if (sqDist < bestSqDist) | ||
614 | { | ||
615 | bestSqDist = sqDist; | ||
616 | finalResult.ClosestPointOnSimplex = q; | ||
617 | finalResult.UsedVertices.Reset(); | ||
618 | finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexA; | ||
619 | finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB; | ||
620 | finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC; | ||
621 | |||
622 | finalResult.SetBarycentricCoordinates( | ||
623 | 0, | ||
624 | tempResult.BarycentricCoords[VertexA], | ||
625 | tempResult.BarycentricCoords[VertexC], | ||
626 | tempResult.BarycentricCoords[VertexB]); | ||
627 | } | ||
628 | } | ||
629 | |||
630 | //help! we ended up full ! | ||
631 | |||
632 | if (finalResult.UsedVertices.UsedVertexA && | ||
633 | finalResult.UsedVertices.UsedVertexB && | ||
634 | finalResult.UsedVertices.UsedVertexC && | ||
635 | finalResult.UsedVertices.UsedVertexD) | ||
636 | { | ||
637 | return true; | ||
638 | } | ||
639 | |||
640 | return true; | ||
641 | } | ||
642 | } | ||
643 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs new file mode 100644 index 0000000..e4a9ae2 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs | |||
@@ -0,0 +1,488 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX.Dynamics | ||
28 | { | ||
29 | public delegate float ContactSolverFunc (RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo info); | ||
30 | |||
31 | public enum ContactSolverType | ||
32 | { | ||
33 | Default = 0, | ||
34 | TypeA, | ||
35 | TypeB, | ||
36 | User, | ||
37 | MaxContactSolverType, | ||
38 | } | ||
39 | |||
40 | public class ConstraintPersistentData | ||
41 | { | ||
42 | // total applied impulse during most recent frame | ||
43 | private float _appliedImpulse; | ||
44 | private float _previousAppliedImpulse; | ||
45 | private float _accumulatedTangentImpulse0; | ||
46 | private float _accumulatedTangentImpulse1; | ||
47 | |||
48 | private float _jacDiagABInv; | ||
49 | private float _jacDiagABInvTangentA; | ||
50 | private float _jacDiagABInvTangentB; | ||
51 | private int _persistentLifeTime; | ||
52 | private float _restitution; | ||
53 | private float _friction; | ||
54 | private float _penetration; | ||
55 | private Vector3 _frictionWorldTangentialA; | ||
56 | private Vector3 _frictionWorldTangentialB; | ||
57 | |||
58 | private Vector3 _frictionAngularComponent0A; | ||
59 | private Vector3 _frictionAngularComponent0B; | ||
60 | private Vector3 _frictionAngularComponent1A; | ||
61 | private Vector3 _frictionAngularComponent1B; | ||
62 | |||
63 | //some data doesn't need to be persistent over frames: todo: clean/reuse this | ||
64 | private Vector3 _angularComponentA; | ||
65 | private Vector3 _angularComponentB; | ||
66 | |||
67 | private ContactSolverFunc _contactSolverFunc; | ||
68 | private ContactSolverFunc _frictionSolverFunc; | ||
69 | |||
70 | public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } } | ||
71 | public float PreviousAppliedImpulse { get { return _previousAppliedImpulse; } set { _previousAppliedImpulse = value; } } | ||
72 | public float AccumulatedTangentImpulseA { get { return _accumulatedTangentImpulse0; } set { _accumulatedTangentImpulse0 = value; } } | ||
73 | public float AccumulatedTangentImpulseB { get { return _accumulatedTangentImpulse1; } set { _accumulatedTangentImpulse1 = value; } } | ||
74 | |||
75 | public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } } | ||
76 | public float JacDiagABInvTangentA { get { return _jacDiagABInvTangentA; } set { _jacDiagABInvTangentA = value; } } | ||
77 | public float JacDiagABInvTangentB { get { return _jacDiagABInvTangentB; } set { _jacDiagABInvTangentB = value; } } | ||
78 | public int PersistentLifeTime { get { return _persistentLifeTime; } set { _persistentLifeTime = value; } } | ||
79 | public float Restitution { get { return _restitution; } set { _restitution = value; } } | ||
80 | public float Friction { get { return _friction; } set { _friction = value; } } | ||
81 | public float Penetration { get { return _penetration; } set { _penetration = value; } } | ||
82 | public Vector3 FrictionWorldTangentialA { get { return _frictionWorldTangentialA; } set { _frictionWorldTangentialA = value; } } | ||
83 | public Vector3 FrictionWorldTangentialB { get { return _frictionWorldTangentialB; } set { _frictionWorldTangentialB = value; } } | ||
84 | |||
85 | public Vector3 FrictionAngularComponent0A { get { return _frictionAngularComponent0A; } set { _frictionAngularComponent0A = value; } } | ||
86 | public Vector3 FrictionAngularComponent0B { get { return _frictionAngularComponent0B; } set { _frictionAngularComponent0B = value; } } | ||
87 | public Vector3 FrictionAngularComponent1A { get { return _frictionAngularComponent1A; } set { _frictionAngularComponent1A = value; } } | ||
88 | public Vector3 FrictionAngularComponent1B { get { return _frictionAngularComponent1B; } set { _frictionAngularComponent1B = value; } } | ||
89 | |||
90 | public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } } | ||
91 | public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } } | ||
92 | |||
93 | public ContactSolverFunc ContactSolverFunc { get { return _contactSolverFunc; } set { _contactSolverFunc = value; } } | ||
94 | public ContactSolverFunc FrictionSolverFunc { get { return _frictionSolverFunc; } set { _frictionSolverFunc = value; } } | ||
95 | } | ||
96 | |||
97 | public static class ContactConstraint | ||
98 | { | ||
99 | private const int UseInternalApplyImpulse = 1; | ||
100 | |||
101 | /// <summary> | ||
102 | /// bilateral constraint between two dynamic objects | ||
103 | /// positive distance = separation, negative distance = penetration | ||
104 | /// </summary> | ||
105 | /// <param name="body1"></param> | ||
106 | /// <param name="pos1"></param> | ||
107 | /// <param name="body2"></param> | ||
108 | /// <param name="pos2"></param> | ||
109 | /// <param name="distance"></param> | ||
110 | /// <param name="normal"></param> | ||
111 | /// <param name="impulse"></param> | ||
112 | /// <param name="timeStep"></param> | ||
113 | public static void ResolveSingleBilateral(RigidBody bodyA, Vector3 posA, | ||
114 | RigidBody bodyB, Vector3 posB, | ||
115 | float distance, Vector3 normal, out float impulse, float timeStep) | ||
116 | { | ||
117 | float normalLenSqr = normal.LengthSquared(); | ||
118 | |||
119 | if (Math.Abs(normalLenSqr) >= 1.1f) | ||
120 | throw new BulletException(); | ||
121 | |||
122 | /*if (normalLenSqr > 1.1f) | ||
123 | { | ||
124 | impulse = 0f; | ||
125 | return; | ||
126 | }*/ | ||
127 | Vector3 rel_pos1 = posA - bodyA.CenterOfMassPosition; | ||
128 | Vector3 rel_pos2 = posB - bodyB.CenterOfMassPosition; | ||
129 | //this jacobian entry could be re-used for all iterations | ||
130 | |||
131 | Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); | ||
132 | Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); | ||
133 | Vector3 vel = vel1 - vel2; | ||
134 | |||
135 | |||
136 | JacobianEntry jac = new JacobianEntry(Matrix.Transpose(bodyA.CenterOfMassTransform), | ||
137 | Matrix.Transpose(bodyB.CenterOfMassTransform), | ||
138 | rel_pos1, rel_pos2, normal, bodyA.InvInertiaDiagLocal, bodyA.InverseMass, | ||
139 | bodyB.InvInertiaDiagLocal, bodyB.InverseMass); | ||
140 | |||
141 | float jacDiagAB = jac.Diagonal; | ||
142 | float jacDiagABInv = 1f / jacDiagAB; | ||
143 | |||
144 | float rel_vel = jac.GetRelativeVelocity( | ||
145 | bodyA.LinearVelocity, | ||
146 | Vector3.TransformNormal(bodyA.AngularVelocity, Matrix.Transpose(bodyA.CenterOfMassTransform)), | ||
147 | bodyB.LinearVelocity, | ||
148 | Vector3.TransformNormal(bodyB.AngularVelocity, Matrix.Transpose(bodyB.CenterOfMassTransform))); | ||
149 | float a; | ||
150 | a = jacDiagABInv; | ||
151 | |||
152 | |||
153 | rel_vel = Vector3.Dot(normal, vel); | ||
154 | |||
155 | float contactDamping = 0.2f; | ||
156 | |||
157 | float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; | ||
158 | impulse = velocityImpulse; | ||
159 | } | ||
160 | |||
161 | |||
162 | /// <summary> | ||
163 | /// contact constraint resolution: | ||
164 | /// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint | ||
165 | /// positive distance = separation, negative distance = penetration | ||
166 | /// </summary> | ||
167 | /// <param name="body1"></param> | ||
168 | /// <param name="body2"></param> | ||
169 | /// <param name="contactPoint"></param> | ||
170 | /// <param name="info"></param> | ||
171 | /// <returns></returns> | ||
172 | public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB, | ||
173 | ManifoldPoint contactPoint, ContactSolverInfo solverInfo) | ||
174 | { | ||
175 | Vector3 pos1 = contactPoint.PositionWorldOnA; | ||
176 | Vector3 pos2 = contactPoint.PositionWorldOnB; | ||
177 | |||
178 | |||
179 | // printf("distance=%f\n",distance); | ||
180 | |||
181 | Vector3 normal = contactPoint.NormalWorldOnB; | ||
182 | |||
183 | Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition; | ||
184 | Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition; | ||
185 | |||
186 | Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); | ||
187 | Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); | ||
188 | Vector3 vel = vel1 - vel2; | ||
189 | float rel_vel; | ||
190 | rel_vel = Vector3.Dot(normal, vel); | ||
191 | |||
192 | |||
193 | float Kfps = 1f / solverInfo.TimeStep; | ||
194 | |||
195 | //float damping = solverInfo.m_damping; | ||
196 | float Kerp = solverInfo.Erp; | ||
197 | |||
198 | float Kcor = Kerp * Kfps; | ||
199 | |||
200 | //printf("dist=%f\n",distance); | ||
201 | |||
202 | ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; | ||
203 | if (cpd == null) | ||
204 | throw new BulletException(); | ||
205 | |||
206 | float distance = cpd.Penetration;//contactPoint.getDistance(); | ||
207 | |||
208 | |||
209 | //distance = 0.f; | ||
210 | float positionalError = Kcor * -distance; | ||
211 | //jacDiagABInv; | ||
212 | float velocityError = cpd.Restitution - rel_vel;// * damping; | ||
213 | |||
214 | |||
215 | float penetrationImpulse = positionalError * cpd.JacDiagABInv; | ||
216 | float velocityImpulse = velocityError * cpd.JacDiagABInv; | ||
217 | float normalImpulse = penetrationImpulse + velocityImpulse; | ||
218 | |||
219 | // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse | ||
220 | float oldNormalImpulse = cpd.AppliedImpulse; | ||
221 | float sum = oldNormalImpulse + normalImpulse; | ||
222 | cpd.AppliedImpulse = 0f > sum ? 0f : sum; | ||
223 | |||
224 | normalImpulse = cpd.AppliedImpulse - oldNormalImpulse; | ||
225 | |||
226 | if (bodyA.InverseMass != 0) | ||
227 | { | ||
228 | bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse); | ||
229 | } | ||
230 | if (bodyB.InverseMass != 0) | ||
231 | { | ||
232 | bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse); | ||
233 | } | ||
234 | |||
235 | /*body1.applyImpulse(normal * (normalImpulse), rel_pos1); | ||
236 | body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/ | ||
237 | |||
238 | return normalImpulse; | ||
239 | } | ||
240 | |||
241 | public static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB, | ||
242 | ManifoldPoint contactPoint, ContactSolverInfo solverInfo) | ||
243 | { | ||
244 | |||
245 | Vector3 pos1 = contactPoint.PositionWorldOnA; | ||
246 | Vector3 pos2 = contactPoint.PositionWorldOnB; | ||
247 | |||
248 | Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition; | ||
249 | Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition; | ||
250 | |||
251 | ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; | ||
252 | if (cpd == null) | ||
253 | throw new BulletException(); | ||
254 | |||
255 | float combinedFriction = cpd.Friction; | ||
256 | |||
257 | float limit = cpd.AppliedImpulse * combinedFriction; | ||
258 | |||
259 | //friction | ||
260 | if (cpd.AppliedImpulse > 0) | ||
261 | { | ||
262 | //apply friction in the 2 tangential directions | ||
263 | |||
264 | // 1st tangent | ||
265 | Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); | ||
266 | Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); | ||
267 | Vector3 vel = vel1 - vel2; | ||
268 | |||
269 | float j1, j2; | ||
270 | |||
271 | { | ||
272 | |||
273 | float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel); | ||
274 | |||
275 | // calculate j that moves us to zero relative velocity | ||
276 | j1 = -vrel * cpd.JacDiagABInvTangentA; | ||
277 | float oldTangentImpulse = cpd.AccumulatedTangentImpulseA; | ||
278 | cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1; | ||
279 | float atia = cpd.AccumulatedTangentImpulseA; | ||
280 | MathHelper.SetMin(ref atia, limit); | ||
281 | MathHelper.SetMax(ref atia, -limit); | ||
282 | cpd.AccumulatedTangentImpulseA = atia; | ||
283 | j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse; | ||
284 | |||
285 | } | ||
286 | { | ||
287 | // 2nd tangent | ||
288 | |||
289 | float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel); | ||
290 | |||
291 | // calculate j that moves us to zero relative velocity | ||
292 | j2 = -vrel * cpd.JacDiagABInvTangentB; | ||
293 | float oldTangentImpulse = cpd.AccumulatedTangentImpulseB; | ||
294 | cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2; | ||
295 | float atib = cpd.AccumulatedTangentImpulseB; | ||
296 | MathHelper.SetMin(ref atib, limit); | ||
297 | MathHelper.SetMax(ref atib, -limit); | ||
298 | cpd.AccumulatedTangentImpulseB = atib; | ||
299 | j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse; | ||
300 | } | ||
301 | |||
302 | if (bodyA.InverseMass != 0) | ||
303 | { | ||
304 | bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1); | ||
305 | bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2); | ||
306 | } | ||
307 | if (bodyB.InverseMass != 0) | ||
308 | { | ||
309 | bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1); | ||
310 | bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2); | ||
311 | } | ||
312 | |||
313 | } | ||
314 | return cpd.AppliedImpulse; | ||
315 | } | ||
316 | |||
317 | public static float ResolveSingleFrictionOriginal( | ||
318 | RigidBody bodyA, | ||
319 | RigidBody bodyB, | ||
320 | ManifoldPoint contactPoint, | ||
321 | ContactSolverInfo solverInfo) | ||
322 | { | ||
323 | Vector3 posA = contactPoint.PositionWorldOnA; | ||
324 | Vector3 posB = contactPoint.PositionWorldOnB; | ||
325 | |||
326 | Vector3 relPosA = posA - bodyA.CenterOfMassPosition; | ||
327 | Vector3 relPosB = posB - bodyB.CenterOfMassPosition; | ||
328 | |||
329 | ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; | ||
330 | if (cpd == null) | ||
331 | throw new BulletException(); | ||
332 | |||
333 | float combinedFriction = cpd.Friction; | ||
334 | |||
335 | float limit = cpd.AppliedImpulse * combinedFriction; | ||
336 | //if (contactPoint.m_appliedImpulse>0.f) | ||
337 | //friction | ||
338 | { | ||
339 | //apply friction in the 2 tangential directions | ||
340 | |||
341 | { | ||
342 | // 1st tangent | ||
343 | Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); | ||
344 | Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); | ||
345 | Vector3 vel = velA - velB; | ||
346 | |||
347 | float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel); | ||
348 | |||
349 | // calculate j that moves us to zero relative velocity | ||
350 | float j = -vrel * cpd.JacDiagABInvTangentA; | ||
351 | float total = cpd.AccumulatedTangentImpulseA + j; | ||
352 | if (limit < total) | ||
353 | total = limit; | ||
354 | if (total < -limit) | ||
355 | total = -limit; | ||
356 | j = total - cpd.AccumulatedTangentImpulseA; | ||
357 | cpd.AccumulatedTangentImpulseA = total; | ||
358 | bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA); | ||
359 | bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB); | ||
360 | } | ||
361 | |||
362 | |||
363 | { | ||
364 | // 2nd tangent | ||
365 | Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); | ||
366 | Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); | ||
367 | Vector3 vel = velA - velB; | ||
368 | |||
369 | float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel); | ||
370 | |||
371 | // calculate j that moves us to zero relative velocity | ||
372 | float j = -vrel * cpd.JacDiagABInvTangentB; | ||
373 | float total = cpd.AccumulatedTangentImpulseB + j; | ||
374 | if (limit < total) | ||
375 | total = limit; | ||
376 | if (total < -limit) | ||
377 | total = -limit; | ||
378 | j = total - cpd.AccumulatedTangentImpulseB; | ||
379 | cpd.AccumulatedTangentImpulseB = total; | ||
380 | bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA); | ||
381 | bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB); | ||
382 | } | ||
383 | } | ||
384 | |||
385 | return cpd.AppliedImpulse; | ||
386 | } | ||
387 | |||
388 | //velocity + friction | ||
389 | //response between two dynamic objects with friction | ||
390 | public static float ResolveSingleCollisionCombined( | ||
391 | RigidBody bodyA, | ||
392 | RigidBody bodyB, | ||
393 | ManifoldPoint contactPoint, | ||
394 | ContactSolverInfo solverInfo) | ||
395 | { | ||
396 | |||
397 | Vector3 posA = contactPoint.PositionWorldOnA; | ||
398 | Vector3 posB = contactPoint.PositionWorldOnB; | ||
399 | Vector3 normal = contactPoint.NormalWorldOnB; | ||
400 | |||
401 | Vector3 relPosA = posA - bodyA.CenterOfMassPosition; | ||
402 | Vector3 relPosB = posB - bodyB.CenterOfMassPosition; | ||
403 | |||
404 | Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); | ||
405 | Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); | ||
406 | Vector3 vel = velA - velB; | ||
407 | float relVel; | ||
408 | relVel = Vector3.Dot(normal, vel); | ||
409 | |||
410 | float Kfps = 1f / solverInfo.TimeStep; | ||
411 | |||
412 | //float damping = solverInfo.m_damping; | ||
413 | float Kerp = solverInfo.Erp; | ||
414 | float Kcor = Kerp * Kfps; | ||
415 | |||
416 | ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; | ||
417 | if (cpd == null) | ||
418 | throw new BulletException(); | ||
419 | |||
420 | float distance = cpd.Penetration; | ||
421 | float positionalError = Kcor * -distance; | ||
422 | float velocityError = cpd.Restitution - relVel;// * damping; | ||
423 | |||
424 | float penetrationImpulse = positionalError * cpd.JacDiagABInv; | ||
425 | |||
426 | float velocityImpulse = velocityError * cpd.JacDiagABInv; | ||
427 | |||
428 | float normalImpulse = penetrationImpulse + velocityImpulse; | ||
429 | |||
430 | // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse | ||
431 | float oldNormalImpulse = cpd.AppliedImpulse; | ||
432 | float sum = oldNormalImpulse + normalImpulse; | ||
433 | cpd.AppliedImpulse = 0 > sum ? 0 : sum; | ||
434 | |||
435 | normalImpulse = cpd.AppliedImpulse - oldNormalImpulse; | ||
436 | |||
437 | if (bodyA.InverseMass != 0) | ||
438 | { | ||
439 | bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse); | ||
440 | } | ||
441 | if (bodyB.InverseMass != 0) | ||
442 | { | ||
443 | bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse); | ||
444 | } | ||
445 | |||
446 | { | ||
447 | //friction | ||
448 | Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA); | ||
449 | Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB); | ||
450 | Vector3 vel3 = vel12 - vel22; | ||
451 | |||
452 | relVel = Vector3.Dot(normal, vel3); | ||
453 | |||
454 | |||
455 | Vector3 latVel = vel3 - normal * relVel; | ||
456 | float lat_rel_vel = latVel.Length(); | ||
457 | |||
458 | float combinedFriction = cpd.Friction; | ||
459 | |||
460 | if (cpd.AppliedImpulse > 0) | ||
461 | if (lat_rel_vel > float.Epsilon) | ||
462 | { | ||
463 | latVel /= lat_rel_vel; | ||
464 | Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld); | ||
465 | Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld); | ||
466 | float friction_impulse = lat_rel_vel / | ||
467 | (bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB))); | ||
468 | float normal_impulse = cpd.AppliedImpulse * combinedFriction; | ||
469 | |||
470 | MathHelper.SetMin(ref friction_impulse, normal_impulse); | ||
471 | MathHelper.SetMin(ref friction_impulse, -normal_impulse); | ||
472 | bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA); | ||
473 | bodyB.ApplyImpulse(latVel * friction_impulse, relPosB); | ||
474 | } | ||
475 | } | ||
476 | return normalImpulse; | ||
477 | } | ||
478 | |||
479 | public static float ResolveSingleFrictionEmpty( | ||
480 | RigidBody bodyA, | ||
481 | RigidBody bodyB, | ||
482 | ManifoldPoint contactPoint, | ||
483 | ContactSolverInfo solverInfo) | ||
484 | { | ||
485 | return 0; | ||
486 | } | ||
487 | } | ||
488 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs new file mode 100644 index 0000000..c1763df --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs | |||
@@ -0,0 +1,62 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX.Dynamics | ||
27 | { | ||
28 | public class ContactSolverInfo | ||
29 | { | ||
30 | private float _tau; | ||
31 | private float _damping; | ||
32 | private float _friction; | ||
33 | private float _timeStep; | ||
34 | private float _restitution; | ||
35 | private int _numIterations; | ||
36 | private float _maxErrorReduction; | ||
37 | private float _sor; | ||
38 | private float _erp; | ||
39 | |||
40 | public ContactSolverInfo() | ||
41 | { | ||
42 | _tau = 0.6f; | ||
43 | _damping = 1.0f; | ||
44 | _friction = 0.3f; | ||
45 | _restitution = 0f; | ||
46 | _maxErrorReduction = 20f; | ||
47 | _numIterations = 10; | ||
48 | _erp = 0.4f; | ||
49 | _sor = 1.3f; | ||
50 | } | ||
51 | |||
52 | public float Tau { get { return _tau; } set { _tau = value; } } | ||
53 | public float Damping { get { return _damping; } set { _damping = value; } } | ||
54 | public float Friction { get { return _friction; } set { _friction = value; } } | ||
55 | public float TimeStep { get { return _timeStep; } set { _timeStep = value; } } | ||
56 | public float Restitution { get { return _restitution; } set { _restitution = value; } } | ||
57 | public int IterationsCount { get { return _numIterations; } set { _numIterations = value; } } | ||
58 | public float MaxErrorReduction { get { return _maxErrorReduction; } set { _maxErrorReduction = value; } } | ||
59 | public float Sor { get { return _sor; } set { _sor = value; } } | ||
60 | public float Erp { get { return _erp; } set { _erp = value; } } | ||
61 | } | ||
62 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs new file mode 100644 index 0000000..479c863 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs | |||
@@ -0,0 +1,440 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX.Dynamics | ||
28 | { | ||
29 | /// <summary> | ||
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' | ||
32 | /// Work in progress (is still a Hinge actually) | ||
33 | /// </summary> | ||
34 | public class Generic6DofConstraint : TypedConstraint | ||
35 | { | ||
36 | private static readonly float[] _sign = { 1.0f, -1.0f, 1.0f }; | ||
37 | private static readonly int[] _axisA = { 1, 0, 0 }; | ||
38 | private static readonly int[] _axisB = { 2, 2, 1 }; | ||
39 | |||
40 | private JacobianEntry[] _jacLinear = new JacobianEntry[3]; // 3 orthogonal linear constraints | ||
41 | private JacobianEntry[] _jacAng = new JacobianEntry[3]; // 3 orthogonal angular constraints | ||
42 | |||
43 | private Matrix _frameInA; // the constraint space w.r.t body A | ||
44 | private Matrix _frameInB; // the constraint space w.r.t body B | ||
45 | |||
46 | private float[] _lowerLimit = new float[6]; // the constraint lower limits | ||
47 | private float[] _upperLimit = new float[6]; // the constraint upper limits | ||
48 | |||
49 | private float[] _accumulatedImpulse = new float[6]; | ||
50 | |||
51 | public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB) | ||
52 | : base(rbA, rbB) | ||
53 | { | ||
54 | _frameInA = frameInA; | ||
55 | _frameInB = frameInB; | ||
56 | //free means upper < lower, | ||
57 | //locked means upper == lower | ||
58 | //limited means upper > lower | ||
59 | //so start all locked | ||
60 | for (int i = 0; i < 6; ++i) | ||
61 | { | ||
62 | _lowerLimit[i] = 0.0f; | ||
63 | _upperLimit[i] = 0.0f; | ||
64 | _accumulatedImpulse[i] = 0.0f; | ||
65 | } | ||
66 | } | ||
67 | |||
68 | public Generic6DofConstraint() { } | ||
69 | |||
70 | public void UpdateRHS(float timeStep) { } | ||
71 | |||
72 | public float ComputeAngle(int axis) | ||
73 | { | ||
74 | float angle = 0; | ||
75 | |||
76 | switch (axis) | ||
77 | { | ||
78 | case 0: | ||
79 | { | ||
80 | Vector3 v1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 1), RigidBodyA.CenterOfMassTransform); | ||
81 | Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); | ||
82 | Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); | ||
83 | |||
84 | float s = Vector3.Dot(v1, w2); | ||
85 | float c = Vector3.Dot(v1, v2); | ||
86 | |||
87 | angle = (float)Math.Atan2(s, c); | ||
88 | break; | ||
89 | } | ||
90 | case 1: | ||
91 | { | ||
92 | Vector3 w1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 2), RigidBodyA.CenterOfMassTransform); | ||
93 | Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); | ||
94 | Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); | ||
95 | |||
96 | float s = Vector3.Dot(w1, u2); | ||
97 | float c = Vector3.Dot(w1, w2); | ||
98 | |||
99 | angle = (float)Math.Atan2(s, c); | ||
100 | break; | ||
101 | } | ||
102 | case 2: | ||
103 | { | ||
104 | Vector3 u1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 0), RigidBodyA.CenterOfMassTransform); | ||
105 | Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); | ||
106 | Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); | ||
107 | |||
108 | float s = Vector3.Dot(u1, v2); | ||
109 | float c = Vector3.Dot(u1, u2); | ||
110 | |||
111 | angle = (float)Math.Atan2(s, c); | ||
112 | break; | ||
113 | } | ||
114 | default: BulletDebug.Assert(false); break; | ||
115 | } | ||
116 | |||
117 | return angle; | ||
118 | } | ||
119 | |||
120 | public void SetLinearLowerLimit(Vector3 linearLower) | ||
121 | { | ||
122 | _lowerLimit[0] = linearLower.X; | ||
123 | _lowerLimit[1] = linearLower.Y; | ||
124 | _lowerLimit[2] = linearLower.Z; | ||
125 | } | ||
126 | |||
127 | public void SetLinearUpperLimit(Vector3 linearUpper) | ||
128 | { | ||
129 | _upperLimit[0] = linearUpper.X; | ||
130 | _upperLimit[1] = linearUpper.Y; | ||
131 | _upperLimit[2] = linearUpper.Z; | ||
132 | } | ||
133 | |||
134 | public void SetAngularLowerLimit(Vector3 angularLower) | ||
135 | { | ||
136 | _lowerLimit[3] = angularLower.X; | ||
137 | _lowerLimit[4] = angularLower.Y; | ||
138 | _lowerLimit[5] = angularLower.Z; | ||
139 | } | ||
140 | |||
141 | public void SetAngularUpperLimit(Vector3 angularUpper) | ||
142 | { | ||
143 | _upperLimit[3] = angularUpper.X; | ||
144 | _upperLimit[4] = angularUpper.Y; | ||
145 | _upperLimit[5] = angularUpper.Z; | ||
146 | } | ||
147 | |||
148 | //first 3 are linear, next 3 are angular | ||
149 | public void SetLimit(int axis, float lo, float hi) | ||
150 | { | ||
151 | _lowerLimit[axis] = lo; | ||
152 | _upperLimit[axis] = hi; | ||
153 | } | ||
154 | |||
155 | //free means upper < lower, | ||
156 | //locked means upper == lower | ||
157 | //limited means upper > lower | ||
158 | //limitIndex: first 3 are linear, next 3 are angular | ||
159 | public bool IsLimited(int limitIndex) | ||
160 | { | ||
161 | return (_upperLimit[limitIndex] >= _lowerLimit[limitIndex]); | ||
162 | } | ||
163 | |||
164 | public override void BuildJacobian() | ||
165 | { | ||
166 | Vector3 localNormalInA = new Vector3(0, 0, 0); | ||
167 | |||
168 | Vector3 pivotInA = _frameInA.Translation; | ||
169 | Vector3 pivotInB = _frameInB.Translation; | ||
170 | |||
171 | Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); | ||
172 | Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); | ||
173 | |||
174 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; | ||
175 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; | ||
176 | |||
177 | //linear part | ||
178 | for (int i = 0; i < 3; i++) | ||
179 | { | ||
180 | if (IsLimited(i)) | ||
181 | { | ||
182 | if (i == 0) | ||
183 | localNormalInA = new Vector3(1, 0, 0); | ||
184 | else if (i == 1) | ||
185 | localNormalInA = new Vector3(0, 1, 0); | ||
186 | else | ||
187 | localNormalInA = new Vector3(0, 0, 1); | ||
188 | |||
189 | Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); | ||
190 | |||
191 | // Create linear atom | ||
192 | _jacLinear[i] = new JacobianEntry( | ||
193 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | ||
194 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | ||
195 | MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, | ||
196 | MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, | ||
197 | normalWorld, | ||
198 | RigidBodyA.InvInertiaDiagLocal, | ||
199 | RigidBodyA.InverseMass, | ||
200 | RigidBodyB.InvInertiaDiagLocal, | ||
201 | RigidBodyB.InverseMass); | ||
202 | |||
203 | //optionally disable warmstarting | ||
204 | _accumulatedImpulse[i] = 0f; | ||
205 | |||
206 | // Apply accumulated impulse | ||
207 | Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld; | ||
208 | |||
209 | RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); | ||
210 | RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); | ||
211 | } | ||
212 | } | ||
213 | |||
214 | // angular part | ||
215 | for (int i = 0; i < 3; i++) | ||
216 | { | ||
217 | if (IsLimited(i + 3)) | ||
218 | { | ||
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); | ||
221 | |||
222 | Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB); | ||
223 | |||
224 | // Create angular atom | ||
225 | _jacAng[i] = new JacobianEntry(axis, | ||
226 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | ||
227 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | ||
228 | RigidBodyA.InvInertiaDiagLocal, | ||
229 | RigidBodyB.InvInertiaDiagLocal); | ||
230 | |||
231 | _accumulatedImpulse[i + 3] = 0f; | ||
232 | |||
233 | // Apply accumulated impulse | ||
234 | Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis; | ||
235 | |||
236 | RigidBodyA.ApplyTorqueImpulse(impulse_vector); | ||
237 | RigidBodyB.ApplyTorqueImpulse(-impulse_vector); | ||
238 | } | ||
239 | } | ||
240 | } | ||
241 | |||
242 | public override void SolveConstraint(float timeStep) | ||
243 | { | ||
244 | float tau = 0.1f; | ||
245 | float damping = 1.0f; | ||
246 | |||
247 | Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); | ||
248 | Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); | ||
249 | |||
250 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; | ||
251 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; | ||
252 | |||
253 | Vector3 localNormalInA = new Vector3(); | ||
254 | |||
255 | // linear | ||
256 | for (int i = 0; i < 3; i++) | ||
257 | { | ||
258 | if (IsLimited(i)) | ||
259 | { | ||
260 | Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); | ||
261 | Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); | ||
262 | |||
263 | if (i == 0) | ||
264 | localNormalInA = new Vector3(1, 0, 0); | ||
265 | else if (i == 1) | ||
266 | localNormalInA = new Vector3(0, 1, 0); | ||
267 | else | ||
268 | localNormalInA = new Vector3(0, 0, 1); | ||
269 | |||
270 | Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); | ||
271 | |||
272 | float jacDiagABInv = 1f / _jacLinear[i].Diagonal; | ||
273 | |||
274 | //velocity error (first order error) | ||
275 | float rel_vel = _jacLinear[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, | ||
276 | RigidBodyB.LinearVelocity, angvelB); | ||
277 | |||
278 | //positional error (zeroth order error) | ||
279 | float depth = -Vector3.Dot(pivotAInW - pivotBInW, normalWorld); | ||
280 | float lo = -1e30f; | ||
281 | float hi = 1e30f; | ||
282 | |||
283 | //handle the limits | ||
284 | if (_lowerLimit[i] < _upperLimit[i]) | ||
285 | { | ||
286 | if (depth > _upperLimit[i]) | ||
287 | { | ||
288 | depth -= _upperLimit[i]; | ||
289 | lo = 0f; | ||
290 | } | ||
291 | else | ||
292 | { | ||
293 | if (depth < _lowerLimit[i]) | ||
294 | { | ||
295 | depth -= _lowerLimit[i]; | ||
296 | hi = 0f; | ||
297 | } | ||
298 | else | ||
299 | { | ||
300 | continue; | ||
301 | } | ||
302 | } | ||
303 | } | ||
304 | |||
305 | float normalImpulse = (tau * depth / timeStep - damping * rel_vel) * jacDiagABInv; | ||
306 | float oldNormalImpulse = _accumulatedImpulse[i]; | ||
307 | float sum = oldNormalImpulse + normalImpulse; | ||
308 | _accumulatedImpulse[i] = sum > hi ? 0f : sum < lo ? 0f : sum; | ||
309 | normalImpulse = _accumulatedImpulse[i] - oldNormalImpulse; | ||
310 | |||
311 | Vector3 impulse_vector = normalWorld * normalImpulse; | ||
312 | RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); | ||
313 | RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); | ||
314 | } | ||
315 | } | ||
316 | |||
317 | Vector3 axis; | ||
318 | float angle; | ||
319 | Matrix frameAWorld = RigidBodyA.CenterOfMassTransform * _frameInA; | ||
320 | Matrix frameBWorld = RigidBodyB.CenterOfMassTransform * _frameInB; | ||
321 | |||
322 | TransformUtil.CalculateDiffAxisAngle(frameAWorld, frameBWorld, out axis, out angle); | ||
323 | Quaternion diff = new Quaternion(axis, angle); | ||
324 | Matrix diffMat = Matrix.CreateFromQuaternion(diff); | ||
325 | Vector3 xyz; | ||
326 | // this is not perfect, we can first check which axis are limited, and choose a more appropriate order | ||
327 | MatrixToEulerXYZ(diffMat, out xyz); | ||
328 | |||
329 | // angular | ||
330 | for (int i = 0; i < 3; i++) | ||
331 | { | ||
332 | if (IsLimited(i + 3)) | ||
333 | { | ||
334 | Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); | ||
335 | Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); | ||
336 | |||
337 | float jacDiagABInv = 1f / _jacAng[i].Diagonal; | ||
338 | |||
339 | //velocity error (first order error) | ||
340 | float rel_vel = _jacAng[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, | ||
341 | RigidBodyB.LinearVelocity, angvelB); | ||
342 | |||
343 | //positional error (zeroth order error) | ||
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); | ||
346 | |||
347 | float rel_pos = _sign[i] * Vector3.Dot(axisA, axisB); | ||
348 | |||
349 | float lo = -1e30f; | ||
350 | float hi = 1e30f; | ||
351 | |||
352 | //handle the twist limit | ||
353 | if (_lowerLimit[i + 3] < _upperLimit[i + 3]) | ||
354 | { | ||
355 | //clamp the values | ||
356 | float loLimit = _upperLimit[i + 3] > -3.1415 ? _lowerLimit[i + 3] : -1e30f; | ||
357 | float hiLimit = _upperLimit[i + 3] < 3.1415 ? _upperLimit[i + 3] : 1e30f; | ||
358 | |||
359 | float projAngle; | ||
360 | |||
361 | if (i == 0) | ||
362 | projAngle = -2f * xyz.Z; | ||
363 | else if (i == 1) | ||
364 | projAngle = -2f * xyz.Y; | ||
365 | else | ||
366 | projAngle = -2f * xyz.Z; | ||
367 | |||
368 | if (projAngle < loLimit) | ||
369 | { | ||
370 | hi = 0f; | ||
371 | rel_pos = loLimit - projAngle; | ||
372 | } | ||
373 | else | ||
374 | { | ||
375 | if (projAngle > hiLimit) | ||
376 | { | ||
377 | lo = 0f; | ||
378 | rel_pos = (hiLimit - projAngle); | ||
379 | } | ||
380 | else | ||
381 | { | ||
382 | continue; | ||
383 | } | ||
384 | } | ||
385 | } | ||
386 | |||
387 | //impulse | ||
388 | |||
389 | float normalImpulse = -(tau * rel_pos / timeStep + damping * rel_vel) * jacDiagABInv; | ||
390 | float oldNormalImpulse = _accumulatedImpulse[i + 3]; | ||
391 | float sum = oldNormalImpulse + normalImpulse; | ||
392 | _accumulatedImpulse[i + 3] = sum > hi ? 0f : sum < lo ? 0f : sum; | ||
393 | normalImpulse = _accumulatedImpulse[i + 3] - oldNormalImpulse; | ||
394 | |||
395 | Vector3 axis2 = _sign[i] * Vector3.Cross(axisA, axisB); | ||
396 | Vector3 impulse_vector = axis2 * normalImpulse; | ||
397 | |||
398 | RigidBodyA.ApplyTorqueImpulse(impulse_vector); | ||
399 | RigidBodyB.ApplyTorqueImpulse(-impulse_vector); | ||
400 | } | ||
401 | } | ||
402 | } | ||
403 | |||
404 | //MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html | ||
405 | private bool MatrixToEulerXYZ(Matrix mat, out Vector3 xyz) | ||
406 | { | ||
407 | // rot = cy*cz -cy*sz sy | ||
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 | ||
410 | xyz = new Vector3(); | ||
411 | |||
412 | if (MathHelper.GetElement(mat, 2) < 1.0f) | ||
413 | { | ||
414 | if (MathHelper.GetElement(mat, 2) > -1.0f) | ||
415 | { | ||
416 | xyz.X = (float)Math.Atan2(-MathHelper.GetElement(mat, 5), MathHelper.GetElement(mat, 8)); | ||
417 | xyz.Y = (float)Math.Asin(MathHelper.GetElement(mat, 2)); | ||
418 | xyz.Z = (float)Math.Atan2(-MathHelper.GetElement(mat, 1), MathHelper.GetElement(mat, 0)); | ||
419 | return true; | ||
420 | } | ||
421 | else | ||
422 | { | ||
423 | // WARNING. Not unique. XA - ZA = -atan2(r10,r11) | ||
424 | xyz.X = -(float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); | ||
425 | xyz.Y = -(float)Math.PI / 2; | ||
426 | xyz.Z = 0.0f; | ||
427 | return false; | ||
428 | } | ||
429 | } | ||
430 | else | ||
431 | { | ||
432 | // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) | ||
433 | xyz.X = (float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); | ||
434 | xyz.Y = (float)Math.PI / 2; | ||
435 | xyz.Z = 0.0f; | ||
436 | return false; | ||
437 | } | ||
438 | } | ||
439 | } | ||
440 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs new file mode 100644 index 0000000..3e35550 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs | |||
@@ -0,0 +1,246 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX.Dynamics | ||
28 | { | ||
29 | /// <summary> | ||
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 | ||
32 | /// </summary> | ||
33 | public class HingeConstraint : TypedConstraint | ||
34 | { | ||
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 | ||
37 | |||
38 | private Vector3 _pivotInA; | ||
39 | private Vector3 _pivotInB; | ||
40 | private Vector3 _axisInA; | ||
41 | private Vector3 _axisInB; | ||
42 | |||
43 | private bool _angularOnly; | ||
44 | |||
45 | private float _motorTargetVelocity; | ||
46 | private float _maxMotorImpulse; | ||
47 | private bool _enableAngularMotor; | ||
48 | |||
49 | public HingeConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB, Vector3 axisInA, Vector3 axisInB) | ||
50 | : base(rbA, rbB) | ||
51 | { | ||
52 | _pivotInA = pivotInA; | ||
53 | _pivotInB = pivotInB; | ||
54 | _axisInA = axisInA; | ||
55 | _axisInB = -axisInB; | ||
56 | _angularOnly = false; | ||
57 | } | ||
58 | |||
59 | public HingeConstraint(RigidBody rbA, Vector3 pivotInA, Vector3 axisInA) | ||
60 | : base(rbA) | ||
61 | { | ||
62 | _pivotInA = pivotInA; | ||
63 | _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, pivotInA); | ||
64 | _axisInA = axisInA; | ||
65 | //fixed axis in worldspace | ||
66 | _axisInB = MathHelper.TransformNormal(-axisInA, rbA.CenterOfMassTransform); | ||
67 | _angularOnly = false; | ||
68 | } | ||
69 | |||
70 | public HingeConstraint() { } | ||
71 | |||
72 | public bool AngularOnly { set { _angularOnly = value; } } | ||
73 | |||
74 | public override void BuildJacobian() | ||
75 | { | ||
76 | AppliedImpulse = 0f; | ||
77 | |||
78 | Vector3 normal = new Vector3(); | ||
79 | |||
80 | if (!_angularOnly) | ||
81 | { | ||
82 | for (int i = 0; i < 3; i++) | ||
83 | { | ||
84 | MathHelper.SetElement(ref normal, i, 1); | ||
85 | _jac[i] = new JacobianEntry( | ||
86 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | ||
87 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | ||
88 | MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, | ||
89 | MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, | ||
90 | normal, | ||
91 | RigidBodyA.InvInertiaDiagLocal, | ||
92 | RigidBodyA.InverseMass, | ||
93 | RigidBodyB.InvInertiaDiagLocal, | ||
94 | RigidBodyB.InverseMass); | ||
95 | MathHelper.SetElement(ref normal, i, 0); | ||
96 | } | ||
97 | } | ||
98 | |||
99 | //calculate two perpendicular jointAxis, orthogonal to hingeAxis | ||
100 | //these two jointAxis require equal angular velocities for both bodies | ||
101 | //this is unused for now, it's a todo | ||
102 | Vector3 jointAxisALocal = new Vector3(); | ||
103 | Vector3 jointAxisBLocal = new Vector3(); | ||
104 | MathHelper.PlaneSpace1(_axisInA, ref jointAxisALocal, ref jointAxisBLocal); | ||
105 | |||
106 | Vector3 jointAxisA = MathHelper.TransformNormal(jointAxisALocal, RigidBodyA.CenterOfMassTransform); | ||
107 | Vector3 jointAxisB = MathHelper.TransformNormal(jointAxisBLocal, RigidBodyA.CenterOfMassTransform); | ||
108 | Vector3 hingeAxisWorld = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform); | ||
109 | |||
110 | _jacAng[0] = new JacobianEntry(jointAxisA, | ||
111 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | ||
112 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | ||
113 | RigidBodyA.InvInertiaDiagLocal, | ||
114 | RigidBodyB.InvInertiaDiagLocal); | ||
115 | |||
116 | _jacAng[1] = new JacobianEntry(jointAxisB, | ||
117 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | ||
118 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | ||
119 | RigidBodyA.InvInertiaDiagLocal, | ||
120 | RigidBodyB.InvInertiaDiagLocal); | ||
121 | |||
122 | _jacAng[2] = new JacobianEntry(hingeAxisWorld, | ||
123 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | ||
124 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | ||
125 | RigidBodyA.InvInertiaDiagLocal, | ||
126 | RigidBodyB.InvInertiaDiagLocal); | ||
127 | } | ||
128 | |||
129 | public override void SolveConstraint(float timeStep) | ||
130 | { | ||
131 | Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform); | ||
132 | Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform); | ||
133 | |||
134 | Vector3 normal = new Vector3(0, 0, 0); | ||
135 | float tau = 0.3f; | ||
136 | float damping = 1f; | ||
137 | |||
138 | //linear part | ||
139 | if (!_angularOnly) | ||
140 | { | ||
141 | for (int i = 0; i < 3; i++) | ||
142 | { | ||
143 | if (i == 0) | ||
144 | normal = new Vector3(1, 0, 0); | ||
145 | else if (i == 1) | ||
146 | normal = new Vector3(0, 1, 0); | ||
147 | else | ||
148 | normal = new Vector3(0, 0, 1); | ||
149 | |||
150 | float jacDiagABInv = 1f / _jac[i].Diagonal; | ||
151 | |||
152 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; | ||
153 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; | ||
154 | |||
155 | Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1); | ||
156 | Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2); | ||
157 | Vector3 vel = vel1 - vel2; | ||
158 | float rel_vel; | ||
159 | rel_vel = Vector3.Dot(normal, vel); | ||
160 | //positional error (zeroth order error) | ||
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; | ||
163 | AppliedImpulse += impulse; | ||
164 | Vector3 impulse_vector = normal * impulse; | ||
165 | RigidBodyA.ApplyImpulse(impulse_vector, pivotAInW - RigidBodyA.CenterOfMassPosition); | ||
166 | RigidBodyB.ApplyImpulse(-impulse_vector, pivotBInW - RigidBodyB.CenterOfMassPosition); | ||
167 | } | ||
168 | } | ||
169 | //solve angular part | ||
170 | // get axes in world space | ||
171 | Vector3 axisA = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform); | ||
172 | Vector3 axisB = MathHelper.TransformNormal(_axisInB, RigidBodyB.CenterOfMassTransform); | ||
173 | |||
174 | Vector3 angVelA = RigidBodyA.AngularVelocity; | ||
175 | Vector3 angVelB = RigidBodyB.AngularVelocity; | ||
176 | Vector3 angVelAroundHingeAxisA = axisA * Vector3.Dot(axisA, angVelA); | ||
177 | Vector3 angVelAroundHingeAxisB = axisB * Vector3.Dot(axisB, angVelB); | ||
178 | |||
179 | Vector3 angAOrthog = angVelA - angVelAroundHingeAxisA; | ||
180 | Vector3 angBOrthog = angVelB - angVelAroundHingeAxisB; | ||
181 | Vector3 velrelOrthog = angAOrthog - angBOrthog; | ||
182 | |||
183 | //solve angular velocity correction | ||
184 | float relaxation = 1f; | ||
185 | float len = velrelOrthog.Length(); | ||
186 | if (len > 0.00001f) | ||
187 | { | ||
188 | Vector3 normal2 = Vector3.Normalize(velrelOrthog); | ||
189 | float denom = RigidBodyA.ComputeAngularImpulseDenominator(normal2) + | ||
190 | RigidBodyB.ComputeAngularImpulseDenominator(normal2); | ||
191 | // scale for mass and relaxation | ||
192 | velrelOrthog *= (1f / denom) * 0.9f; | ||
193 | } | ||
194 | |||
195 | //solve angular positional correction | ||
196 | Vector3 angularError = -Vector3.Cross(axisA, axisB) * (1f / timeStep); | ||
197 | float len2 = angularError.Length(); | ||
198 | if (len2 > 0.00001f) | ||
199 | { | ||
200 | Vector3 normal2 = Vector3.Normalize(angularError); | ||
201 | float denom2 = RigidBodyA.ComputeAngularImpulseDenominator(normal2) + | ||
202 | RigidBodyB.ComputeAngularImpulseDenominator(normal2); | ||
203 | angularError *= (1f / denom2) * relaxation; | ||
204 | } | ||
205 | |||
206 | RigidBodyA.ApplyTorqueImpulse(-velrelOrthog + angularError); | ||
207 | RigidBodyB.ApplyTorqueImpulse(velrelOrthog - angularError); | ||
208 | |||
209 | //apply motor | ||
210 | if (_enableAngularMotor) | ||
211 | { | ||
212 | //todo: add limits too | ||
213 | Vector3 angularLimit = Vector3.Zero; | ||
214 | |||
215 | Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; | ||
216 | float projRelVel = Vector3.Dot(velrel, axisA); | ||
217 | |||
218 | float desiredMotorVel = _motorTargetVelocity; | ||
219 | float motorRelvel = desiredMotorVel - projRelVel; | ||
220 | |||
221 | float denom3 = RigidBodyA.ComputeAngularImpulseDenominator(axisA) + | ||
222 | RigidBodyB.ComputeAngularImpulseDenominator(axisA); | ||
223 | |||
224 | float unclippedMotorImpulse = (1f / denom3) * motorRelvel; | ||
225 | //todo: should clip against accumulated impulse | ||
226 | float clippedMotorImpulse = unclippedMotorImpulse > _maxMotorImpulse ? _maxMotorImpulse : unclippedMotorImpulse; | ||
227 | clippedMotorImpulse = clippedMotorImpulse < -_maxMotorImpulse ? -_maxMotorImpulse : clippedMotorImpulse; | ||
228 | Vector3 motorImp = clippedMotorImpulse * axisA; | ||
229 | |||
230 | RigidBodyA.ApplyTorqueImpulse(motorImp + angularLimit); | ||
231 | RigidBodyB.ApplyTorqueImpulse(-motorImp - angularLimit); | ||
232 | } | ||
233 | } | ||
234 | |||
235 | public void EnableAngularMotor(bool enableMotor, float targetVelocity, float maxMotorImpulse) | ||
236 | { | ||
237 | _enableAngularMotor = enableMotor; | ||
238 | _motorTargetVelocity = targetVelocity; | ||
239 | _maxMotorImpulse = maxMotorImpulse; | ||
240 | } | ||
241 | |||
242 | public void UpdateRHS(float timeStep) | ||
243 | { | ||
244 | } | ||
245 | } | ||
246 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs new file mode 100644 index 0000000..de25225 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs | |||
@@ -0,0 +1,32 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX.Dynamics | ||
27 | { | ||
28 | public interface IConstraintSolver | ||
29 | { | ||
30 | float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo info, IDebugDraw debugDrawer); | ||
31 | } | ||
32 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs new file mode 100644 index 0000000..3b8d8d8 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs | |||
@@ -0,0 +1,155 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX.Dynamics | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// Jacobian entry is an abstraction that allows to describe constraints | ||
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 | ||
33 | /// </summary> | ||
34 | public class JacobianEntry | ||
35 | { | ||
36 | private Vector3 _linearJointAxis; | ||
37 | private Vector3 _aJ; | ||
38 | private Vector3 _bJ; | ||
39 | private Vector3 _0MinvJt; | ||
40 | private Vector3 _1MinvJt; | ||
41 | private float _adiag; | ||
42 | |||
43 | public JacobianEntry() { } | ||
44 | |||
45 | //constraint between two different rigidbodies | ||
46 | public JacobianEntry( | ||
47 | Matrix world2A, | ||
48 | Matrix world2B, | ||
49 | Vector3 relPosA, Vector3 relPosB, | ||
50 | Vector3 jointAxis, | ||
51 | Vector3 inertiaInvA, | ||
52 | float massInvA, | ||
53 | Vector3 inertiaInvB, | ||
54 | float massInvB) | ||
55 | { | ||
56 | _linearJointAxis = jointAxis; | ||
57 | _aJ = Vector3.TransformNormal(Vector3.Cross(relPosA, _linearJointAxis), world2A); | ||
58 | _bJ = Vector3.TransformNormal(Vector3.Cross(relPosB, -_linearJointAxis), world2B); | ||
59 | _0MinvJt = inertiaInvA * _aJ; | ||
60 | _1MinvJt = inertiaInvB * _bJ; | ||
61 | _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ) + massInvB + Vector3.Dot(_1MinvJt, _bJ); | ||
62 | |||
63 | if (_adiag <= 0.0f) | ||
64 | throw new BulletException(); | ||
65 | } | ||
66 | |||
67 | //angular constraint between two different rigidbodies | ||
68 | public JacobianEntry(Vector3 jointAxis, | ||
69 | Matrix world2A, | ||
70 | Matrix world2B, | ||
71 | Vector3 inertiaInvA, | ||
72 | Vector3 inertiaInvB) | ||
73 | { | ||
74 | _linearJointAxis = new Vector3(); | ||
75 | _aJ = Vector3.TransformNormal(jointAxis, world2A); | ||
76 | _bJ = Vector3.TransformNormal(-jointAxis, world2B); | ||
77 | _0MinvJt = inertiaInvA * _aJ; | ||
78 | _1MinvJt = inertiaInvB * _bJ; | ||
79 | _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ); | ||
80 | |||
81 | if (_adiag <= 0.0f) | ||
82 | throw new BulletException(); | ||
83 | } | ||
84 | |||
85 | //angular constraint between two different rigidbodies | ||
86 | public JacobianEntry(Vector3 axisInA, | ||
87 | Vector3 axisInB, | ||
88 | Vector3 inertiaInvA, | ||
89 | Vector3 inertiaInvB) | ||
90 | { | ||
91 | _linearJointAxis = new Vector3(); | ||
92 | _aJ = axisInA; | ||
93 | _bJ = -axisInB; | ||
94 | _0MinvJt = inertiaInvA * _aJ; | ||
95 | _1MinvJt = inertiaInvB * _bJ; | ||
96 | _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ); | ||
97 | |||
98 | if (_adiag <= 0.0f) | ||
99 | throw new BulletException(); | ||
100 | } | ||
101 | |||
102 | //constraint on one rigidbody | ||
103 | public JacobianEntry( | ||
104 | Matrix world2A, | ||
105 | Vector3 rel_pos1, Vector3 rel_pos2, | ||
106 | Vector3 jointAxis, | ||
107 | Vector3 inertiaInvA, | ||
108 | float massInvA) | ||
109 | { | ||
110 | _linearJointAxis = jointAxis; | ||
111 | _aJ = Vector3.TransformNormal(Vector3.Cross(rel_pos1, jointAxis), world2A); | ||
112 | _bJ = Vector3.TransformNormal(Vector3.Cross(rel_pos2, -jointAxis), world2A); | ||
113 | _0MinvJt = inertiaInvA * _aJ; | ||
114 | _1MinvJt = new Vector3(); | ||
115 | _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ); | ||
116 | |||
117 | if (_adiag <= 0.0f) | ||
118 | throw new BulletException(); | ||
119 | } | ||
120 | |||
121 | public float Diagonal { get { return _adiag; } } | ||
122 | |||
123 | // for two constraints on the same rigidbody (for example vehicle friction) | ||
124 | public float GetNonDiagonal(JacobianEntry jacB, float massInvA) | ||
125 | { | ||
126 | float lin = massInvA * Vector3.Dot(_linearJointAxis, jacB._linearJointAxis); | ||
127 | float ang = Vector3.Dot(_0MinvJt, jacB._aJ); | ||
128 | return lin + ang; | ||
129 | } | ||
130 | |||
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) | ||
133 | { | ||
134 | Vector3 lin = _linearJointAxis * jacB._linearJointAxis; | ||
135 | Vector3 ang0 = _0MinvJt * jacB._aJ; | ||
136 | Vector3 ang1 = _1MinvJt * jacB._bJ; | ||
137 | Vector3 lin0 = massInvA * lin; | ||
138 | Vector3 lin1 = massInvB * lin; | ||
139 | Vector3 sum = ang0 + ang1 + lin0 + lin1; | ||
140 | return sum.X + sum.Y + sum.Z; | ||
141 | } | ||
142 | |||
143 | public float GetRelativeVelocity(Vector3 linvelA, Vector3 angvelA, Vector3 linvelB, Vector3 angvelB) | ||
144 | { | ||
145 | Vector3 linrel = linvelA - linvelB; | ||
146 | Vector3 angvela = angvelA * _aJ; | ||
147 | Vector3 angvelb = angvelB * _bJ; | ||
148 | linrel *= _linearJointAxis; | ||
149 | angvela += angvelb; | ||
150 | angvela += linrel; | ||
151 | float rel_vel2 = angvela.X + angvela.Y + angvela.Z; | ||
152 | return rel_vel2 + float.Epsilon; | ||
153 | } | ||
154 | } | ||
155 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs new file mode 100644 index 0000000..d4206ea --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs | |||
@@ -0,0 +1,157 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX.Dynamics | ||
28 | { | ||
29 | public class ConstraintSetting | ||
30 | { | ||
31 | private float _tau, _damping; | ||
32 | |||
33 | public ConstraintSetting() | ||
34 | { | ||
35 | _tau = 0.3f; | ||
36 | _damping = 1.0f; | ||
37 | } | ||
38 | |||
39 | public float Damping | ||
40 | { | ||
41 | get { return _damping; } | ||
42 | set { _damping = value; } | ||
43 | } | ||
44 | |||
45 | public float Tau | ||
46 | { | ||
47 | get { return _tau; } | ||
48 | set { _tau = value; } | ||
49 | } | ||
50 | } | ||
51 | |||
52 | public class Point2PointConstraint : TypedConstraint | ||
53 | { | ||
54 | private JacobianEntry[] _jacobian; | ||
55 | private Vector3 _pivotInA, _pivotInB; | ||
56 | |||
57 | private ConstraintSetting _setting = new ConstraintSetting(); | ||
58 | |||
59 | public Point2PointConstraint() | ||
60 | { | ||
61 | _jacobian = new JacobianEntry[3]; | ||
62 | } | ||
63 | |||
64 | public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB) | ||
65 | : base(rbA, rbB) | ||
66 | { | ||
67 | _jacobian = new JacobianEntry[3]; | ||
68 | |||
69 | _pivotInA = pivotInA; | ||
70 | _pivotInB = pivotInB; | ||
71 | } | ||
72 | |||
73 | public Point2PointConstraint(RigidBody rbA, Vector3 pivotInA) | ||
74 | : base(rbA) | ||
75 | { | ||
76 | _jacobian = new JacobianEntry[3]; | ||
77 | |||
78 | _pivotInA = pivotInA; | ||
79 | _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, _pivotInA); | ||
80 | } | ||
81 | |||
82 | public ConstraintSetting Settings { get { return _setting; } set { _setting = value; } } | ||
83 | |||
84 | public Vector3 PivotInA | ||
85 | { | ||
86 | set | ||
87 | { | ||
88 | _pivotInA = value; | ||
89 | } | ||
90 | } | ||
91 | |||
92 | public Vector3 PivotInB | ||
93 | { | ||
94 | set | ||
95 | { | ||
96 | _pivotInB = value; | ||
97 | } | ||
98 | } | ||
99 | |||
100 | public override void BuildJacobian() | ||
101 | { | ||
102 | Vector3 normal = new Vector3(); | ||
103 | |||
104 | for (int i = 0; i < 3; i++) | ||
105 | { | ||
106 | MathHelper.SetElement(ref normal, i, 1); | ||
107 | _jacobian[i] = new JacobianEntry( | ||
108 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | ||
109 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | ||
110 | MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, | ||
111 | MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, | ||
112 | normal, | ||
113 | RigidBodyA.InvInertiaDiagLocal, | ||
114 | RigidBodyA.InverseMass, | ||
115 | RigidBodyB.InvInertiaDiagLocal, | ||
116 | RigidBodyB.InverseMass | ||
117 | ); | ||
118 | MathHelper.SetElement(ref normal, i, 0); | ||
119 | } | ||
120 | } | ||
121 | |||
122 | public override void SolveConstraint(float timeStep) | ||
123 | { | ||
124 | Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform); | ||
125 | Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform); | ||
126 | |||
127 | Vector3 normal = new Vector3(); | ||
128 | |||
129 | for (int i = 0; i < 3; i++) | ||
130 | { | ||
131 | MathHelper.SetElement(ref normal, i, 1); | ||
132 | |||
133 | float jacDiagABInv = 1.0f / _jacobian[i].Diagonal; | ||
134 | |||
135 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; | ||
136 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; | ||
137 | |||
138 | Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1); | ||
139 | Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2); | ||
140 | |||
141 | Vector3 vel = vel1 - vel2; | ||
142 | |||
143 | float rel_vel = Vector3.Dot(normal, vel); | ||
144 | float depth = -Vector3.Dot((pivotAInW - pivotBInW), normal); | ||
145 | |||
146 | float impulse = depth * _setting.Tau / timeStep * jacDiagABInv - _setting.Damping * rel_vel * jacDiagABInv; | ||
147 | AppliedImpulse += impulse; | ||
148 | Vector3 impulseVector = normal * impulse; | ||
149 | |||
150 | RigidBodyA.ApplyImpulse(impulseVector, pivotAInW - RigidBodyA.CenterOfMassPosition); | ||
151 | RigidBodyB.ApplyImpulse(-impulseVector, pivotBInW - RigidBodyB.CenterOfMassPosition); | ||
152 | |||
153 | MathHelper.SetElement(ref normal, i, 0); | ||
154 | } | ||
155 | } | ||
156 | } | ||
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 new file mode 100644 index 0000000..4d5e93d --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs | |||
@@ -0,0 +1,915 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX.Dynamics | ||
28 | { | ||
29 | [Flags] | ||
30 | public enum SolverMode | ||
31 | { | ||
32 | None = 0, | ||
33 | RandomizeOrder = 1, | ||
34 | FrictionSeperate = 2, | ||
35 | UseWarmstarting = 4, | ||
36 | CacheFriendly = 8, | ||
37 | } | ||
38 | |||
39 | public class SequentialImpulseConstraintSolver : IConstraintSolver | ||
40 | { | ||
41 | private static int _totalContactPoints = 0; | ||
42 | |||
43 | private SolverMode _solverMode; | ||
44 | private int _totalCpd = 0; | ||
45 | private ContactSolverFunc[,] _contactDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType]; | ||
46 | private ContactSolverFunc[,] _frictionDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType]; | ||
47 | |||
48 | private float _penetrationResolveFactor = 0.9f; | ||
49 | private List<SolverBody> _tmpSolverBodyPool = new List<SolverBody>(); | ||
50 | private List<SolverConstraint> _tmpSolverConstraintPool = new List<SolverConstraint>(); | ||
51 | private List<SolverConstraint> _tmpSolverFrictionConstraintPool = new List<SolverConstraint>(); | ||
52 | |||
53 | private const int _sequentialImpulseMaxSolverPoints = 16384; | ||
54 | private static OrderIndex[] _order = new OrderIndex[SequentialImpulseMaxSolverPoints]; | ||
55 | private static long _seed2 = 0; | ||
56 | |||
57 | public SequentialImpulseConstraintSolver() | ||
58 | { | ||
59 | _solverMode = SolverMode.RandomizeOrder | SolverMode.CacheFriendly; | ||
60 | PersistentManifold.ContactDestroyedCallback = MyContactDestroyedCallback; | ||
61 | |||
62 | //initialize default friction/contact funcs | ||
63 | int i, j; | ||
64 | for (i = 0; i < (int)ContactSolverType.MaxContactSolverType; i++) | ||
65 | for (j = 0; j < (int)ContactSolverType.MaxContactSolverType; j++) | ||
66 | { | ||
67 | |||
68 | _contactDispatch[i, j] = ContactConstraint.ResolveSingleCollision; | ||
69 | _frictionDispatch[i, j] = ContactConstraint.ResolveSingleFriction; | ||
70 | } | ||
71 | } | ||
72 | |||
73 | public SolverMode SolverMode { get { return _solverMode; } set { _solverMode = value; } } | ||
74 | public static int SequentialImpulseMaxSolverPoints { get { return _sequentialImpulseMaxSolverPoints; } } | ||
75 | protected static OrderIndex[] Order { get { return _order; } set { _order = value; } } | ||
76 | public static long RandSeed { get { return _seed2; } set { _seed2 = value; } } | ||
77 | |||
78 | ///<summary> | ||
79 | /// Advanced: Override the default contact solving function for contacts, for certain types of rigidbody | ||
80 | /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType | ||
81 | ///</summary> | ||
82 | public void SetContactSolverFunc(ContactSolverFunc func, int typeA, int typeB) | ||
83 | { | ||
84 | _contactDispatch[typeA, typeB] = func; | ||
85 | } | ||
86 | |||
87 | /// <summary> | ||
88 | /// Advanced: Override the default friction solving function for contacts, for certain types of rigidbody | ||
89 | /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType | ||
90 | ///</summary> | ||
91 | public void SetFrictionSolverFunc(ContactSolverFunc func, int typeA, int typeB) | ||
92 | { | ||
93 | _frictionDispatch[typeA, typeB] = func; | ||
94 | } | ||
95 | |||
96 | protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) | ||
97 | { | ||
98 | float maxImpulse = 0; | ||
99 | |||
100 | Vector3 color = new Vector3(0, 1, 0); | ||
101 | if (cp.Distance <= 0) | ||
102 | { | ||
103 | if (iter == 0) | ||
104 | if(debugDraw != null) | ||
105 | debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); | ||
106 | |||
107 | ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; | ||
108 | float impulse = cpd.ContactSolverFunc( | ||
109 | bodyA, bodyB, | ||
110 | cp, | ||
111 | info); | ||
112 | |||
113 | if (maxImpulse < impulse) | ||
114 | maxImpulse = impulse; | ||
115 | } | ||
116 | return maxImpulse; | ||
117 | } | ||
118 | |||
119 | protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter) | ||
120 | { | ||
121 | return Solve(bodyA, bodyB, cp, info, iter, null); | ||
122 | } | ||
123 | |||
124 | protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) | ||
125 | { | ||
126 | float maxImpulse = 0; | ||
127 | |||
128 | Vector3 color = new Vector3(0, 1, 0); | ||
129 | if (cp.Distance <= 0) | ||
130 | { | ||
131 | if (iter == 0) | ||
132 | if (debugDraw != null) | ||
133 | debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); | ||
134 | |||
135 | float impulse = ContactConstraint.ResolveSingleCollisionCombined( | ||
136 | bodyA, bodyB, | ||
137 | cp, | ||
138 | info); | ||
139 | |||
140 | if (maxImpulse < impulse) | ||
141 | maxImpulse = impulse; | ||
142 | } | ||
143 | return maxImpulse; | ||
144 | } | ||
145 | |||
146 | protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) | ||
147 | { | ||
148 | Vector3 color = new Vector3(0, 1, 0); | ||
149 | |||
150 | if (cp.Distance <= 0) | ||
151 | { | ||
152 | |||
153 | ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; | ||
154 | cpd.FrictionSolverFunc( | ||
155 | bodyA, bodyB, | ||
156 | cp, | ||
157 | info); | ||
158 | } | ||
159 | return 0; | ||
160 | } | ||
161 | |||
162 | protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info) | ||
163 | { | ||
164 | RigidBody body0 = manifold.BodyA as RigidBody; | ||
165 | RigidBody body1 = manifold.BodyB as RigidBody; | ||
166 | |||
167 | |||
168 | //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop | ||
169 | { | ||
170 | manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform); | ||
171 | |||
172 | int numpoints = manifold.ContactsCount; | ||
173 | |||
174 | _totalContactPoints += numpoints; | ||
175 | |||
176 | Vector3 color = new Vector3(0, 1, 0); | ||
177 | for (int i = 0; i < numpoints; i++) | ||
178 | { | ||
179 | ManifoldPoint cp = manifold.GetContactPoint(i); | ||
180 | if (cp.Distance <= 0) | ||
181 | { | ||
182 | Vector3 pos1 = cp.PositionWorldOnA; | ||
183 | Vector3 pos2 = cp.PositionWorldOnB; | ||
184 | |||
185 | Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition; | ||
186 | Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition; | ||
187 | |||
188 | |||
189 | //this jacobian entry is re-used for all iterations | ||
190 | JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform), | ||
191 | MatrixOperations.Transpose(body1.CenterOfMassTransform), | ||
192 | rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass, | ||
193 | body1.InvInertiaDiagLocal, body1.InverseMass); | ||
194 | |||
195 | float jacDiagAB = jac.Diagonal; | ||
196 | |||
197 | ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; | ||
198 | if (cpd != null) | ||
199 | { | ||
200 | //might be invalid | ||
201 | cpd.PersistentLifeTime++; | ||
202 | if (cpd.PersistentLifeTime != cp.LifeTime) | ||
203 | { | ||
204 | //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); | ||
205 | cpd = new ConstraintPersistentData(); | ||
206 | cpd.PersistentLifeTime = cp.LifeTime; | ||
207 | |||
208 | } | ||
209 | } | ||
210 | else | ||
211 | { | ||
212 | |||
213 | cpd = new ConstraintPersistentData(); | ||
214 | _totalCpd++; | ||
215 | //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); | ||
216 | cp.UserPersistentData = cpd; | ||
217 | cpd.PersistentLifeTime = cp.LifeTime; | ||
218 | //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime()); | ||
219 | |||
220 | } | ||
221 | if (cpd == null) | ||
222 | throw new BulletException(); | ||
223 | |||
224 | cpd.JacDiagABInv = 1f / jacDiagAB; | ||
225 | |||
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... | ||
228 | |||
229 | cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType]; | ||
230 | cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType]; | ||
231 | |||
232 | Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1); | ||
233 | Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2); | ||
234 | Vector3 vel = vel1 - vel2; | ||
235 | float rel_vel; | ||
236 | rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); | ||
237 | |||
238 | float combinedRestitution = cp.CombinedRestitution; | ||
239 | |||
240 | cpd.Penetration = cp.Distance; | ||
241 | cpd.Friction = cp.CombinedFriction; | ||
242 | cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution); | ||
243 | if (cpd.Restitution < 0f) | ||
244 | { | ||
245 | cpd.Restitution = 0.0f; | ||
246 | |||
247 | }; | ||
248 | |||
249 | //restitution and penetration work in same direction so | ||
250 | //rel_vel | ||
251 | |||
252 | float penVel = -cpd.Penetration / info.TimeStep; | ||
253 | |||
254 | if (cpd.Restitution > penVel) | ||
255 | { | ||
256 | cpd.Penetration = 0; | ||
257 | } | ||
258 | |||
259 | |||
260 | float relaxation = info.Damping; | ||
261 | if ((_solverMode & SolverMode.UseWarmstarting) != 0) | ||
262 | { | ||
263 | cpd.AppliedImpulse *= relaxation; | ||
264 | } | ||
265 | else | ||
266 | { | ||
267 | cpd.AppliedImpulse = 0f; | ||
268 | } | ||
269 | |||
270 | //for friction | ||
271 | cpd.PreviousAppliedImpulse = cpd.AppliedImpulse; | ||
272 | |||
273 | //re-calculate friction direction every frame, todo: check if this is really needed | ||
274 | Vector3 fwta = cpd.FrictionWorldTangentialA; | ||
275 | Vector3 fwtb = cpd.FrictionWorldTangentialB; | ||
276 | MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb); | ||
277 | cpd.FrictionWorldTangentialA = fwta; | ||
278 | cpd.FrictionWorldTangentialB = fwtb; | ||
279 | |||
280 | cpd.AccumulatedTangentImpulseA = 0; | ||
281 | cpd.AccumulatedTangentImpulseB = 0; | ||
282 | float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA); | ||
283 | float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA); | ||
284 | float denom = relaxation / (denom0 + denom1); | ||
285 | cpd.JacDiagABInvTangentA = denom; | ||
286 | |||
287 | |||
288 | denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB); | ||
289 | denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB); | ||
290 | denom = relaxation / (denom0 + denom1); | ||
291 | cpd.JacDiagABInvTangentB = denom; | ||
292 | |||
293 | Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse; | ||
294 | |||
295 | { | ||
296 | Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); | ||
297 | cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld); | ||
298 | Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); | ||
299 | cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld); | ||
300 | } | ||
301 | { | ||
302 | Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA); | ||
303 | cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld); | ||
304 | } | ||
305 | { | ||
306 | Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB); | ||
307 | cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld); | ||
308 | } | ||
309 | { | ||
310 | Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA); | ||
311 | cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld); | ||
312 | } | ||
313 | { | ||
314 | Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB); | ||
315 | cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld); | ||
316 | } | ||
317 | |||
318 | |||
319 | //apply previous frames impulse on both bodies | ||
320 | body0.ApplyImpulse(totalImpulse, rel_pos1); | ||
321 | body1.ApplyImpulse(-totalImpulse, rel_pos2); | ||
322 | } | ||
323 | } | ||
324 | } | ||
325 | } | ||
326 | |||
327 | private bool MyContactDestroyedCallback(object userPersistentData) | ||
328 | { | ||
329 | if (userPersistentData == null) | ||
330 | throw new BulletException(); | ||
331 | ConstraintPersistentData cpd = userPersistentData as ConstraintPersistentData; | ||
332 | _totalCpd--; | ||
333 | return true; | ||
334 | } | ||
335 | |||
336 | private float RestitutionCurve(float relVel, float restitution) | ||
337 | { | ||
338 | float rest = restitution * -relVel; | ||
339 | return rest; | ||
340 | } | ||
341 | |||
342 | //velocity + friction | ||
343 | //response between two dynamic objects with friction | ||
344 | public virtual float ResolveSingleCollisionCombinedCacheFriendly( | ||
345 | SolverBody bodyA, | ||
346 | SolverBody bodyB, | ||
347 | SolverConstraint contactConstraint, | ||
348 | ContactSolverInfo solverInfo) | ||
349 | { | ||
350 | float normalImpulse = 0; | ||
351 | |||
352 | if (contactConstraint.Penetration < 0) | ||
353 | return 0; | ||
354 | |||
355 | float relVel; | ||
356 | float velADotn = Vector3.Dot(contactConstraint.ContactNormal,bodyA.LinearVelocity) | ||
357 | + Vector3.Dot(contactConstraint.RelPosACrossNormal,bodyA.AngularVelocity); | ||
358 | float velBDotn = Vector3.Dot(contactConstraint.ContactNormal,bodyB.LinearVelocity) | ||
359 | + Vector3.Dot(contactConstraint.RelPosBCrossNormal,bodyB.AngularVelocity); | ||
360 | |||
361 | relVel = velADotn - velBDotn; | ||
362 | |||
363 | float positionalError = contactConstraint.Penetration; | ||
364 | float velocityError = contactConstraint.Restitution - relVel;// * damping; | ||
365 | |||
366 | float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv; | ||
367 | float velocityImpulse = velocityError * contactConstraint.JacDiagABInv; | ||
368 | normalImpulse = penetrationImpulse + velocityImpulse; | ||
369 | |||
370 | // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse | ||
371 | float oldNormalImpulse = contactConstraint.AppliedImpulse; | ||
372 | float sum = oldNormalImpulse + normalImpulse; | ||
373 | contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum; | ||
374 | |||
375 | float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse; | ||
376 | float velocitySum = oldVelocityImpulse + velocityImpulse; | ||
377 | contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum; | ||
378 | |||
379 | normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse; | ||
380 | |||
381 | if (bodyA.InvMass != 0) | ||
382 | { | ||
383 | bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, | ||
384 | contactConstraint.AngularComponentA, normalImpulse); | ||
385 | } | ||
386 | if (bodyB.InvMass != 0) | ||
387 | { | ||
388 | bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, | ||
389 | contactConstraint.AngularComponentB, -normalImpulse); | ||
390 | } | ||
391 | |||
392 | return normalImpulse; | ||
393 | } | ||
394 | |||
395 | public virtual float ResolveSingleFrictionCacheFriendly( | ||
396 | SolverBody bodyA, | ||
397 | SolverBody bodyB, | ||
398 | SolverConstraint contactConstraint, | ||
399 | ContactSolverInfo solverInfo, | ||
400 | float appliedNormalImpulse) | ||
401 | { | ||
402 | float combinedFriction = contactConstraint.Friction; | ||
403 | float limit = appliedNormalImpulse * combinedFriction; | ||
404 | |||
405 | if (appliedNormalImpulse > 0) | ||
406 | //friction | ||
407 | { | ||
408 | float j1; | ||
409 | { | ||
410 | float relVel; | ||
411 | float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity) | ||
412 | + Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity); | ||
413 | float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity) | ||
414 | + Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity); | ||
415 | relVel = velADotn - velBDotn; | ||
416 | |||
417 | // calculate j that moves us to zero relative velocity | ||
418 | j1 = -relVel * contactConstraint.JacDiagABInv; | ||
419 | float oldTangentImpulse = contactConstraint.AppliedImpulse; | ||
420 | contactConstraint.AppliedImpulse = oldTangentImpulse + j1; | ||
421 | |||
422 | float test = contactConstraint.AppliedImpulse; | ||
423 | MathHelper.SetMin(ref test, limit); | ||
424 | MathHelper.SetMax(ref test, -limit); | ||
425 | contactConstraint.AppliedImpulse = test; | ||
426 | |||
427 | j1 = contactConstraint.AppliedImpulse - oldTangentImpulse; | ||
428 | } | ||
429 | |||
430 | if (bodyA.InvMass != 0) | ||
431 | { | ||
432 | bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1); | ||
433 | } | ||
434 | if (bodyB.InvMass != 0) | ||
435 | { | ||
436 | bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1); | ||
437 | } | ||
438 | } | ||
439 | return 0; | ||
440 | } | ||
441 | |||
442 | public virtual float SolveGroupCacheFriendly(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) | ||
443 | { | ||
444 | if (constraints.Count + numManifolds == 0) | ||
445 | { | ||
446 | return 0; | ||
447 | } | ||
448 | |||
449 | for (int i = 0; i < numManifolds; i++) | ||
450 | { | ||
451 | PersistentManifold manifold = manifolds[i]; | ||
452 | RigidBody rbA = (RigidBody)manifold.BodyA; | ||
453 | RigidBody rbB = (RigidBody)manifold.BodyB; | ||
454 | |||
455 | manifold.RefreshContactPoints(rbA.CenterOfMassTransform, rbB.CenterOfMassTransform); | ||
456 | } | ||
457 | |||
458 | int minReservation = manifolds.Count * 2; | ||
459 | |||
460 | _tmpSolverBodyPool = new List<SolverBody>(minReservation); | ||
461 | |||
462 | for (int i = 0; i < bodies.Count; i++) | ||
463 | { | ||
464 | RigidBody rb = RigidBody.Upcast(bodies[i]); | ||
465 | if (rb != null && rb.IslandTag >= 0) | ||
466 | { | ||
467 | BulletDebug.Assert(rb.CompanionID < 0); | ||
468 | int solverBodyId = _tmpSolverBodyPool.Count; | ||
469 | SolverBody solverBody; | ||
470 | InitSolverBody(out solverBody, rb); | ||
471 | _tmpSolverBodyPool.Add(solverBody); | ||
472 | rb.CompanionID = solverBodyId; | ||
473 | } | ||
474 | } | ||
475 | |||
476 | _tmpSolverConstraintPool = new List<SolverConstraint>(minReservation); | ||
477 | _tmpSolverFrictionConstraintPool = new List<SolverConstraint>(minReservation); | ||
478 | |||
479 | for (int i = 0; i < numManifolds; i++) | ||
480 | { | ||
481 | PersistentManifold manifold = manifolds[i]; | ||
482 | RigidBody rb0 = (RigidBody)manifold.BodyA; | ||
483 | RigidBody rb1 = (RigidBody)manifold.BodyB; | ||
484 | |||
485 | int solverBodyIdA = -1; | ||
486 | int solverBodyIdB = -1; | ||
487 | |||
488 | //if (i == 89) | ||
489 | // System.Diagnostics.Debugger.Break(); | ||
490 | |||
491 | if (manifold.ContactsCount != 0) | ||
492 | { | ||
493 | if (rb0.IslandTag >= 0) | ||
494 | { | ||
495 | solverBodyIdA = rb0.CompanionID; | ||
496 | } | ||
497 | else | ||
498 | { | ||
499 | //create a static body | ||
500 | solverBodyIdA = _tmpSolverBodyPool.Count; | ||
501 | SolverBody solverBody; | ||
502 | InitSolverBody(out solverBody, rb0); | ||
503 | _tmpSolverBodyPool.Add(solverBody); | ||
504 | } | ||
505 | |||
506 | if (rb1.IslandTag >= 0) | ||
507 | { | ||
508 | solverBodyIdB = rb1.CompanionID; | ||
509 | } | ||
510 | else | ||
511 | { | ||
512 | //create a static body | ||
513 | solverBodyIdB = _tmpSolverBodyPool.Count; | ||
514 | SolverBody solverBody; | ||
515 | InitSolverBody(out solverBody, rb1); | ||
516 | _tmpSolverBodyPool.Add(solverBody); | ||
517 | } | ||
518 | } | ||
519 | |||
520 | if (solverBodyIdB == -1 || solverBodyIdA == -1) | ||
521 | System.Diagnostics.Debug.WriteLine(string.Format("We're in ass ! {0}", i)); | ||
522 | |||
523 | for (int j = 0; j < manifold.ContactsCount; j++) | ||
524 | { | ||
525 | ManifoldPoint cp = manifold.GetContactPoint(j); | ||
526 | |||
527 | int frictionIndex = _tmpSolverConstraintPool.Count; | ||
528 | |||
529 | if (cp.Distance <= 0) | ||
530 | { | ||
531 | |||
532 | Vector3 pos1 = cp.PositionWorldOnA; | ||
533 | Vector3 pos2 = cp.PositionWorldOnB; | ||
534 | |||
535 | Vector3 rel_pos1 = pos1 - rb0.CenterOfMassPosition; | ||
536 | Vector3 rel_pos2 = pos2 - rb1.CenterOfMassPosition; | ||
537 | |||
538 | float relaxation = 1; | ||
539 | { | ||
540 | SolverConstraint solverConstraint = new SolverConstraint(); | ||
541 | _tmpSolverConstraintPool.Add(solverConstraint); | ||
542 | |||
543 | solverConstraint.SolverBodyIdA = solverBodyIdA; | ||
544 | solverConstraint.SolverBodyIdB = solverBodyIdB; | ||
545 | solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Contact; | ||
546 | |||
547 | //can be optimized, the cross products are already calculated | ||
548 | float denom0 = rb0.ComputeImpulseDenominator(pos1, cp.NormalWorldOnB); | ||
549 | float denom1 = rb1.ComputeImpulseDenominator(pos2, cp.NormalWorldOnB); | ||
550 | float denom = relaxation / (denom0 + denom1); | ||
551 | solverConstraint.JacDiagABInv = denom; | ||
552 | |||
553 | solverConstraint.ContactNormal = cp.NormalWorldOnB; | ||
554 | solverConstraint.RelPosACrossNormal = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); | ||
555 | solverConstraint.RelPosBCrossNormal = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); | ||
556 | |||
557 | Vector3 vel1 = rb0.GetVelocityInLocalPoint(rel_pos1); | ||
558 | Vector3 vel2 = rb1.GetVelocityInLocalPoint(rel_pos2); | ||
559 | |||
560 | Vector3 vel = vel1 - vel2; | ||
561 | float rel_vel; | ||
562 | rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); | ||
563 | |||
564 | |||
565 | solverConstraint.Penetration = cp.Distance;//btScalar(infoGlobal.m_numIterations); | ||
566 | solverConstraint.Friction = cp.CombinedFriction; | ||
567 | float rest = RestitutionCurve(rel_vel, cp.CombinedRestitution); | ||
568 | if (rest <= 0) | ||
569 | { | ||
570 | rest = 0; | ||
571 | } | ||
572 | |||
573 | float penVel = -solverConstraint.Penetration / infoGlobal.TimeStep; | ||
574 | if (rest > penVel) | ||
575 | { | ||
576 | rest = 0; | ||
577 | } | ||
578 | solverConstraint.Restitution = rest; | ||
579 | |||
580 | solverConstraint.Penetration *= -(infoGlobal.Erp / infoGlobal.TimeStep); | ||
581 | |||
582 | solverConstraint.AppliedImpulse = 0f; | ||
583 | solverConstraint.AppliedVelocityImpulse = 0f; | ||
584 | |||
585 | #warning Check to see if we need Vector3.Transform | ||
586 | Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); | ||
587 | solverConstraint.AngularComponentA = Vector3.TransformNormal(torqueAxis0, rb0.InvInertiaTensorWorld); | ||
588 | Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); | ||
589 | solverConstraint.AngularComponentB = Vector3.TransformNormal(torqueAxis1, rb1.InvInertiaTensorWorld); | ||
590 | } | ||
591 | //create 2 '1d axis' constraints for 2 tangential friction directions | ||
592 | |||
593 | //re-calculate friction direction every frame, todo: check if this is really needed | ||
594 | Vector3 frictionTangential0a = new Vector3(), | ||
595 | frictionTangential1b = new Vector3(); | ||
596 | |||
597 | MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref frictionTangential0a, ref frictionTangential1b); | ||
598 | { | ||
599 | SolverConstraint solverConstraint = new SolverConstraint(); | ||
600 | _tmpSolverFrictionConstraintPool.Add(solverConstraint); | ||
601 | solverConstraint.ContactNormal = frictionTangential0a; | ||
602 | |||
603 | solverConstraint.SolverBodyIdA = solverBodyIdA; | ||
604 | solverConstraint.SolverBodyIdB = solverBodyIdB; | ||
605 | solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; | ||
606 | solverConstraint.FrictionIndex = frictionIndex; | ||
607 | |||
608 | solverConstraint.Friction = cp.CombinedFriction; | ||
609 | |||
610 | solverConstraint.AppliedImpulse = 0; | ||
611 | solverConstraint.AppliedVelocityImpulse = 0; | ||
612 | |||
613 | float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); | ||
614 | float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); | ||
615 | float denom = relaxation / (denom0 + denom1); | ||
616 | solverConstraint.JacDiagABInv = denom; | ||
617 | |||
618 | { | ||
619 | Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); | ||
620 | solverConstraint.RelPosACrossNormal = ftorqueAxis0; | ||
621 | solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis0, rb0.InvInertiaTensorWorld); | ||
622 | } | ||
623 | { | ||
624 | Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); | ||
625 | solverConstraint.RelPosBCrossNormal = ftorqueAxis0; | ||
626 | solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis0, rb1.InvInertiaTensorWorld); | ||
627 | } | ||
628 | } | ||
629 | |||
630 | |||
631 | { | ||
632 | |||
633 | SolverConstraint solverConstraint = new SolverConstraint(); | ||
634 | _tmpSolverFrictionConstraintPool.Add(solverConstraint); | ||
635 | solverConstraint.ContactNormal = frictionTangential1b; | ||
636 | |||
637 | solverConstraint.SolverBodyIdA = solverBodyIdA; | ||
638 | solverConstraint.SolverBodyIdB = solverBodyIdB; | ||
639 | solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; | ||
640 | solverConstraint.FrictionIndex = frictionIndex; | ||
641 | |||
642 | solverConstraint.Friction = cp.CombinedFriction; | ||
643 | |||
644 | solverConstraint.AppliedImpulse = 0; | ||
645 | solverConstraint.AppliedVelocityImpulse = 0; | ||
646 | |||
647 | float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); | ||
648 | float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); | ||
649 | float denom = relaxation / (denom0 + denom1); | ||
650 | solverConstraint.JacDiagABInv = denom; | ||
651 | { | ||
652 | Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); | ||
653 | solverConstraint.RelPosACrossNormal = ftorqueAxis1; | ||
654 | solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis1, rb0.InvInertiaTensorWorld); | ||
655 | } | ||
656 | { | ||
657 | Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); | ||
658 | solverConstraint.RelPosBCrossNormal = ftorqueAxis1; | ||
659 | solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis1, rb1.InvInertiaTensorWorld); | ||
660 | } | ||
661 | } | ||
662 | } | ||
663 | } | ||
664 | } | ||
665 | |||
666 | ContactSolverInfo info = infoGlobal; | ||
667 | { | ||
668 | for (int j = 0; j < constraints.Count; j++) | ||
669 | { | ||
670 | TypedConstraint constraint = constraints[j]; | ||
671 | constraint.BuildJacobian(); | ||
672 | } | ||
673 | } | ||
674 | |||
675 | int numConstraintPool = _tmpSolverConstraintPool.Count; | ||
676 | int numFrictionPool = _tmpSolverFrictionConstraintPool.Count; | ||
677 | |||
678 | //todo: use stack allocator for such temporarily memory, same for solver bodies/constraints | ||
679 | List<int> gOrderTmpConstraintPool = new List<int>(numConstraintPool); | ||
680 | List<int> gOrderFrictionConstraintPool = new List<int>(numFrictionPool); | ||
681 | { | ||
682 | for (int i = 0; i < numConstraintPool; i++) | ||
683 | { | ||
684 | gOrderTmpConstraintPool.Add(i); | ||
685 | } | ||
686 | for (int i = 0; i < numFrictionPool; i++) | ||
687 | { | ||
688 | gOrderFrictionConstraintPool.Add(i); | ||
689 | } | ||
690 | } | ||
691 | |||
692 | //should traverse the contacts random order... | ||
693 | int iteration; | ||
694 | { | ||
695 | for (iteration = 0; iteration < info.IterationsCount; iteration++) | ||
696 | { | ||
697 | |||
698 | int j; | ||
699 | if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) | ||
700 | { | ||
701 | if ((iteration & 7) == 0) | ||
702 | { | ||
703 | for (j = 0; j < numConstraintPool; ++j) | ||
704 | { | ||
705 | int tmp = gOrderTmpConstraintPool[j]; | ||
706 | int swapi = RandInt2(j + 1); | ||
707 | gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi]; | ||
708 | gOrderTmpConstraintPool[swapi] = tmp; | ||
709 | } | ||
710 | |||
711 | for (j = 0; j < numFrictionPool; ++j) | ||
712 | { | ||
713 | int tmp = gOrderFrictionConstraintPool[j]; | ||
714 | int swapi = RandInt2(j + 1); | ||
715 | gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi]; | ||
716 | gOrderFrictionConstraintPool[swapi] = tmp; | ||
717 | } | ||
718 | } | ||
719 | } | ||
720 | |||
721 | for (j = 0; j < constraints.Count; j++) | ||
722 | { | ||
723 | TypedConstraint constraint = constraints[j]; | ||
724 | //todo: use solver bodies, so we don't need to copy from/to btRigidBody | ||
725 | |||
726 | if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) | ||
727 | { | ||
728 | _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].WriteBackVelocity(); | ||
729 | } | ||
730 | if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) | ||
731 | { | ||
732 | _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].WriteBackVelocity(); | ||
733 | } | ||
734 | |||
735 | constraint.SolveConstraint(info.TimeStep); | ||
736 | |||
737 | if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) | ||
738 | { | ||
739 | _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].ReadVelocity(); | ||
740 | } | ||
741 | if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) | ||
742 | { | ||
743 | _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].ReadVelocity(); | ||
744 | } | ||
745 | |||
746 | } | ||
747 | |||
748 | { | ||
749 | int numPoolConstraints = _tmpSolverConstraintPool.Count; | ||
750 | for (j = 0; j < numPoolConstraints; j++) | ||
751 | { | ||
752 | SolverConstraint solveManifold = _tmpSolverConstraintPool[gOrderTmpConstraintPool[j]]; | ||
753 | ResolveSingleCollisionCombinedCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], | ||
754 | _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info); | ||
755 | } | ||
756 | } | ||
757 | |||
758 | { | ||
759 | int numFrictionPoolConstraints = _tmpSolverFrictionConstraintPool.Count; | ||
760 | for (j = 0; j < numFrictionPoolConstraints; j++) | ||
761 | { | ||
762 | SolverConstraint solveManifold = _tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]]; | ||
763 | float appliedNormalImpulse = _tmpSolverConstraintPool[solveManifold.FrictionIndex].AppliedImpulse; | ||
764 | |||
765 | ResolveSingleFrictionCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], | ||
766 | _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info, appliedNormalImpulse); | ||
767 | } | ||
768 | } | ||
769 | } | ||
770 | } | ||
771 | |||
772 | for (int i = 0; i < _tmpSolverBodyPool.Count; i++) | ||
773 | { | ||
774 | _tmpSolverBodyPool[i].WriteBackVelocity(); | ||
775 | } | ||
776 | |||
777 | _tmpSolverBodyPool.Clear(); | ||
778 | _tmpSolverConstraintPool.Clear(); | ||
779 | _tmpSolverFrictionConstraintPool.Clear(); | ||
780 | |||
781 | return 0; | ||
782 | } | ||
783 | |||
784 | public virtual float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) | ||
785 | { | ||
786 | if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None) | ||
787 | { | ||
788 | return SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer); | ||
789 | } | ||
790 | |||
791 | ContactSolverInfo info = infoGlobal; | ||
792 | int totalPoints = 0; | ||
793 | |||
794 | int numiter = infoGlobal.IterationsCount; | ||
795 | |||
796 | for (int j = 0; j < manifolds.Count; j++) | ||
797 | { | ||
798 | PersistentManifold manifold = manifolds[j]; | ||
799 | PrepareConstraints(manifold, info); | ||
800 | |||
801 | for (int p = 0; p < manifolds[j].ContactsCount; p++) | ||
802 | { | ||
803 | _order[totalPoints].ManifoldIndex = j; | ||
804 | _order[totalPoints].PointIndex = p; | ||
805 | totalPoints++; | ||
806 | } | ||
807 | } | ||
808 | |||
809 | for (int j = 0; j < constraints.Count; j++) | ||
810 | { | ||
811 | constraints[j].BuildJacobian(); | ||
812 | } | ||
813 | |||
814 | //should traverse the contacts random order... | ||
815 | int iteration; | ||
816 | |||
817 | for (iteration = 0; iteration < numiter; iteration++) | ||
818 | { | ||
819 | int j; | ||
820 | if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) | ||
821 | { | ||
822 | if ((iteration & 7) == 0) | ||
823 | { | ||
824 | for (j = 0; j < totalPoints; ++j) | ||
825 | { | ||
826 | OrderIndex tmp = _order[j]; | ||
827 | int swapi = RandInt2(j + 1); | ||
828 | _order[j] = _order[swapi]; | ||
829 | _order[swapi] = tmp; | ||
830 | } | ||
831 | } | ||
832 | } | ||
833 | |||
834 | for (j = 0; j < constraints.Count; j++) | ||
835 | { | ||
836 | constraints[j].SolveConstraint(info.TimeStep); | ||
837 | } | ||
838 | |||
839 | for (j = 0; j < totalPoints; j++) | ||
840 | { | ||
841 | PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; | ||
842 | Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB, | ||
843 | manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); | ||
844 | } | ||
845 | |||
846 | for (j = 0; j < totalPoints; j++) | ||
847 | { | ||
848 | PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; | ||
849 | SolveFriction((RigidBody)manifold.BodyA, | ||
850 | (RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); | ||
851 | } | ||
852 | } | ||
853 | |||
854 | return 0; | ||
855 | } | ||
856 | |||
857 | private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody) | ||
858 | { | ||
859 | solverBody = new SolverBody(); | ||
860 | solverBody.AngularVelocity = rigidBody.AngularVelocity; | ||
861 | solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition; | ||
862 | solverBody.Friction = rigidBody.Friction; | ||
863 | solverBody.InvMass = rigidBody.InverseMass; | ||
864 | solverBody.LinearVelocity = rigidBody.LinearVelocity; | ||
865 | solverBody.OriginalBody = rigidBody; | ||
866 | solverBody.AngularFactor = rigidBody.AngularFactor; | ||
867 | } | ||
868 | |||
869 | private long Rand2() | ||
870 | { | ||
871 | _seed2 = (1664525L * _seed2 + 1013904223L) & 0xffffffff; | ||
872 | return _seed2; | ||
873 | } | ||
874 | |||
875 | private int RandInt2(int n) | ||
876 | { | ||
877 | // seems good; xor-fold and modulus | ||
878 | long un = n; | ||
879 | long r = Rand2(); | ||
880 | |||
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. | ||
883 | if (un <= 0x00010000L) | ||
884 | { | ||
885 | r ^= (r >> 16); | ||
886 | if (un <= 0x00000100L) | ||
887 | { | ||
888 | r ^= (r >> 8); | ||
889 | if (un <= 0x00000010L) | ||
890 | { | ||
891 | r ^= (r >> 4); | ||
892 | if (un <= 0x00000004L) | ||
893 | { | ||
894 | r ^= (r >> 2); | ||
895 | if (un <= 0x00000002L) | ||
896 | { | ||
897 | r ^= (r >> 1); | ||
898 | } | ||
899 | } | ||
900 | } | ||
901 | } | ||
902 | } | ||
903 | return (int)(r % un); | ||
904 | } | ||
905 | |||
906 | protected struct OrderIndex | ||
907 | { | ||
908 | private int _manifoldIndex; | ||
909 | private int _pointIndex; | ||
910 | |||
911 | public int ManifoldIndex { get { return _manifoldIndex; } set { _manifoldIndex = value; } } | ||
912 | public int PointIndex { get { return _pointIndex; } set { _pointIndex = value; } } | ||
913 | } | ||
914 | } | ||
915 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs new file mode 100644 index 0000000..6ced783 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs | |||
@@ -0,0 +1,188 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX.Dynamics | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// constraint class used for lateral tyre friction | ||
31 | /// </summary> | ||
32 | public class Solve2LinearConstraint | ||
33 | { | ||
34 | private float _tau; | ||
35 | private float _damping; | ||
36 | |||
37 | public Solve2LinearConstraint(float tau, float damping) | ||
38 | { | ||
39 | _tau = tau; | ||
40 | _damping = damping; | ||
41 | } | ||
42 | |||
43 | // solve unilateral constraint (equality, direct method) | ||
44 | public void ResolveUnilateralPairConstraint( | ||
45 | RigidBody body1, RigidBody body2, | ||
46 | Matrix world2A, | ||
47 | Matrix world2B, | ||
48 | Vector3 invInertiaADiag, | ||
49 | float invMassA, | ||
50 | Vector3 linvelA, Vector3 angvelA, | ||
51 | Vector3 rel_posA1, | ||
52 | Vector3 invInertiaBDiag, | ||
53 | float invMassB, | ||
54 | Vector3 linvelB, Vector3 angvelB, | ||
55 | Vector3 rel_posA2, | ||
56 | float depthA, Vector3 normalA, | ||
57 | Vector3 rel_posB1, Vector3 rel_posB2, | ||
58 | float depthB, Vector3 normalB, | ||
59 | out float imp0, out float imp1) | ||
60 | { | ||
61 | imp0 = 0; | ||
62 | imp1 = 0; | ||
63 | |||
64 | float len = Math.Abs(normalA.Length()) - 1f; | ||
65 | if (Math.Abs(len) >= float.Epsilon) | ||
66 | return; | ||
67 | |||
68 | BulletDebug.Assert(len < float.Epsilon); | ||
69 | |||
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, | ||
72 | invInertiaBDiag, invMassB); | ||
73 | JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, | ||
74 | invInertiaBDiag, invMassB); | ||
75 | |||
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)); | ||
78 | |||
79 | // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv | ||
80 | float massTerm = 1f / (invMassA + invMassB); | ||
81 | |||
82 | // calculate rhs (or error) terms | ||
83 | float dv0 = depthA * _tau * massTerm - vel0 * _damping; | ||
84 | float dv1 = depthB * _tau * massTerm - vel1 * _damping; | ||
85 | |||
86 | float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); | ||
87 | float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); | ||
88 | |||
89 | imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; | ||
90 | imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; | ||
91 | } | ||
92 | |||
93 | // solving 2x2 lcp problem (inequality, direct solution ) | ||
94 | public void ResolveBilateralPairConstraint( | ||
95 | RigidBody body1, RigidBody body2, | ||
96 | Matrix world2A, Matrix world2B, | ||
97 | Vector3 invInertiaADiag, | ||
98 | float invMassA, | ||
99 | Vector3 linvelA, Vector3 angvelA, | ||
100 | Vector3 rel_posA1, | ||
101 | Vector3 invInertiaBDiag, | ||
102 | float invMassB, | ||
103 | Vector3 linvelB, Vector3 angvelB, | ||
104 | Vector3 rel_posA2, | ||
105 | float depthA, Vector3 normalA, | ||
106 | Vector3 rel_posB1, Vector3 rel_posB2, | ||
107 | float depthB, Vector3 normalB, | ||
108 | out float imp0, out float imp1) | ||
109 | { | ||
110 | imp0 = 0f; | ||
111 | imp1 = 0f; | ||
112 | |||
113 | float len = Math.Abs(normalA.Length()) - 1f; | ||
114 | if (Math.Abs(len) >= float.Epsilon) | ||
115 | return; | ||
116 | |||
117 | BulletDebug.Assert(len < float.Epsilon); | ||
118 | |||
119 | JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, | ||
120 | invInertiaBDiag, invMassB); | ||
121 | JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, | ||
122 | invInertiaBDiag, invMassB); | ||
123 | |||
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)); | ||
126 | |||
127 | // calculate rhs (or error) terms | ||
128 | float dv0 = depthA * _tau - vel0 * _damping; | ||
129 | float dv1 = depthB * _tau - vel1 * _damping; | ||
130 | |||
131 | float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); | ||
132 | float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); | ||
133 | |||
134 | imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; | ||
135 | imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; | ||
136 | |||
137 | if (imp0 > 0.0f) | ||
138 | { | ||
139 | if (imp1 <= 0.0f) | ||
140 | { | ||
141 | imp1 = 0f; | ||
142 | |||
143 | // now imp0>0 imp1<0 | ||
144 | imp0 = dv0 / jacA.Diagonal; | ||
145 | if (imp0 < 0.0f) | ||
146 | imp0 = 0f; | ||
147 | } | ||
148 | } | ||
149 | else | ||
150 | { | ||
151 | imp0 = 0f; | ||
152 | |||
153 | imp1 = dv1 / jacB.Diagonal; | ||
154 | if (imp1 <= 0.0f) | ||
155 | { | ||
156 | imp1 = 0f; | ||
157 | // now imp0>0 imp1<0 | ||
158 | imp0 = dv0 / jacA.Diagonal; | ||
159 | if (imp0 > 0.0f) | ||
160 | { | ||
161 | } | ||
162 | else | ||
163 | { | ||
164 | imp0 = 0f; | ||
165 | } | ||
166 | } | ||
167 | } | ||
168 | } | ||
169 | |||
170 | //public void ResolveAngularConstraint( | ||
171 | // Matrix invInertiaAWS, | ||
172 | // float invMassA, | ||
173 | // Vector3 linvelA, Vector3 angvelA, | ||
174 | // Vector3 rel_posA1, | ||
175 | // Matrix invInertiaBWS, | ||
176 | // float invMassB, | ||
177 | // Vector3 linvelB, Vector3 angvelB, | ||
178 | // Vector3 rel_posA2, | ||
179 | // float depthA, Vector3 normalA, | ||
180 | // Vector3 rel_posB1, Vector3 rel_posB2, | ||
181 | // float depthB, Vector3 normalB, | ||
182 | // out float imp0, out float imp1) | ||
183 | //{ | ||
184 | // imp0 = 0; | ||
185 | // imp1 = 0; | ||
186 | //} | ||
187 | } | ||
188 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs new file mode 100644 index 0000000..09b73cc --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs | |||
@@ -0,0 +1,78 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | using System.Runtime.InteropServices; | ||
27 | |||
28 | namespace XnaDevRu.BulletX.Dynamics | ||
29 | { | ||
30 | public class SolverBody | ||
31 | { | ||
32 | private Vector3 _centerOfMassPosition = new Vector3(); | ||
33 | private Vector3 _linearVelocity = new Vector3(); | ||
34 | private Vector3 _angularVelocity = new Vector3(); | ||
35 | private RigidBody _originalBody = null; | ||
36 | private float _invMass; | ||
37 | private float _friction; | ||
38 | private float _angularFactor; | ||
39 | |||
40 | public Vector3 CenterOfMassPosition { get { return _centerOfMassPosition; } set { _centerOfMassPosition = value; } } | ||
41 | public Vector3 LinearVelocity { get { return _linearVelocity; } set { _linearVelocity = value; } } | ||
42 | public Vector3 AngularVelocity { get { return _angularVelocity; } set { _angularVelocity = value; } } | ||
43 | public RigidBody OriginalBody { get { return _originalBody; } set { _originalBody = value; } } | ||
44 | public float InvMass { get { return _invMass; } set { _invMass = value; } } | ||
45 | public float Friction { get { return _friction; } set { _friction = value; } } | ||
46 | public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } } | ||
47 | |||
48 | public void GetVelocityInLocalPoint(Vector3 relPos, out Vector3 velocity) | ||
49 | { | ||
50 | velocity = _linearVelocity + Vector3.Cross(_angularVelocity, relPos); | ||
51 | } | ||
52 | |||
53 | public void WriteBackVelocity() | ||
54 | { | ||
55 | if (_invMass != 0) | ||
56 | { | ||
57 | _originalBody.LinearVelocity = _linearVelocity; | ||
58 | _originalBody.AngularVelocity = _angularVelocity; | ||
59 | } | ||
60 | } | ||
61 | |||
62 | public void ReadVelocity() | ||
63 | { | ||
64 | if (_invMass != 0) | ||
65 | { | ||
66 | _linearVelocity = _originalBody.LinearVelocity; | ||
67 | _angularVelocity = _originalBody.AngularVelocity; | ||
68 | } | ||
69 | } | ||
70 | |||
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) | ||
73 | { | ||
74 | _linearVelocity += linearComponent * impulseMagnitude; | ||
75 | _angularVelocity += angularComponent * impulseMagnitude * _angularFactor; | ||
76 | } | ||
77 | } | ||
78 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs new file mode 100644 index 0000000..3e3a64a --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs | |||
@@ -0,0 +1,77 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | using System.Runtime.InteropServices; | ||
27 | |||
28 | namespace XnaDevRu.BulletX.Dynamics | ||
29 | { | ||
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 | ||
32 | { | ||
33 | private Vector3 _relpos1CrossNormal = new Vector3(); | ||
34 | private Vector3 _relpos2CrossNormal = new Vector3(); | ||
35 | private Vector3 _contactNormal = new Vector3(); | ||
36 | private Vector3 _angularComponentA = new Vector3(); | ||
37 | private Vector3 _angularComponentB = new Vector3(); | ||
38 | |||
39 | private float _appliedVelocityImpulse; | ||
40 | private int _solverBodyIdA; | ||
41 | int _solverBodyIdB; | ||
42 | private float _friction; | ||
43 | private float _restitution; | ||
44 | private float _jacDiagABInv; | ||
45 | private float _penetration; | ||
46 | private float _appliedImpulse; | ||
47 | |||
48 | private SolverConstraintType _constraintType = SolverConstraintType.Contact; | ||
49 | private int _frictionIndex; | ||
50 | private int[] _unusedPadding = new int[2]; | ||
51 | |||
52 | public Vector3 RelPosACrossNormal { get { return _relpos1CrossNormal; } set { _relpos1CrossNormal = value; } } | ||
53 | public Vector3 RelPosBCrossNormal { get { return _relpos2CrossNormal; } set { _relpos2CrossNormal = value; } } | ||
54 | public Vector3 ContactNormal { get { return _contactNormal; } set { _contactNormal = value; } } | ||
55 | public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } } | ||
56 | public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } } | ||
57 | |||
58 | public float AppliedVelocityImpulse { get { return _appliedVelocityImpulse; } set { _appliedVelocityImpulse = value; } } | ||
59 | public int SolverBodyIdA { get { return _solverBodyIdA; } set { _solverBodyIdA = value; } } | ||
60 | public int SolverBodyIdB { get { return _solverBodyIdB; } set { _solverBodyIdB = value; } } | ||
61 | public float Friction { get { return _friction; } set { _friction = value; } } | ||
62 | public float Restitution { get { return _restitution; } set { _restitution = value; } } | ||
63 | public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } } | ||
64 | public float Penetration { get { return _penetration; } set { _penetration = value; } } | ||
65 | public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } } | ||
66 | |||
67 | public SolverConstraintType ConstraintType { get { return _constraintType; } set { _constraintType = value; } } | ||
68 | public int FrictionIndex { get { return _frictionIndex; } set { _frictionIndex = value; } } | ||
69 | public int[] UnusedPadding { get { return _unusedPadding; } set { _unusedPadding = value; } } | ||
70 | |||
71 | public enum SolverConstraintType | ||
72 | { | ||
73 | Contact = 0, | ||
74 | Friction, | ||
75 | } | ||
76 | } | ||
77 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs new file mode 100644 index 0000000..abd10f1 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs | |||
@@ -0,0 +1,88 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX.Dynamics | ||
28 | { | ||
29 | public abstract class TypedConstraint | ||
30 | { | ||
31 | private static RigidBody _fixed = new RigidBody(0, null, null, new Vector3(), 0, 0, 0.5f, 0); | ||
32 | private int _userConstraintType; | ||
33 | private int _userConstraintId; | ||
34 | |||
35 | private RigidBody _rbA; | ||
36 | private RigidBody _rbB; | ||
37 | private float _appliedImpulse; | ||
38 | |||
39 | public TypedConstraint() | ||
40 | : this(_fixed, _fixed) { } | ||
41 | |||
42 | public TypedConstraint(RigidBody rbA) | ||
43 | : this(rbA, _fixed) { } | ||
44 | |||
45 | public TypedConstraint(RigidBody rbA, RigidBody rbB) | ||
46 | { | ||
47 | _userConstraintType = -1; | ||
48 | _userConstraintId = -1; | ||
49 | _rbA = rbA; | ||
50 | _rbB = rbB; | ||
51 | _appliedImpulse = 0; | ||
52 | |||
53 | _fixed.SetMassProps(0, new Vector3()); | ||
54 | } | ||
55 | |||
56 | public virtual RigidBody RigidBodyA { get { return _rbA; } protected set { _rbA = value; } } | ||
57 | public virtual RigidBody RigidBodyB { get { return _rbB; } protected set { _rbB = value; } } | ||
58 | |||
59 | public float AppliedImpulse { get { return _appliedImpulse; } protected set { _appliedImpulse = value; } } | ||
60 | public int UserConstraintId { get { return _userConstraintId; } set { _userConstraintId = value; } } | ||
61 | public int UserConstraintType { get { return _userConstraintType; } set { _userConstraintType = value; } } | ||
62 | |||
63 | public abstract void BuildJacobian(); | ||
64 | public abstract void SolveConstraint(float timeStep); | ||
65 | |||
66 | public static int SortConstraintOnIslandPredicate(TypedConstraint left, TypedConstraint right) | ||
67 | { | ||
68 | int rightIslandID, leftIslandID; | ||
69 | rightIslandID = GetConstraintIslandId(right); | ||
70 | leftIslandID = GetConstraintIslandId(left); | ||
71 | if (leftIslandID < rightIslandID) | ||
72 | return -1; | ||
73 | else | ||
74 | return 1; | ||
75 | return 0; | ||
76 | } | ||
77 | |||
78 | internal static int GetConstraintIslandId(TypedConstraint lhs) | ||
79 | { | ||
80 | int islandId; | ||
81 | |||
82 | CollisionObject colObjA = lhs.RigidBodyA; | ||
83 | CollisionObject colObjB = lhs.RigidBodyB; | ||
84 | islandId = colObjA.IslandTag >= 0 ? colObjA.IslandTag : colObjB.IslandTag; | ||
85 | return islandId; | ||
86 | } | ||
87 | } | ||
88 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs new file mode 100644 index 0000000..acceb98 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs | |||
@@ -0,0 +1,790 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX.Dynamics | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// DiscreteDynamicsWorld provides discrete rigid body simulation | ||
31 | /// those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController | ||
32 | /// </summary> | ||
33 | public class DiscreteDynamicsWorld : DynamicsWorld | ||
34 | { | ||
35 | private static bool _reportMe = true; | ||
36 | |||
37 | private IConstraintSolver _constraintSolver; | ||
38 | private SimulationIslandManager _islandManager; | ||
39 | private List<TypedConstraint> _constraints = new List<TypedConstraint>(); | ||
40 | private IDebugDraw _debugDrawer; | ||
41 | private ContactSolverInfo _solverInfo = new ContactSolverInfo(); | ||
42 | private Vector3 _gravity; | ||
43 | //for variable timesteps | ||
44 | private float _localTime; | ||
45 | //for variable timesteps | ||
46 | private bool _ownsIslandManager; | ||
47 | private bool _ownsConstraintSolver; | ||
48 | private List<RaycastVehicle> _vehicles = new List<RaycastVehicle>(); | ||
49 | private int _profileTimings; | ||
50 | |||
51 | public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) | ||
52 | : this(dispatcher, pairCache, null) { } | ||
53 | |||
54 | //this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those | ||
55 | public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver) | ||
56 | : base(dispatcher, pairCache) | ||
57 | { | ||
58 | _constraintSolver = constraintSolver != null ? constraintSolver : new SequentialImpulseConstraintSolver(); | ||
59 | _debugDrawer = null; | ||
60 | _gravity = new Vector3(0, -10, 0); | ||
61 | _localTime = 1f / 60f; | ||
62 | _profileTimings = 0; | ||
63 | _islandManager = new SimulationIslandManager(); | ||
64 | _ownsIslandManager = true; | ||
65 | _ownsConstraintSolver = constraintSolver == null; | ||
66 | } | ||
67 | |||
68 | public ContactSolverInfo SolverInfo { get { return _solverInfo; } } | ||
69 | public SimulationIslandManager SimulationIslandManager { get { return _islandManager; } } | ||
70 | public CollisionWorld CollisionWorld { get { return this; } } | ||
71 | |||
72 | public override IDebugDraw DebugDrawer | ||
73 | { | ||
74 | get | ||
75 | { | ||
76 | return _debugDrawer; | ||
77 | } | ||
78 | set | ||
79 | { | ||
80 | _debugDrawer = value; | ||
81 | } | ||
82 | } | ||
83 | |||
84 | public override Vector3 Gravity | ||
85 | { | ||
86 | set | ||
87 | { | ||
88 | _gravity = value; | ||
89 | for (int i = 0; i < CollisionObjects.Count; i++) | ||
90 | { | ||
91 | CollisionObject colObj = CollisionObjects[i]; | ||
92 | RigidBody body = RigidBody.Upcast(colObj); | ||
93 | if (body != null) | ||
94 | { | ||
95 | body.Gravity = value; | ||
96 | } | ||
97 | } | ||
98 | } | ||
99 | } | ||
100 | |||
101 | public override IConstraintSolver ConstraintSolver | ||
102 | { | ||
103 | set | ||
104 | { | ||
105 | _ownsConstraintSolver = false; | ||
106 | _constraintSolver = value; | ||
107 | } | ||
108 | } | ||
109 | |||
110 | public override int ConstraintsCount | ||
111 | { | ||
112 | get | ||
113 | { | ||
114 | return _constraints.Count; | ||
115 | } | ||
116 | } | ||
117 | |||
118 | //if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's | ||
119 | public override void StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep) | ||
120 | { | ||
121 | int numSimulationSubSteps = 0; | ||
122 | |||
123 | if (maxSubSteps != 0) | ||
124 | { | ||
125 | //fixed timestep with interpolation | ||
126 | _localTime += timeStep; | ||
127 | if (_localTime >= fixedTimeStep) | ||
128 | { | ||
129 | numSimulationSubSteps = (int)(_localTime / fixedTimeStep); | ||
130 | _localTime -= numSimulationSubSteps * fixedTimeStep; | ||
131 | } | ||
132 | } | ||
133 | else | ||
134 | { | ||
135 | //variable timestep | ||
136 | fixedTimeStep = timeStep; | ||
137 | _localTime = timeStep; | ||
138 | if (Math.Abs(timeStep) < float.Epsilon) | ||
139 | { | ||
140 | numSimulationSubSteps = 0; | ||
141 | maxSubSteps = 0; | ||
142 | } | ||
143 | else | ||
144 | { | ||
145 | numSimulationSubSteps = 1; | ||
146 | maxSubSteps = 1; | ||
147 | } | ||
148 | } | ||
149 | |||
150 | //process some debugging flags | ||
151 | if (DebugDrawer != null) | ||
152 | { | ||
153 | RigidBody.DisableDeactivation = (DebugDrawer.DebugMode & DebugDrawModes.NoDeactivation) != 0; | ||
154 | } | ||
155 | if (numSimulationSubSteps != 0) | ||
156 | { | ||
157 | |||
158 | SaveKinematicState(fixedTimeStep); | ||
159 | |||
160 | //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt | ||
161 | int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; | ||
162 | |||
163 | for (int i = 0; i < clampedSimulationSteps; i++) | ||
164 | { | ||
165 | InternalSingleStepSimulation(fixedTimeStep); | ||
166 | SynchronizeMotionStates(); | ||
167 | } | ||
168 | |||
169 | } | ||
170 | |||
171 | SynchronizeMotionStates(); | ||
172 | |||
173 | //return numSimulationSubSteps; | ||
174 | } | ||
175 | |||
176 | public void StepSimulation(float timeStep, int maxSubSteps) | ||
177 | { | ||
178 | StepSimulation(timeStep, maxSubSteps, 1f / 60f); | ||
179 | } | ||
180 | |||
181 | public override void UpdateAabbs() | ||
182 | { | ||
183 | Vector3 colorvec = new Vector3(1, 0, 0); | ||
184 | for (int i = 0; i < CollisionObjects.Count; i++) | ||
185 | { | ||
186 | CollisionObject colObj = CollisionObjects[i]; | ||
187 | RigidBody body = RigidBody.Upcast(colObj); | ||
188 | |||
189 | if (body != null) | ||
190 | { | ||
191 | // if (body->IsActive() && (!body->IsStatic())) | ||
192 | { | ||
193 | Vector3 minAabb, maxAabb; | ||
194 | colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb); | ||
195 | OverlappingPairCache bp = BroadphasePairCache; | ||
196 | |||
197 | //moving objects should be moderately sized, probably something wrong if not | ||
198 | if (colObj.IsStaticObject || ((maxAabb - minAabb).LengthSquared() < 1e12f)) | ||
199 | { | ||
200 | bp.SetAabb(body.Broadphase, minAabb, maxAabb); | ||
201 | } | ||
202 | else | ||
203 | { | ||
204 | //something went wrong, investigate | ||
205 | //this assert is unwanted in 3D modelers (danger of loosing work) | ||
206 | BulletDebug.Assert(false); | ||
207 | body.ActivationState = ActivationState.DisableSimulation; | ||
208 | |||
209 | if (_reportMe) | ||
210 | { | ||
211 | _reportMe = false; | ||
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"); | ||
214 | Console.WriteLine("Please include above information, your Platform, version of OS.\n"); | ||
215 | Console.WriteLine("Thanks.\n"); | ||
216 | } | ||
217 | } | ||
218 | if (_debugDrawer != null && (_debugDrawer.DebugMode & DebugDrawModes.DrawAabb) != 0) | ||
219 | DrawAabb(_debugDrawer, minAabb, maxAabb, colorvec); | ||
220 | } | ||
221 | } | ||
222 | } | ||
223 | } | ||
224 | |||
225 | public override void AddConstraint(TypedConstraint constraint) | ||
226 | { | ||
227 | _constraints.Add(constraint); | ||
228 | } | ||
229 | |||
230 | public override void RemoveConstraint(TypedConstraint constraint) | ||
231 | { | ||
232 | _constraints.Remove(constraint); | ||
233 | } | ||
234 | |||
235 | public void AddVehicle(RaycastVehicle vehicle) | ||
236 | { | ||
237 | _vehicles.Add(vehicle); | ||
238 | } | ||
239 | |||
240 | public void RemoveVehicle(RaycastVehicle vehicle) | ||
241 | { | ||
242 | _vehicles.Remove(vehicle); | ||
243 | } | ||
244 | |||
245 | public override void AddRigidBody(RigidBody body) | ||
246 | { | ||
247 | if (!body.IsStaticOrKinematicObject) | ||
248 | { | ||
249 | body.Gravity = _gravity; | ||
250 | } | ||
251 | |||
252 | if (body.CollisionShape != null) | ||
253 | { | ||
254 | bool isDynamic = !(body.IsStaticObject || body.IsKinematicObject); | ||
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); | ||
257 | |||
258 | AddCollisionObject(body, collisionFilterGroup, collisionFilterMask); | ||
259 | } | ||
260 | } | ||
261 | |||
262 | public override void RemoveRigidBody(RigidBody body) | ||
263 | { | ||
264 | RemoveCollisionObject(body); | ||
265 | } | ||
266 | |||
267 | public void DebugDrawObject(Matrix worldTransform, CollisionShape shape, Vector3 color) | ||
268 | { | ||
269 | if (shape.ShapeType == BroadphaseNativeTypes.Compound) | ||
270 | { | ||
271 | CompoundShape compoundShape = shape as CompoundShape; | ||
272 | for (int i = compoundShape.ChildShapeCount - 1; i >= 0; i--) | ||
273 | { | ||
274 | Matrix childTrans = compoundShape.GetChildTransform(i); | ||
275 | CollisionShape colShape = compoundShape.GetChildShape(i); | ||
276 | DebugDrawObject(worldTransform * childTrans, colShape, color); | ||
277 | } | ||
278 | |||
279 | } | ||
280 | else | ||
281 | { | ||
282 | switch (shape.ShapeType) | ||
283 | { | ||
284 | |||
285 | case BroadphaseNativeTypes.Sphere: | ||
286 | { | ||
287 | SphereShape sphereShape = shape as SphereShape; | ||
288 | float radius = sphereShape.Margin;//radius doesn't include the margin, so draw with margin | ||
289 | Vector3 start = worldTransform.Translation; | ||
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); | ||
292 | DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, 0, radius), worldTransform), color); | ||
293 | //drawSphere | ||
294 | break; | ||
295 | } | ||
296 | case BroadphaseNativeTypes.MultiSphere: | ||
297 | case BroadphaseNativeTypes.Cone: | ||
298 | { | ||
299 | ConeShape coneShape = shape as ConeShape; | ||
300 | float radius = coneShape.Radius;//+coneShape->getMargin(); | ||
301 | float height = coneShape.Height;//+coneShape->getMargin(); | ||
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); | ||
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); | ||
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; | ||
308 | } | ||
309 | case BroadphaseNativeTypes.Cylinder: | ||
310 | { | ||
311 | CylinderShape cylinder = shape as CylinderShape; | ||
312 | int upAxis = cylinder.UpAxis; | ||
313 | float radius = cylinder.Radius; | ||
314 | float halfHeight = MathHelper.GetElement(cylinder.HalfExtents, upAxis); | ||
315 | Vector3 start = worldTransform.Translation; | ||
316 | Vector3 offsetHeight = new Vector3(); | ||
317 | MathHelper.SetElement(ref offsetHeight, upAxis, halfHeight); | ||
318 | Vector3 offsetRadius = new Vector3(); | ||
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); | ||
321 | DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight - offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color); | ||
322 | break; | ||
323 | } | ||
324 | default: | ||
325 | { | ||
326 | if (shape.ShapeType == BroadphaseNativeTypes.TriangleMesh) | ||
327 | { | ||
328 | TriangleMeshShape concaveMesh = shape as TriangleMeshShape; | ||
329 | //btVector3 aabbMax(1e30f,1e30f,1e30f); | ||
330 | //btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f); | ||
331 | |||
332 | //todo pass camera, for some culling | ||
333 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | ||
334 | Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); | ||
335 | |||
336 | DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color); | ||
337 | concaveMesh.ProcessAllTriangles(drawCallback, aabbMin, aabbMax); | ||
338 | } | ||
339 | |||
340 | if (shape.ShapeType == BroadphaseNativeTypes.ConvexTriangleMesh) | ||
341 | { | ||
342 | ConvexTriangleMeshShape convexMesh = shape as ConvexTriangleMeshShape; | ||
343 | //todo: pass camera for some culling | ||
344 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | ||
345 | Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); | ||
346 | //DebugDrawcallback drawCallback; | ||
347 | DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color); | ||
348 | convexMesh.getStridingMesh().InternalProcessAllTriangles(drawCallback, aabbMin, aabbMax); | ||
349 | } | ||
350 | |||
351 | // for polyhedral shapes | ||
352 | if (shape.IsPolyhedral) | ||
353 | { | ||
354 | PolyhedralConvexShape polyshape = shape as PolyhedralConvexShape; | ||
355 | |||
356 | for (int i = 0; i < polyshape.EdgeCount; i++) | ||
357 | { | ||
358 | Vector3 a, b; | ||
359 | polyshape.GetEdge(i, out a, out b); | ||
360 | a = Vector3.TransformNormal(a, worldTransform); | ||
361 | b = Vector3.TransformNormal(b, worldTransform); | ||
362 | DebugDrawer.DrawLine(a, b, color); | ||
363 | } | ||
364 | } | ||
365 | break; | ||
366 | } | ||
367 | } | ||
368 | } | ||
369 | } | ||
370 | |||
371 | public override TypedConstraint GetConstraint(int index) | ||
372 | { | ||
373 | return _constraints[index]; | ||
374 | } | ||
375 | |||
376 | public static void DrawAabb(IDebugDraw debugDrawer, Vector3 from, Vector3 to, Vector3 color) | ||
377 | { | ||
378 | Vector3 halfExtents = (to - from) * 0.5f; | ||
379 | Vector3 center = (to + from) * 0.5f; | ||
380 | |||
381 | Vector3 edgecoord = new Vector3(1f, 1f, 1f), pa, pb; | ||
382 | for (int i = 0; i < 4; i++) | ||
383 | { | ||
384 | for (int j = 0; j < 3; j++) | ||
385 | { | ||
386 | pa = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y, | ||
387 | edgecoord.Z * halfExtents.Z); | ||
388 | pa += center; | ||
389 | |||
390 | int othercoord = j % 3; | ||
391 | MathHelper.SetElement(ref edgecoord, othercoord, MathHelper.GetElement(edgecoord, othercoord) * -1f); | ||
392 | pb = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y, | ||
393 | edgecoord.Z * halfExtents.Z); | ||
394 | pb += center; | ||
395 | |||
396 | debugDrawer.DrawLine(pa, pb, color); | ||
397 | } | ||
398 | edgecoord = new Vector3(-1f, -1f, -1f); | ||
399 | if (i < 3) | ||
400 | MathHelper.SetElement(ref edgecoord, i, MathHelper.GetElement(edgecoord, i) * -1f); | ||
401 | } | ||
402 | } | ||
403 | |||
404 | protected void PredictUnconstraintMotion(float timeStep) | ||
405 | { | ||
406 | for (int i = 0; i < CollisionObjects.Count; i++) | ||
407 | { | ||
408 | CollisionObject colObj = CollisionObjects[i]; | ||
409 | RigidBody body = RigidBody.Upcast(colObj); | ||
410 | if (body != null) | ||
411 | { | ||
412 | if (!body.IsStaticOrKinematicObject) | ||
413 | { | ||
414 | if (body.IsActive) | ||
415 | { | ||
416 | body.ApplyForces(timeStep); | ||
417 | body.IntegrateVelocities(timeStep); | ||
418 | Matrix temp = body.InterpolationWorldTransform; | ||
419 | body.PredictIntegratedTransform(timeStep, ref temp); | ||
420 | body.InterpolationWorldTransform = temp; | ||
421 | } | ||
422 | } | ||
423 | } | ||
424 | } | ||
425 | } | ||
426 | |||
427 | protected void IntegrateTransforms(float timeStep) | ||
428 | { | ||
429 | Matrix predictedTrans = new Matrix(); | ||
430 | for (int i = 0; i < CollisionObjects.Count; i++) | ||
431 | { | ||
432 | CollisionObject colObj = CollisionObjects[i]; | ||
433 | RigidBody body = RigidBody.Upcast(colObj); | ||
434 | if (body != null) | ||
435 | { | ||
436 | if (body.IsActive && (!body.IsStaticOrKinematicObject)) | ||
437 | { | ||
438 | body.PredictIntegratedTransform(timeStep, ref predictedTrans); | ||
439 | body.ProceedToTransform(predictedTrans); | ||
440 | } | ||
441 | } | ||
442 | } | ||
443 | } | ||
444 | |||
445 | protected void CalculateSimulationIslands() | ||
446 | { | ||
447 | SimulationIslandManager.UpdateActivationState(this, Dispatcher); | ||
448 | |||
449 | for (int i = 0; i < _constraints.Count; i++) | ||
450 | { | ||
451 | TypedConstraint constraint = _constraints[i]; | ||
452 | |||
453 | RigidBody colObj0 = constraint.RigidBodyA; | ||
454 | RigidBody colObj1 = constraint.RigidBodyB; | ||
455 | |||
456 | if (((colObj0 != null) && (colObj0.MergesSimulationIslands)) && | ||
457 | ((colObj1 != null) && (colObj1.MergesSimulationIslands))) | ||
458 | { | ||
459 | if (colObj0.IsActive || colObj1.IsActive) | ||
460 | { | ||
461 | |||
462 | SimulationIslandManager.UnionFind.Unite((colObj0).IslandTag, | ||
463 | (colObj1).IslandTag); | ||
464 | } | ||
465 | } | ||
466 | } | ||
467 | |||
468 | //Store the island id in each body | ||
469 | SimulationIslandManager.StoreIslandActivationState(this); | ||
470 | } | ||
471 | |||
472 | //protected void SolveNonContactConstraints(ContactSolverInfo solverInfo) | ||
473 | //{ | ||
474 | // //constraint preparation: building jacobians | ||
475 | // for (int i = 0; i < _constraints.Count; i++) | ||
476 | // { | ||
477 | // TypedConstraint constraint = _constraints[i]; | ||
478 | // constraint.BuildJacobian(); | ||
479 | // } | ||
480 | |||
481 | // //solve the regular non-contact constraints (point 2 point, hinge, generic d6) | ||
482 | // for (int g = 0; g < solverInfo.IterationsCount; g++) | ||
483 | // { | ||
484 | // for (int i = 0; i < _constraints.Count; i++) | ||
485 | // { | ||
486 | // TypedConstraint constraint = _constraints[i]; | ||
487 | // constraint.SolveConstraint(solverInfo.TimeStep); | ||
488 | // } | ||
489 | // } | ||
490 | //} | ||
491 | |||
492 | //protected void SolveContactConstraints(ContactSolverInfo solverInfo) | ||
493 | //{ | ||
494 | // InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, _debugDrawer); | ||
495 | |||
496 | // // solve all the contact points and contact friction | ||
497 | // _islandManager.BuildAndProcessIslands(Dispatcher, CollisionObjects, solverCallback); | ||
498 | //} | ||
499 | |||
500 | protected void SolveConstraints(ContactSolverInfo solverInfo) | ||
501 | { | ||
502 | //sorted version of all btTypedConstraint, based on islandId | ||
503 | List<TypedConstraint> sortedConstraints = new List<TypedConstraint>(ConstraintsCount); | ||
504 | |||
505 | for (int i = 0; i < ConstraintsCount; i++) | ||
506 | { | ||
507 | sortedConstraints.Add(_constraints[i]); | ||
508 | } | ||
509 | |||
510 | sortedConstraints.Sort(new Comparison<TypedConstraint>(TypedConstraint.SortConstraintOnIslandPredicate)); | ||
511 | List<TypedConstraint> constraintsPtr = ConstraintsCount != 0 ? sortedConstraints : new List<TypedConstraint>(); | ||
512 | |||
513 | InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, constraintsPtr, _debugDrawer); | ||
514 | |||
515 | // solve all the constraints for this island | ||
516 | _islandManager.BuildAndProcessIslands(CollisionWorld.Dispatcher, CollisionWorld.CollisionObjects, solverCallback); | ||
517 | } | ||
518 | |||
519 | protected void UpdateActivationState(float timeStep) | ||
520 | { | ||
521 | for (int i = 0; i < CollisionObjects.Count; i++) | ||
522 | { | ||
523 | CollisionObject colObj = CollisionObjects[i]; | ||
524 | RigidBody body = RigidBody.Upcast(colObj); | ||
525 | if (body != null) | ||
526 | { | ||
527 | body.UpdateDeactivation(timeStep); | ||
528 | |||
529 | if (body.WantsSleeping()) | ||
530 | { | ||
531 | if (body.IsStaticOrKinematicObject) | ||
532 | { | ||
533 | body.ActivationState = ActivationState.IslandSleeping; | ||
534 | } | ||
535 | else | ||
536 | { | ||
537 | if (body.ActivationState == ActivationState.Active) | ||
538 | body.ActivationState = ActivationState.WantsDeactivation; | ||
539 | } | ||
540 | } | ||
541 | else | ||
542 | { | ||
543 | if (body.ActivationState != ActivationState.DisableDeactivation) | ||
544 | body.ActivationState = ActivationState.Active; | ||
545 | } | ||
546 | } | ||
547 | } | ||
548 | } | ||
549 | |||
550 | protected void UpdateVehicles(float timeStep) | ||
551 | { | ||
552 | for (int i = 0; i < _vehicles.Count; i++) | ||
553 | { | ||
554 | RaycastVehicle vehicle = _vehicles[i]; | ||
555 | vehicle.updateVehicle(timeStep); | ||
556 | } | ||
557 | } | ||
558 | |||
559 | protected void StartProfiling(float timeStep) { } | ||
560 | |||
561 | protected virtual void InternalSingleStepSimulation(float timeStep) | ||
562 | { | ||
563 | StartProfiling(timeStep); | ||
564 | |||
565 | //update aabbs information | ||
566 | UpdateAabbs(); | ||
567 | |||
568 | //apply gravity, predict motion | ||
569 | PredictUnconstraintMotion(timeStep); | ||
570 | |||
571 | DispatcherInfo dispatchInfo = DispatchInfo; | ||
572 | dispatchInfo.TimeStep = timeStep; | ||
573 | dispatchInfo.StepCount = 0; | ||
574 | dispatchInfo.DebugDraw = DebugDrawer; | ||
575 | |||
576 | //perform collision detection | ||
577 | PerformDiscreteCollisionDetection(); | ||
578 | |||
579 | CalculateSimulationIslands(); | ||
580 | |||
581 | SolverInfo.TimeStep = timeStep; | ||
582 | |||
583 | //solve contact and other joint constraints | ||
584 | SolveConstraints(SolverInfo); | ||
585 | |||
586 | //CallbackTriggers(); | ||
587 | |||
588 | //integrate transforms | ||
589 | IntegrateTransforms(timeStep); | ||
590 | |||
591 | //update vehicle simulation | ||
592 | UpdateVehicles(timeStep); | ||
593 | |||
594 | UpdateActivationState(timeStep); | ||
595 | } | ||
596 | |||
597 | protected void SynchronizeMotionStates() | ||
598 | { | ||
599 | //debug vehicle wheels | ||
600 | { | ||
601 | //todo: iterate over awake simulation islands! | ||
602 | for (int i = 0; i < CollisionObjects.Count; i++) | ||
603 | { | ||
604 | CollisionObject colObj = CollisionObjects[i]; | ||
605 | if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0) | ||
606 | { | ||
607 | Vector3 color = new Vector3(255f, 255f, 255f); | ||
608 | switch (colObj.ActivationState) | ||
609 | { | ||
610 | case ActivationState.Active: | ||
611 | color = new Vector3(255f, 255f, 255f); break; | ||
612 | case ActivationState.IslandSleeping: | ||
613 | color = new Vector3(0f, 255f, 0f); break; | ||
614 | case ActivationState.WantsDeactivation: | ||
615 | color = new Vector3(0f, 255f, 255f); break; | ||
616 | case ActivationState.DisableDeactivation: | ||
617 | color = new Vector3(255f, 0f, 0f); break; | ||
618 | case ActivationState.DisableSimulation: | ||
619 | color = new Vector3(255f, 255f, 0f); break; | ||
620 | default: | ||
621 | color = new Vector3(255f, 0f, 0f); break; | ||
622 | } | ||
623 | |||
624 | DebugDrawObject(colObj.WorldTransform, colObj.CollisionShape, color); | ||
625 | } | ||
626 | RigidBody body = RigidBody.Upcast(colObj); | ||
627 | if (body != null && body.MotionState != null && !body.IsStaticOrKinematicObject) | ||
628 | { | ||
629 | //if (body.ActivationState != ActivationState.IslandSleeping) | ||
630 | { | ||
631 | Matrix interpolatedTransform = new Matrix(); | ||
632 | TransformUtil.IntegrateTransform(body.InterpolationWorldTransform, | ||
633 | body.InterpolationLinearVelocity, body.InterpolationAngularVelocity, _localTime, ref interpolatedTransform); | ||
634 | body.MotionState.SetWorldTransform(interpolatedTransform); | ||
635 | } | ||
636 | } | ||
637 | } | ||
638 | } | ||
639 | |||
640 | if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0) | ||
641 | { | ||
642 | for (int i = 0; i < _vehicles.Count; i++) | ||
643 | { | ||
644 | for (int v = 0; v < _vehicles[i].getNumWheels(); v++) | ||
645 | { | ||
646 | Vector3 wheelColor = new Vector3(0, 255, 255); | ||
647 | if (_vehicles[i].getWheelInfo(v).RaycastInfo.IsInContact) | ||
648 | { | ||
649 | wheelColor = new Vector3(0, 0, 255); | ||
650 | } | ||
651 | else | ||
652 | { | ||
653 | wheelColor = new Vector3(255, 0, 255); | ||
654 | } | ||
655 | |||
656 | //synchronize the wheels with the (interpolated) chassis worldtransform | ||
657 | _vehicles[i].updateWheelTransform(v, true); | ||
658 | |||
659 | Vector3 wheelPosWS = _vehicles[i].getWheelInfo(v).WorldTransform.Translation; | ||
660 | |||
661 | Vector3 axle = new Vector3( | ||
662 | MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 0, _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())); | ||
665 | |||
666 | |||
667 | //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS | ||
668 | //debug wheels (cylinders) | ||
669 | _debugDrawer.DrawLine(wheelPosWS, wheelPosWS + axle, wheelColor); | ||
670 | _debugDrawer.DrawLine(wheelPosWS, _vehicles[i].getWheelInfo(v).RaycastInfo.ContactPointWS, wheelColor); | ||
671 | } | ||
672 | } | ||
673 | } | ||
674 | } | ||
675 | |||
676 | protected void SaveKinematicState(float timeStep) | ||
677 | { | ||
678 | for (int i = 0; i < CollisionObjects.Count; i++) | ||
679 | { | ||
680 | CollisionObject colObj = CollisionObjects[i]; | ||
681 | RigidBody body = RigidBody.Upcast(colObj); | ||
682 | if (body != null) | ||
683 | { | ||
684 | if (body.ActivationState != ActivationState.IslandSleeping) | ||
685 | { | ||
686 | if (body.IsKinematicObject) | ||
687 | { | ||
688 | //to calculate velocities next frame | ||
689 | body.SaveKinematicState(timeStep); | ||
690 | } | ||
691 | } | ||
692 | } | ||
693 | } | ||
694 | } | ||
695 | |||
696 | internal class InplaceSolverIslandCallback : SimulationIslandManager.IIslandCallback | ||
697 | { | ||
698 | private ContactSolverInfo _solverInfo; | ||
699 | private IConstraintSolver _solver; | ||
700 | private IDebugDraw _debugDrawer; | ||
701 | private List<TypedConstraint> _sortedConstraints; | ||
702 | |||
703 | public InplaceSolverIslandCallback( | ||
704 | ContactSolverInfo solverInfo, | ||
705 | IConstraintSolver solver, | ||
706 | List<TypedConstraint> sortedConstraints, | ||
707 | IDebugDraw debugDrawer) | ||
708 | { | ||
709 | _solverInfo = solverInfo; | ||
710 | _solver = solver; | ||
711 | _sortedConstraints = sortedConstraints; | ||
712 | _debugDrawer = debugDrawer; | ||
713 | } | ||
714 | |||
715 | public ContactSolverInfo SolverInfo { get { return _solverInfo; } set { _solverInfo = value; } } | ||
716 | public IConstraintSolver Solver { get { return _solver; } set { _solver = value; } } | ||
717 | public List<TypedConstraint> Constraints { get { return _sortedConstraints; } set { _sortedConstraints = value; } } | ||
718 | public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } } | ||
719 | |||
720 | public void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID) | ||
721 | { | ||
722 | //also add all non-contact constraints/joints for this island | ||
723 | List<TypedConstraint> startConstraint = new List<TypedConstraint>(); | ||
724 | int numCurConstraints = 0; | ||
725 | int startIndex = 0; | ||
726 | int i; | ||
727 | |||
728 | //find the first constraint for this island | ||
729 | for (i = 0; i < _sortedConstraints.Count; i++) | ||
730 | { | ||
731 | if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID) | ||
732 | { | ||
733 | //startConstraint = &m_sortedConstraints[i]; | ||
734 | startIndex = i; | ||
735 | break; | ||
736 | } | ||
737 | } | ||
738 | //count the number of constraints in this island | ||
739 | for (; i < _sortedConstraints.Count; i++) | ||
740 | { | ||
741 | if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID) | ||
742 | { | ||
743 | numCurConstraints++; | ||
744 | } | ||
745 | } | ||
746 | |||
747 | for (i = startIndex; i < startIndex + numCurConstraints; i++) | ||
748 | { | ||
749 | startConstraint.Add(_sortedConstraints[i]); | ||
750 | } | ||
751 | |||
752 | _solver.SolveGroup(bodies, manifolds, numManifolds, startConstraint, _solverInfo, _debugDrawer); | ||
753 | } | ||
754 | } | ||
755 | } | ||
756 | |||
757 | internal class DebugDrawCallback : ITriangleIndexCallback, ITriangleCallback | ||
758 | { | ||
759 | private IDebugDraw _debugDrawer; | ||
760 | private Vector3 _color; | ||
761 | private Matrix _worldTrans; | ||
762 | |||
763 | public DebugDrawCallback(IDebugDraw debugDrawer, Matrix worldTrans, Vector3 color) | ||
764 | { | ||
765 | _debugDrawer = debugDrawer; | ||
766 | _worldTrans = worldTrans; | ||
767 | _color = color; | ||
768 | } | ||
769 | |||
770 | public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) | ||
771 | { | ||
772 | ProcessTriangle(triangle, partId, triangleIndex); | ||
773 | } | ||
774 | |||
775 | #region ITriangleCallback Members | ||
776 | |||
777 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) | ||
778 | { | ||
779 | Vector3 wv0, wv1, wv2; | ||
780 | wv0 = Vector3.TransformNormal(triangle[0], _worldTrans); | ||
781 | wv1 = Vector3.TransformNormal(triangle[1], _worldTrans); | ||
782 | wv2 = Vector3.TransformNormal(triangle[2], _worldTrans); | ||
783 | _debugDrawer.DrawLine(wv0, wv1, _color); | ||
784 | _debugDrawer.DrawLine(wv1, wv2, _color); | ||
785 | _debugDrawer.DrawLine(wv2, wv0, _color); | ||
786 | } | ||
787 | |||
788 | #endregion | ||
789 | } | ||
790 | } \ No newline at end of file | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs new file mode 100644 index 0000000..647c8ec --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs | |||
@@ -0,0 +1,59 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX.Dynamics | ||
28 | { | ||
29 | public abstract class DynamicsWorld : CollisionWorld | ||
30 | { | ||
31 | public DynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) | ||
32 | : base(dispatcher, pairCache) { } | ||
33 | |||
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 | ||
36 | public abstract Vector3 Gravity { set; } | ||
37 | public abstract IConstraintSolver ConstraintSolver { set; } | ||
38 | public virtual int ConstraintsCount { get { return 0; } } | ||
39 | public abstract IDebugDraw DebugDrawer { get; set; } | ||
40 | |||
41 | //stepSimulation proceeds the simulation over timeStep units | ||
42 | public abstract void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep); | ||
43 | |||
44 | public void StepSimulation(float timeStep) | ||
45 | { | ||
46 | StepSimulation(timeStep, 1, 1f / 60f); | ||
47 | } | ||
48 | |||
49 | public abstract void UpdateAabbs(); | ||
50 | |||
51 | public virtual void AddConstraint(TypedConstraint constraint) { } | ||
52 | public virtual void RemoveConstraint(TypedConstraint constraint) { } | ||
53 | |||
54 | public abstract void AddRigidBody(RigidBody body); | ||
55 | public abstract void RemoveRigidBody(RigidBody body); | ||
56 | |||
57 | public virtual TypedConstraint GetConstraint(int index) { return null; } | ||
58 | } | ||
59 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs new file mode 100644 index 0000000..ec14daf --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs | |||
@@ -0,0 +1,447 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX.Dynamics | ||
28 | { | ||
29 | public class RigidBody : CollisionObject | ||
30 | { | ||
31 | private static float _linearAirDamping = 1; | ||
32 | //'temporarily' global variables | ||
33 | private static float _rigidBodyDeactivationTime = 2; | ||
34 | private static bool _disableDeactivation = false; | ||
35 | |||
36 | private static float _linearSleepingThreshold = 0.8f; | ||
37 | private static float _angularSleepingThreshold = 1.0f; | ||
38 | private static int _uniqueId = 0; | ||
39 | |||
40 | private Matrix _invInertiaTensorWorld; | ||
41 | private Vector3 _linearVelocity; | ||
42 | private Vector3 _angularVelocity; | ||
43 | private float _inverseMass; | ||
44 | private float _angularFactor; | ||
45 | |||
46 | private Vector3 _gravity; | ||
47 | private Vector3 _invInertiaLocal; | ||
48 | private Vector3 _totalForce; | ||
49 | private Vector3 _totalTorque; | ||
50 | |||
51 | private float _linearDamping; | ||
52 | private float _angularDamping; | ||
53 | |||
54 | //m_optionalMotionState allows to automatic synchronize the world transform for active objects | ||
55 | private MotionState _optionalMotionState; | ||
56 | |||
57 | //for experimental overriding of friction/contact solver func | ||
58 | private ContactSolverType _contactSolverType; | ||
59 | private ContactSolverType _frictionSolverType; | ||
60 | |||
61 | private int _debugBodyId; | ||
62 | |||
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) | ||
65 | //note there this influences deactivation thresholds! | ||
66 | private float _clippedAngvelThresholdSqr = 0.01f; | ||
67 | private float _clippedLinearThresholdSqr = 0.01f; | ||
68 | |||
69 | private float _jitterVelocityDampingFactor = 0.7f; | ||
70 | |||
71 | public RigidBody(float mass, MotionState motionState, CollisionShape collisionShape, Vector3 localInertia, float linearDamping, float angularDamping, float friction, float restitution) | ||
72 | { | ||
73 | _optionalMotionState = motionState; | ||
74 | _angularFactor = 1; | ||
75 | _angularDamping = 0.5f; | ||
76 | |||
77 | if (motionState != null) | ||
78 | { | ||
79 | motionState.GetWorldTransform(out _worldTransform); | ||
80 | } | ||
81 | else | ||
82 | { | ||
83 | WorldTransform = Matrix.Identity; | ||
84 | } | ||
85 | |||
86 | InterpolationWorldTransform = WorldTransform; | ||
87 | InterpolationLinearVelocity = new Vector3(); | ||
88 | InterpolationAngularVelocity = new Vector3(); | ||
89 | |||
90 | //moved to btCollisionObject | ||
91 | Friction = friction; | ||
92 | Restitution = restitution; | ||
93 | |||
94 | CollisionShape = collisionShape; | ||
95 | _debugBodyId = UniqueID++; | ||
96 | |||
97 | //m_internalOwner is to allow upcasting from collision object to rigid body | ||
98 | Owner = this; | ||
99 | |||
100 | SetMassProps(mass, localInertia); | ||
101 | SetDamping(linearDamping, angularDamping); | ||
102 | UpdateInertiaTensor(); | ||
103 | } | ||
104 | |||
105 | public int DebugBodyID { get { return _debugBodyId; } set { _debugBodyId = value; } } | ||
106 | |||
107 | public ContactSolverType ContactSolverType { get { return _contactSolverType; } set { _contactSolverType = value; } } | ||
108 | public ContactSolverType FrictionSolverType { get { return _frictionSolverType; } set { _frictionSolverType = value; } } | ||
109 | |||
110 | public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } } | ||
111 | |||
112 | //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase? | ||
113 | public bool IsInWorld { get { return Broadphase != null; } } | ||
114 | |||
115 | public Vector3 Gravity | ||
116 | { | ||
117 | get { return _gravity; } | ||
118 | set | ||
119 | { | ||
120 | if (_inverseMass != 0.0f) | ||
121 | { | ||
122 | _gravity = value * (1.0f / _inverseMass); | ||
123 | } | ||
124 | } | ||
125 | } | ||
126 | public Matrix InvInertiaTensorWorld { get { return _invInertiaTensorWorld; } } | ||
127 | public float InverseMass { get { return _inverseMass; } } | ||
128 | public Vector3 InvInertiaDiagLocal { get { return _invInertiaLocal; } set { _invInertiaLocal = value; } } | ||
129 | public Vector3 CenterOfMassPosition { get { return WorldTransform.Translation; } } | ||
130 | public Quaternion Orientation { get { return Quaternion.CreateFromRotationMatrix(WorldTransform); } } | ||
131 | public Matrix CenterOfMassTransform | ||
132 | { | ||
133 | get { return WorldTransform; } | ||
134 | set | ||
135 | { | ||
136 | InterpolationWorldTransform = value; | ||
137 | InterpolationLinearVelocity = LinearVelocity; | ||
138 | InterpolationAngularVelocity = AngularVelocity; | ||
139 | WorldTransform = value; | ||
140 | UpdateInertiaTensor(); | ||
141 | } | ||
142 | } | ||
143 | |||
144 | public Vector3 LinearVelocity | ||
145 | { | ||
146 | get { return _linearVelocity; } | ||
147 | set | ||
148 | { | ||
149 | if (CollisionFlags == CollisionOptions.StaticObject) | ||
150 | throw new BulletException("Static objects can't have linear velocity!"); | ||
151 | _linearVelocity = value; | ||
152 | } | ||
153 | } | ||
154 | |||
155 | public Vector3 AngularVelocity | ||
156 | { | ||
157 | get { return _angularVelocity; } | ||
158 | set | ||
159 | { | ||
160 | if (CollisionFlags == CollisionOptions.StaticObject) | ||
161 | throw new BulletException("Static objects can't have angular velocity!"); | ||
162 | _angularVelocity = value; | ||
163 | } | ||
164 | } | ||
165 | |||
166 | //MotionState allows to automatic synchronize the world transform for active objects | ||
167 | public MotionState MotionState | ||
168 | { | ||
169 | get { return _optionalMotionState; } | ||
170 | set | ||
171 | { | ||
172 | _optionalMotionState = value; | ||
173 | if (_optionalMotionState != null) | ||
174 | value.GetWorldTransform(out _worldTransform); | ||
175 | } | ||
176 | } | ||
177 | |||
178 | public static float LinearAirDamping { get { return _linearAirDamping; } set { _linearAirDamping = value; } } | ||
179 | public static float RigidBodyDeactivationTime { get { return _rigidBodyDeactivationTime; } set { _rigidBodyDeactivationTime = value; } } | ||
180 | public static bool DisableDeactivation { get { return _disableDeactivation; } set { _disableDeactivation = value; } } | ||
181 | public static float LinearSleepingThreshold { get { return _linearSleepingThreshold; } set { _linearSleepingThreshold = value; } } | ||
182 | public static float AngularSleepingThreshold { get { return _angularSleepingThreshold; } set { _angularSleepingThreshold = value; } } | ||
183 | public static int UniqueID { get { return _uniqueId; } set { _uniqueId = value; } } | ||
184 | |||
185 | public void ProceedToTransform(Matrix newTrans) | ||
186 | { | ||
187 | CenterOfMassTransform = newTrans; | ||
188 | } | ||
189 | |||
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 | ||
192 | public static RigidBody Upcast(CollisionObject colObj) | ||
193 | { | ||
194 | return colObj.Owner as RigidBody; | ||
195 | } | ||
196 | |||
197 | // continuous collision detection needs prediction | ||
198 | public void PredictIntegratedTransform(float step, ref Matrix predictedTransform) | ||
199 | { | ||
200 | if ((_angularVelocity.LengthSquared() < _clippedAngvelThresholdSqr) && | ||
201 | (_linearVelocity.LengthSquared() < _clippedLinearThresholdSqr)) | ||
202 | { | ||
203 | _angularVelocity *= _jitterVelocityDampingFactor; | ||
204 | _linearVelocity *= _jitterVelocityDampingFactor; | ||
205 | } | ||
206 | |||
207 | TransformUtil.IntegrateTransform(WorldTransform, _linearVelocity, _angularVelocity, step, ref predictedTransform); | ||
208 | } | ||
209 | |||
210 | public void SaveKinematicState(float step) | ||
211 | { | ||
212 | //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities | ||
213 | if (step != 0) | ||
214 | { | ||
215 | //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform | ||
216 | if (MotionState != null) | ||
217 | MotionState.GetWorldTransform(out _worldTransform); | ||
218 | |||
219 | TransformUtil.CalculateVelocity(InterpolationWorldTransform, WorldTransform, step, ref _linearVelocity, ref _angularVelocity); | ||
220 | InterpolationLinearVelocity = _linearVelocity; | ||
221 | InterpolationAngularVelocity = _angularVelocity; | ||
222 | InterpolationWorldTransform = WorldTransform; | ||
223 | } | ||
224 | } | ||
225 | |||
226 | public void ApplyForces(float step) | ||
227 | { | ||
228 | if (IsStaticOrKinematicObject) | ||
229 | return; | ||
230 | |||
231 | ApplyCentralForce(_gravity); | ||
232 | |||
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)); | ||
235 | |||
236 | float speed = _linearVelocity.Length(); | ||
237 | if (speed < _linearDamping) | ||
238 | { | ||
239 | float dampVel = 0.005f; | ||
240 | if (speed > dampVel) | ||
241 | { | ||
242 | Vector3 dir = _linearVelocity; | ||
243 | dir.Normalize(); | ||
244 | _linearVelocity -= dir * dampVel; | ||
245 | } | ||
246 | else | ||
247 | { | ||
248 | _linearVelocity = new Vector3(); | ||
249 | } | ||
250 | } | ||
251 | |||
252 | float angSpeed = _angularVelocity.Length(); | ||
253 | if (angSpeed < _angularDamping) | ||
254 | { | ||
255 | float angDampVel = 0.005f; | ||
256 | if (angSpeed > angDampVel) | ||
257 | { | ||
258 | Vector3 dir = _angularVelocity; | ||
259 | dir.Normalize(); | ||
260 | _angularVelocity -= dir * angDampVel; | ||
261 | } | ||
262 | else | ||
263 | { | ||
264 | _angularVelocity = new Vector3(); | ||
265 | } | ||
266 | } | ||
267 | } | ||
268 | |||
269 | public void SetDamping(float linDamping, float angDamping) | ||
270 | { | ||
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); | ||
273 | } | ||
274 | |||
275 | public void SetMassProps(float mass, Vector3 inertia) | ||
276 | { | ||
277 | if (mass == 0) | ||
278 | { | ||
279 | CollisionFlags |= CollisionOptions.StaticObject; | ||
280 | _inverseMass = 0; | ||
281 | } | ||
282 | else | ||
283 | { | ||
284 | CollisionFlags &= (~CollisionOptions.StaticObject); | ||
285 | _inverseMass = 1.0f / mass; | ||
286 | } | ||
287 | |||
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, | ||
290 | inertia.Z != 0.0f ? 1.0f / inertia.Z : 0.0f); | ||
291 | } | ||
292 | |||
293 | public void IntegrateVelocities(float step) | ||
294 | { | ||
295 | if (IsStaticOrKinematicObject) | ||
296 | return; | ||
297 | |||
298 | _linearVelocity += _totalForce * (_inverseMass * step); | ||
299 | _angularVelocity += Vector3.TransformNormal(_totalTorque, _invInertiaTensorWorld) * step; | ||
300 | |||
301 | //float MAX_ANGVEL = MonoXnaCompactMaths.MathHelper.PiOver2; | ||
302 | float MAX_ANGVEL = (float)Math.PI / 2.0f; | ||
303 | /// clamp angular velocity. collision calculations will fail on higher angular velocities | ||
304 | float angvel = _angularVelocity.Length(); | ||
305 | if (angvel * step > MAX_ANGVEL) | ||
306 | { | ||
307 | _angularVelocity *= (MAX_ANGVEL / step) / angvel; | ||
308 | } | ||
309 | |||
310 | ClearForces(); | ||
311 | } | ||
312 | |||
313 | public void ApplyCentralForce(Vector3 force) | ||
314 | { | ||
315 | _totalForce += force; | ||
316 | } | ||
317 | |||
318 | public void ApplyTorque(Vector3 torque) | ||
319 | { | ||
320 | _totalTorque += torque; | ||
321 | } | ||
322 | |||
323 | public void ApplyForce(Vector3 force, Vector3 rel_pos) | ||
324 | { | ||
325 | ApplyCentralForce(force); | ||
326 | ApplyTorque(Vector3.Cross(rel_pos, force)); | ||
327 | } | ||
328 | |||
329 | public void ApplyCentralImpulse(Vector3 impulse) | ||
330 | { | ||
331 | _linearVelocity += impulse * _inverseMass; | ||
332 | } | ||
333 | |||
334 | public void ApplyTorqueImpulse(Vector3 torque) | ||
335 | { | ||
336 | _angularVelocity += Vector3.TransformNormal(torque, _invInertiaTensorWorld); | ||
337 | } | ||
338 | |||
339 | public void ApplyImpulse(Vector3 impulse, Vector3 rel_pos) | ||
340 | { | ||
341 | if (_inverseMass != 0) | ||
342 | { | ||
343 | ApplyCentralImpulse(impulse); | ||
344 | if (_angularFactor != 0) | ||
345 | ApplyTorqueImpulse(Vector3.Cross(rel_pos, impulse) * _angularFactor); | ||
346 | } | ||
347 | } | ||
348 | |||
349 | public void InternalApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude) | ||
350 | { | ||
351 | if (_inverseMass != 0) | ||
352 | { | ||
353 | _linearVelocity += linearComponent * impulseMagnitude; | ||
354 | if (_angularFactor != 0) | ||
355 | _angularVelocity += angularComponent * impulseMagnitude * _angularFactor; | ||
356 | } | ||
357 | } | ||
358 | |||
359 | public void ClearForces() | ||
360 | { | ||
361 | _totalForce = new Vector3(); | ||
362 | _totalTorque = new Vector3(); | ||
363 | } | ||
364 | |||
365 | public void UpdateInertiaTensor() | ||
366 | { | ||
367 | Matrix temp = WorldTransform; | ||
368 | temp.Translation = Vector3.Zero; | ||
369 | _invInertiaTensorWorld = MatrixOperations.Multiply(MatrixOperations.Scaled(WorldTransform, _invInertiaLocal), Matrix.Transpose(temp)); | ||
370 | } | ||
371 | |||
372 | public Vector3 GetVelocityInLocalPoint(Vector3 relPos) | ||
373 | { | ||
374 | //we also calculate lin/ang velocity for kinematic objects | ||
375 | return _linearVelocity + Vector3.Cross(_angularVelocity, relPos); | ||
376 | |||
377 | //for kinematic objects, we could also use use: | ||
378 | // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; | ||
379 | } | ||
380 | |||
381 | public void Translate(Vector3 v) | ||
382 | { | ||
383 | Matrix m = WorldTransform; | ||
384 | m.Translation += v; | ||
385 | WorldTransform = m; | ||
386 | } | ||
387 | |||
388 | public void GetAabb(out Vector3 aabbMin, out Vector3 aabbMax) | ||
389 | { | ||
390 | CollisionShape.GetAabb(WorldTransform, out aabbMin, out aabbMax); | ||
391 | } | ||
392 | |||
393 | public float ComputeImpulseDenominator(Vector3 pos, Vector3 normal) | ||
394 | { | ||
395 | Vector3 r0 = pos - CenterOfMassPosition; | ||
396 | Vector3 c0 = Vector3.Cross(r0, normal); | ||
397 | Vector3 vec = Vector3.Cross(Vector3.TransformNormal(c0, InvInertiaTensorWorld), r0); | ||
398 | |||
399 | return _inverseMass + Vector3.Dot(normal, vec); | ||
400 | |||
401 | } | ||
402 | |||
403 | public float ComputeAngularImpulseDenominator(Vector3 axis) | ||
404 | { | ||
405 | Vector3 vec = Vector3.TransformNormal(axis, InvInertiaTensorWorld); | ||
406 | return Vector3.Dot(axis, vec); | ||
407 | } | ||
408 | |||
409 | public void UpdateDeactivation(float timeStep) | ||
410 | { | ||
411 | if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.DisableDeactivation)) | ||
412 | return; | ||
413 | |||
414 | if ((LinearVelocity.LengthSquared() < LinearSleepingThreshold * LinearSleepingThreshold) && | ||
415 | (AngularVelocity.LengthSquared() < AngularSleepingThreshold * AngularSleepingThreshold)) | ||
416 | { | ||
417 | DeactivationTime += timeStep; | ||
418 | } | ||
419 | else | ||
420 | { | ||
421 | DeactivationTime = 0; | ||
422 | ActivationState = ActivationState.Nothing; | ||
423 | } | ||
424 | |||
425 | } | ||
426 | |||
427 | public bool WantsSleeping() | ||
428 | { | ||
429 | |||
430 | if (ActivationState == ActivationState.DisableDeactivation) | ||
431 | return false; | ||
432 | |||
433 | //disable deactivation | ||
434 | if (DisableDeactivation || (RigidBodyDeactivationTime == 0)) | ||
435 | return false; | ||
436 | |||
437 | if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.WantsDeactivation)) | ||
438 | return true; | ||
439 | |||
440 | if (DeactivationTime > RigidBodyDeactivationTime) | ||
441 | { | ||
442 | return true; | ||
443 | } | ||
444 | return false; | ||
445 | } | ||
446 | } | ||
447 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs new file mode 100644 index 0000000..aa91a03 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs | |||
@@ -0,0 +1,211 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX.Dynamics | ||
28 | { | ||
29 | public class SimpleDynamicsWorld : DynamicsWorld | ||
30 | { | ||
31 | private IConstraintSolver _constraintSolver; | ||
32 | private bool _ownsConstraintSolver; | ||
33 | private Vector3 _gravity; | ||
34 | private IDebugDraw _debugDrawer; | ||
35 | |||
36 | /// <summary> | ||
37 | /// this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver | ||
38 | /// </summary> | ||
39 | /// <param name="dispatcher"></param> | ||
40 | /// <param name="pairCache"></param> | ||
41 | /// <param name="constraintSolver"></param> | ||
42 | public SimpleDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver) | ||
43 | : base(dispatcher, pairCache) | ||
44 | { | ||
45 | _constraintSolver = constraintSolver; | ||
46 | _ownsConstraintSolver = false; | ||
47 | _gravity = new Vector3(0, 0, -10); | ||
48 | } | ||
49 | |||
50 | public override Vector3 Gravity | ||
51 | { | ||
52 | set | ||
53 | { | ||
54 | _gravity = value; | ||
55 | for (int i = 0; i < CollisionObjects.Count; i++) | ||
56 | { | ||
57 | CollisionObject colObj = CollisionObjects[i]; | ||
58 | RigidBody body = RigidBody.Upcast(colObj); | ||
59 | if (body != null) | ||
60 | { | ||
61 | body.Gravity = value; | ||
62 | } | ||
63 | } | ||
64 | } | ||
65 | } | ||
66 | |||
67 | public override IConstraintSolver ConstraintSolver | ||
68 | { | ||
69 | set | ||
70 | { | ||
71 | _ownsConstraintSolver = false; | ||
72 | _constraintSolver = value; | ||
73 | } | ||
74 | } | ||
75 | |||
76 | public override IDebugDraw DebugDrawer | ||
77 | { | ||
78 | get | ||
79 | { | ||
80 | return _debugDrawer; | ||
81 | } | ||
82 | set | ||
83 | { | ||
84 | _debugDrawer = value; | ||
85 | } | ||
86 | } | ||
87 | |||
88 | public override void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep) | ||
89 | { | ||
90 | //apply gravity, predict motion | ||
91 | PredictUnconstraintMotion(timeStep); | ||
92 | |||
93 | DispatcherInfo dispatchInfo = new DispatcherInfo(); | ||
94 | dispatchInfo.TimeStep = timeStep; | ||
95 | dispatchInfo.StepCount = 0; | ||
96 | dispatchInfo.DebugDraw = DebugDrawer; | ||
97 | //perform collision detection | ||
98 | PerformDiscreteCollisionDetection(); | ||
99 | |||
100 | //solve contact constraints | ||
101 | int numManifolds = Dispatcher.ManifoldCount; | ||
102 | if (numManifolds != 0) | ||
103 | { | ||
104 | |||
105 | List<PersistentManifold> manifolds = (Dispatcher as CollisionDispatcher).Manifolds; | ||
106 | //int numManifolds = m_dispatcher1.GetNumManifolds(); | ||
107 | ContactSolverInfo infoGlobal = new ContactSolverInfo(); | ||
108 | infoGlobal.TimeStep = timeStep; | ||
109 | |||
110 | _constraintSolver.SolveGroup(new List<CollisionObject>(), manifolds, manifolds.Count, new List<TypedConstraint>(), infoGlobal, _debugDrawer); | ||
111 | } | ||
112 | //integrate transforms | ||
113 | IntegrateTransforms(timeStep); | ||
114 | |||
115 | UpdateAabbs(); | ||
116 | |||
117 | SynchronizeMotionStates(); | ||
118 | } | ||
119 | |||
120 | public override void UpdateAabbs() | ||
121 | { | ||
122 | for (int i = 0; i < CollisionObjects.Count; i++) | ||
123 | { | ||
124 | CollisionObject colObj = CollisionObjects[i]; | ||
125 | RigidBody body = RigidBody.Upcast(colObj); | ||
126 | if (body != null) | ||
127 | { | ||
128 | if (body.IsActive && (!body.IsStaticObject)) | ||
129 | { | ||
130 | Vector3 minAabb, maxAabb; | ||
131 | colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb); | ||
132 | IBroadphase bp = Broadphase; | ||
133 | bp.SetAabb(body.Broadphase, minAabb, maxAabb); | ||
134 | } | ||
135 | } | ||
136 | } | ||
137 | } | ||
138 | |||
139 | public override void AddRigidBody(RigidBody body) | ||
140 | { | ||
141 | body.Gravity = _gravity; | ||
142 | |||
143 | if (body.CollisionShape != null) | ||
144 | { | ||
145 | AddCollisionObject(body); | ||
146 | } | ||
147 | } | ||
148 | |||
149 | public override void RemoveRigidBody(RigidBody body) | ||
150 | { | ||
151 | RemoveCollisionObject(body); | ||
152 | } | ||
153 | |||
154 | public void SynchronizeMotionStates() | ||
155 | { | ||
156 | for (int i = 0; i < CollisionObjects.Count; i++) | ||
157 | { | ||
158 | CollisionObject colObj = CollisionObjects[i]; | ||
159 | RigidBody body = RigidBody.Upcast(colObj); | ||
160 | if (body != null && body.MotionState != null) | ||
161 | { | ||
162 | if (body.ActivationState != ActivationState.IslandSleeping) | ||
163 | { | ||
164 | body.MotionState.SetWorldTransform(body.WorldTransform); | ||
165 | } | ||
166 | } | ||
167 | } | ||
168 | } | ||
169 | |||
170 | protected void PredictUnconstraintMotion(float timeStep) | ||
171 | { | ||
172 | for (int i = 0; i < CollisionObjects.Count; i++) | ||
173 | { | ||
174 | CollisionObject colObj = CollisionObjects[i]; | ||
175 | RigidBody body = RigidBody.Upcast(colObj); | ||
176 | if (body != null) | ||
177 | { | ||
178 | if (!body.IsStaticObject) | ||
179 | { | ||
180 | if (body.IsActive) | ||
181 | { | ||
182 | body.ApplyForces(timeStep); | ||
183 | body.IntegrateVelocities(timeStep); | ||
184 | Matrix temp = body.InterpolationWorldTransform; | ||
185 | body.PredictIntegratedTransform(timeStep, ref temp); | ||
186 | body.InterpolationWorldTransform = temp; | ||
187 | } | ||
188 | } | ||
189 | } | ||
190 | } | ||
191 | } | ||
192 | |||
193 | protected void IntegrateTransforms(float timeStep) | ||
194 | { | ||
195 | Matrix predictedTrans = Matrix.Identity; | ||
196 | for (int i = 0; i < CollisionObjects.Count; i++) | ||
197 | { | ||
198 | CollisionObject colObj = CollisionObjects[i]; | ||
199 | RigidBody body = RigidBody.Upcast(colObj); | ||
200 | if (body != null) | ||
201 | { | ||
202 | if (body.IsActive && (!body.IsStaticObject)) | ||
203 | { | ||
204 | body.PredictIntegratedTransform(timeStep, ref predictedTrans); | ||
205 | body.ProceedToTransform(predictedTrans); | ||
206 | } | ||
207 | } | ||
208 | } | ||
209 | } | ||
210 | } | ||
211 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs new file mode 100644 index 0000000..90db9ee --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs | |||
@@ -0,0 +1,94 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX.Dynamics | ||
28 | { | ||
29 | public class RaycastVehicle : TypedConstraint | ||
30 | { | ||
31 | public override void BuildJacobian() | ||
32 | { | ||
33 | throw new Exception("The method or operation is not implemented."); | ||
34 | } | ||
35 | |||
36 | public override void SolveConstraint(float timeStep) | ||
37 | { | ||
38 | throw new Exception("The method or operation is not implemented."); | ||
39 | } | ||
40 | |||
41 | public int getNumWheels() | ||
42 | { | ||
43 | throw new Exception("The method or operation is not implemented."); | ||
44 | } | ||
45 | |||
46 | public WheelInfo getWheelInfo(int v) | ||
47 | { | ||
48 | throw new Exception("The method or operation is not implemented."); | ||
49 | } | ||
50 | |||
51 | public void updateWheelTransform(int v, bool p) | ||
52 | { | ||
53 | throw new Exception("The method or operation is not implemented."); | ||
54 | } | ||
55 | |||
56 | public int getRightAxis() | ||
57 | { | ||
58 | throw new Exception("The method or operation is not implemented."); | ||
59 | } | ||
60 | |||
61 | public void updateVehicle(float timeStep) | ||
62 | { | ||
63 | throw new Exception("The method or operation is not implemented."); | ||
64 | } | ||
65 | } | ||
66 | |||
67 | public class DefaultVehicleRaycaster : IVehicleRaycaster | ||
68 | { | ||
69 | DynamicsWorld _dynamicsWorld; | ||
70 | |||
71 | public DefaultVehicleRaycaster(DynamicsWorld world) | ||
72 | { | ||
73 | _dynamicsWorld = world; | ||
74 | } | ||
75 | |||
76 | public object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result) | ||
77 | { | ||
78 | CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(from, to); | ||
79 | _dynamicsWorld.RayTest(from, to, rayCallback); | ||
80 | |||
81 | result = new VehicleRaycasterResult(); | ||
82 | |||
83 | if (!rayCallback.HasHit) return 0; | ||
84 | RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject); | ||
85 | if (body == null) return 0; | ||
86 | |||
87 | result.HitPointInWorld = rayCallback.HitPointWorld; | ||
88 | result.HitNormalInWorld = rayCallback.HitNormalWorld; | ||
89 | result.HitNormalInWorld.Normalize(); | ||
90 | result.DistFraction = rayCallback.ClosestHitFraction; | ||
91 | return body; | ||
92 | } | ||
93 | } | ||
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 new file mode 100644 index 0000000..86829e3 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs | |||
@@ -0,0 +1,49 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX.Dynamics | ||
28 | { | ||
29 | public interface IVehicleRaycaster | ||
30 | { | ||
31 | object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result); | ||
32 | } | ||
33 | |||
34 | public class VehicleRaycasterResult | ||
35 | { | ||
36 | private Single _distFraction; | ||
37 | private Vector3 _hitNormalInWorld; | ||
38 | private Vector3 _hitPointInWorld; | ||
39 | |||
40 | public VehicleRaycasterResult() | ||
41 | { | ||
42 | _distFraction = -1; | ||
43 | } | ||
44 | |||
45 | public float DistFraction { get { return _distFraction; } set { _distFraction = value; } } | ||
46 | public Vector3 HitNormalInWorld { get { return _hitNormalInWorld; } set { _hitNormalInWorld = value; } } | ||
47 | public Vector3 HitPointInWorld { get { return _hitPointInWorld; } set { _hitPointInWorld = value; } } | ||
48 | } | ||
49 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs new file mode 100644 index 0000000..d4a94f3 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs | |||
@@ -0,0 +1,529 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX.Dynamics | ||
28 | { | ||
29 | public struct WheelInfoConstructionInfo | ||
30 | { | ||
31 | private Vector3 _chassicConnectionCS; | ||
32 | private Vector3 _wheelDirectionCS; | ||
33 | private Vector3 _wheelAxisCS; | ||
34 | |||
35 | private Single _suspensionRestLength; | ||
36 | private Single _maxSuspensionTravelCm; | ||
37 | private Single _wheelRadius; | ||
38 | private Single _suspensionStiffness; | ||
39 | private Single _wheelsDampingCompression; | ||
40 | private Single _wheelsDampingRelaxation; | ||
41 | private Single _frictionSlip; | ||
42 | |||
43 | private Boolean _isFrontWheel; | ||
44 | |||
45 | #region Basic Properties | ||
46 | public Vector3 ChassicConnectionCS | ||
47 | { | ||
48 | get { return _chassicConnectionCS; } | ||
49 | set { _chassicConnectionCS = value; } | ||
50 | } | ||
51 | |||
52 | public Vector3 WheelDirectionCS | ||
53 | { | ||
54 | get { return _wheelDirectionCS; } | ||
55 | set { _wheelDirectionCS = value; } | ||
56 | } | ||
57 | |||
58 | public Vector3 WheelAxleCS | ||
59 | { | ||
60 | get { return _wheelAxisCS; } | ||
61 | set { _wheelAxisCS = value; } | ||
62 | } | ||
63 | |||
64 | |||
65 | public Single SuspensionRestLength | ||
66 | { | ||
67 | get { return _suspensionRestLength; } | ||
68 | set { _suspensionRestLength = value; } | ||
69 | } | ||
70 | |||
71 | public Single MaxSuspensionTravelCm | ||
72 | { | ||
73 | get { return _maxSuspensionTravelCm; } | ||
74 | set { _maxSuspensionTravelCm = value; } | ||
75 | } | ||
76 | |||
77 | public Single WheelRadius | ||
78 | { | ||
79 | get { return _wheelRadius; } | ||
80 | set { _wheelRadius = value; } | ||
81 | } | ||
82 | |||
83 | |||
84 | public Single SuspensionStiffness | ||
85 | { | ||
86 | get { return _suspensionStiffness; } | ||
87 | set { _suspensionStiffness = value; } | ||
88 | } | ||
89 | |||
90 | public Single WheelsDampingCompression | ||
91 | { | ||
92 | get { return _wheelsDampingCompression; } | ||
93 | set { _wheelsDampingCompression = value; } | ||
94 | } | ||
95 | |||
96 | public Single WheelsDampingRelaxation | ||
97 | { | ||
98 | get { return _wheelsDampingRelaxation; } | ||
99 | set { _wheelsDampingRelaxation = value; } | ||
100 | } | ||
101 | |||
102 | public Single FrictionSlip | ||
103 | { | ||
104 | get { return _frictionSlip; } | ||
105 | set { _frictionSlip = value; } | ||
106 | } | ||
107 | |||
108 | |||
109 | public Boolean IsFrontWheel | ||
110 | { | ||
111 | get { return _isFrontWheel; } | ||
112 | set { _isFrontWheel = value; } | ||
113 | } | ||
114 | #endregion | ||
115 | } | ||
116 | |||
117 | public struct RaycastInfo | ||
118 | { | ||
119 | private Vector3 _contractNormalWS; | ||
120 | private Vector3 _contractPointWS; | ||
121 | |||
122 | private Vector3 _hardPointWS; | ||
123 | private Vector3 _wheelDirectionWS; | ||
124 | private Vector3 _wheelAxleWS; | ||
125 | |||
126 | private Single _suspensionLength; | ||
127 | private Boolean _isInContract; | ||
128 | |||
129 | #region Basic Properties | ||
130 | public Single SuspensionLength | ||
131 | { | ||
132 | get { return _suspensionLength; } | ||
133 | set { _suspensionLength = value; } | ||
134 | } | ||
135 | |||
136 | public Boolean IsInContact | ||
137 | { | ||
138 | get { return _isInContract; } | ||
139 | set { _isInContract = value; } | ||
140 | } | ||
141 | |||
142 | public Vector3 ContactNormalWS | ||
143 | { | ||
144 | get { return _contractNormalWS; } | ||
145 | set { _contractNormalWS = value; } | ||
146 | } | ||
147 | |||
148 | public Vector3 ContactPointWS | ||
149 | { | ||
150 | get { return _contractPointWS; } | ||
151 | set { _contractPointWS = value; } | ||
152 | } | ||
153 | |||
154 | public Vector3 HardPointWS | ||
155 | { | ||
156 | get { return _hardPointWS; } | ||
157 | set { _hardPointWS = value; } | ||
158 | } | ||
159 | |||
160 | public Vector3 WheelDirectionWS | ||
161 | { | ||
162 | get { return _wheelDirectionWS; } | ||
163 | set { _wheelDirectionWS = value; } | ||
164 | } | ||
165 | |||
166 | public Vector3 WheelAxleWS | ||
167 | { | ||
168 | get { return _wheelAxleWS; } | ||
169 | set { _wheelAxleWS = value; } | ||
170 | } | ||
171 | #endregion | ||
172 | } | ||
173 | |||
174 | public struct WheelInfo | ||
175 | { | ||
176 | private RaycastInfo _raycastInfo; | ||
177 | |||
178 | private Matrix _worldTransform; | ||
179 | |||
180 | private Vector3 _chassicConnectionPointCS; | ||
181 | private Vector3 _wheelDirectionCS; | ||
182 | private Vector3 _wheelAxleCS; | ||
183 | |||
184 | private Single _suspensionRestLength; | ||
185 | private Single _maxSuspensionTravelCm; | ||
186 | |||
187 | private Single _wheelsRadius; | ||
188 | private Single _rollInfluence; | ||
189 | private Single _suspensionStiffness; | ||
190 | private Single _wheelsDampingCompression; | ||
191 | private Single _wheelsDampingRelaxation; | ||
192 | private Single _frictionSlip; | ||
193 | private Single _steering; | ||
194 | private Single _rotation; | ||
195 | private Single _deltaRotation; | ||
196 | |||
197 | private Single _engineForce; | ||
198 | private Single _brake; | ||
199 | private Boolean _isFrontWheel; | ||
200 | |||
201 | |||
202 | private Single _clippedInvContactDotSuspension; | ||
203 | private Single _skidInfo; | ||
204 | private Single _wheelsSuspensionForce; | ||
205 | private Single _suspensionRelativeVelocity; | ||
206 | //can be used to store pointer to sync transforms... | ||
207 | private object _clientInfo; | ||
208 | |||
209 | #region Constructor | ||
210 | public WheelInfo(WheelInfoConstructionInfo constructionInfo) | ||
211 | { | ||
212 | _suspensionRestLength = constructionInfo.SuspensionRestLength; | ||
213 | _maxSuspensionTravelCm = constructionInfo.MaxSuspensionTravelCm; | ||
214 | |||
215 | _wheelsRadius = constructionInfo.WheelRadius; | ||
216 | _wheelsDampingCompression = constructionInfo.WheelsDampingCompression; | ||
217 | _wheelsDampingRelaxation = constructionInfo.WheelsDampingRelaxation; | ||
218 | _wheelDirectionCS = constructionInfo.WheelDirectionCS; | ||
219 | |||
220 | _suspensionStiffness = constructionInfo.SuspensionStiffness; | ||
221 | _chassicConnectionPointCS = constructionInfo.ChassicConnectionCS; | ||
222 | |||
223 | _wheelAxleCS = constructionInfo.WheelAxleCS; | ||
224 | _frictionSlip = constructionInfo.FrictionSlip; | ||
225 | |||
226 | _clippedInvContactDotSuspension = 0; | ||
227 | _suspensionRelativeVelocity = 0; | ||
228 | _wheelsSuspensionForce = 0; | ||
229 | _skidInfo = 0; | ||
230 | |||
231 | _steering = 0; | ||
232 | _engineForce = 0; | ||
233 | _rotation = 0; | ||
234 | _rotation = 0; | ||
235 | _deltaRotation = 0; | ||
236 | _brake = 0; | ||
237 | _rollInfluence = 0.1f; | ||
238 | _brake = 0; | ||
239 | _rollInfluence = 0.1f; | ||
240 | |||
241 | _isFrontWheel = constructionInfo.IsFrontWheel; | ||
242 | |||
243 | _raycastInfo = default(RaycastInfo); | ||
244 | _worldTransform = default(Matrix); | ||
245 | _clientInfo = null; | ||
246 | } | ||
247 | #endregion | ||
248 | |||
249 | #region BasicProperties | ||
250 | public object ClientInfo { get { return _clientInfo; } set { _clientInfo = value; } } | ||
251 | |||
252 | public RaycastInfo RaycastInfo | ||
253 | { | ||
254 | get { return _raycastInfo; } | ||
255 | set { _raycastInfo = value; } | ||
256 | } | ||
257 | |||
258 | public Matrix WorldTransform | ||
259 | { | ||
260 | get { return _worldTransform; } | ||
261 | set { _worldTransform = value; } | ||
262 | } | ||
263 | |||
264 | public Vector3 ChassicConnectionPointCS | ||
265 | { | ||
266 | get { return _chassicConnectionPointCS; } | ||
267 | set { _chassicConnectionPointCS = value; } | ||
268 | } | ||
269 | public Vector3 WheelDirectionCS | ||
270 | { | ||
271 | get { return _wheelDirectionCS; } | ||
272 | set { _wheelDirectionCS = value; } | ||
273 | } | ||
274 | public Vector3 WheelAxleCS | ||
275 | { | ||
276 | get { return _wheelAxleCS; } | ||
277 | set { _wheelAxleCS = value; } | ||
278 | } | ||
279 | |||
280 | public Single SuspensionRestLength | ||
281 | { | ||
282 | get { return _suspensionRestLength; } | ||
283 | set { _suspensionRestLength = value; } | ||
284 | } | ||
285 | |||
286 | |||
287 | public Single MaxSuspensionTravelCm | ||
288 | { | ||
289 | get { return _maxSuspensionTravelCm; } | ||
290 | set { _maxSuspensionTravelCm = value; } | ||
291 | } | ||
292 | |||
293 | public Single WheelsRadius | ||
294 | { | ||
295 | get { return _wheelsRadius; } | ||
296 | set { _wheelsRadius = value; } | ||
297 | } | ||
298 | |||
299 | public Single SuspensionStiffness | ||
300 | { | ||
301 | get { return _suspensionStiffness; } | ||
302 | set { _suspensionStiffness = value; } | ||
303 | } | ||
304 | |||
305 | public Single WheelsDampingCompression | ||
306 | { | ||
307 | get { return _wheelsDampingCompression; } | ||
308 | set { _wheelsDampingCompression = value; } | ||
309 | } | ||
310 | |||
311 | public Single WheelsDampingRelaxation | ||
312 | { | ||
313 | get { return _wheelsDampingRelaxation; } | ||
314 | set { _wheelsDampingRelaxation = value; } | ||
315 | } | ||
316 | |||
317 | public Single FrictionSlip | ||
318 | { | ||
319 | get { return _frictionSlip; } | ||
320 | set { _frictionSlip = value; } | ||
321 | } | ||
322 | |||
323 | public Single Steering | ||
324 | { | ||
325 | get { return _steering; } | ||
326 | set { _steering = value; } | ||
327 | } | ||
328 | |||
329 | public Single Rotation | ||
330 | { | ||
331 | get { return _rotation; } | ||
332 | set { _rotation = value; } | ||
333 | } | ||
334 | |||
335 | public Single DeltaRotation | ||
336 | { | ||
337 | get { return _deltaRotation; } | ||
338 | set { _deltaRotation = value; } | ||
339 | } | ||
340 | |||
341 | public Single RollInfluence | ||
342 | { | ||
343 | get { return _rollInfluence; } | ||
344 | set { _rollInfluence = value; } | ||
345 | } | ||
346 | |||
347 | public Single EngineForce | ||
348 | { | ||
349 | get { return _engineForce; } | ||
350 | set { _engineForce = value; } | ||
351 | } | ||
352 | |||
353 | public Single Brake | ||
354 | { | ||
355 | get { return _brake; } | ||
356 | set { _brake = value; } | ||
357 | } | ||
358 | |||
359 | public Boolean IsFrontWheel | ||
360 | { | ||
361 | get { return _isFrontWheel; } | ||
362 | set { _isFrontWheel = value; } | ||
363 | } | ||
364 | |||
365 | public Single ClippedInvContactDotSuspension | ||
366 | { | ||
367 | get { return _clippedInvContactDotSuspension; } | ||
368 | set { _clippedInvContactDotSuspension = value; } | ||
369 | } | ||
370 | |||
371 | public Single SuspensionRelativeVelocity | ||
372 | { | ||
373 | get { return _suspensionRelativeVelocity; } | ||
374 | set { _suspensionRelativeVelocity = value; } | ||
375 | } | ||
376 | |||
377 | public Single WheelsSuspensionForce | ||
378 | { | ||
379 | get { return _wheelsSuspensionForce; } | ||
380 | set { _wheelsSuspensionForce = value; } | ||
381 | } | ||
382 | |||
383 | public Single SkidInfo | ||
384 | { | ||
385 | get { return _skidInfo; } | ||
386 | set { _skidInfo = value; } | ||
387 | } | ||
388 | #endregion | ||
389 | |||
390 | /// <summary> | ||
391 | /// | ||
392 | /// </summary> | ||
393 | /// <param name="chassis"></param> | ||
394 | /// <param name="paramRaycastInfo">Not used!</param> | ||
395 | public void UpdateWheel(RigidBody chassis, RaycastInfo paramRaycastInfo) | ||
396 | { | ||
397 | if (_raycastInfo.IsInContact) | ||
398 | { | ||
399 | float project = Vector3.Dot(_raycastInfo.ContactNormalWS, _raycastInfo.WheelDirectionWS); | ||
400 | |||
401 | Vector3 chassisVelocityAtContactPoint = new Vector3(); | ||
402 | Vector3 relpos = _raycastInfo.ContactPointWS - chassis.CenterOfMassPosition; | ||
403 | chassisVelocityAtContactPoint = chassis.GetVelocityInLocalPoint(relpos); | ||
404 | float projVel = Vector3.Dot(_raycastInfo.ContactNormalWS, chassisVelocityAtContactPoint); | ||
405 | |||
406 | if (project >= -0.1f) | ||
407 | { | ||
408 | _suspensionRelativeVelocity = 0; | ||
409 | _clippedInvContactDotSuspension = 1.0f / 0.1f; | ||
410 | } | ||
411 | else | ||
412 | { | ||
413 | float inv = -1 / project; | ||
414 | _suspensionRelativeVelocity = projVel * inv; | ||
415 | _clippedInvContactDotSuspension = inv; | ||
416 | } | ||
417 | } | ||
418 | else | ||
419 | { | ||
420 | _raycastInfo.SuspensionLength = _suspensionRestLength; | ||
421 | _suspensionRelativeVelocity = 0.0f; | ||
422 | _raycastInfo.ContactNormalWS = -_raycastInfo.WheelDirectionWS; | ||
423 | _clippedInvContactDotSuspension = 1.0f; | ||
424 | } | ||
425 | } | ||
426 | |||
427 | // if (m_raycastInfo.m_isInContact) | ||
428 | |||
429 | //{ | ||
430 | // btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS ); | ||
431 | // btVector3 chassis_velocity_at_contactPoint; | ||
432 | // btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition(); | ||
433 | // chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos ); | ||
434 | // btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); | ||
435 | // if ( project >= -0.1f) | ||
436 | // { | ||
437 | // m_suspensionRelativeVelocity = 0.0f; | ||
438 | // m_clippedInvContactDotSuspension = 1.0f / 0.1f; | ||
439 | // } | ||
440 | // else | ||
441 | // { | ||
442 | // btScalar inv = -1.f / project; | ||
443 | // m_suspensionRelativeVelocity = projVel * inv; | ||
444 | // m_clippedInvContactDotSuspension = inv; | ||
445 | // } | ||
446 | |||
447 | //} | ||
448 | |||
449 | //else // Not in contact : position wheel in a nice (rest length) position | ||
450 | //{ | ||
451 | // m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength(); | ||
452 | // m_suspensionRelativeVelocity = 0.0f; | ||
453 | // m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; | ||
454 | // m_clippedInvContactDotSuspension = 1.0f; | ||
455 | //} | ||
456 | }; | ||
457 | |||
458 | //btScalar m_clippedInvContactDotSuspension; | ||
459 | //btScalar m_suspensionRelativeVelocity; | ||
460 | //btScalar m_wheelsSuspensionForce; | ||
461 | //btScalar m_skidInfo; | ||
462 | |||
463 | //void* m_clientInfo;//can be used to store pointer to sync transforms... | ||
464 | |||
465 | //btWheelInfo(btWheelInfoConstructionInfo& ci) | ||
466 | |||
467 | //{ | ||
468 | |||
469 | // m_suspensionRestLength1 = ci.m_suspensionRestLength; | ||
470 | // m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm; | ||
471 | |||
472 | // m_wheelsRadius = ci.m_wheelRadius; | ||
473 | // m_suspensionStiffness = ci.m_suspensionStiffness; | ||
474 | // m_wheelsDampingCompression = ci.m_wheelsDampingCompression; | ||
475 | // m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation; | ||
476 | // m_chassisConnectionPointCS = ci.m_chassisConnectionCS; | ||
477 | // m_wheelDirectionCS = ci.m_wheelDirectionCS; | ||
478 | // m_wheelAxleCS = ci.m_wheelAxleCS; | ||
479 | // m_frictionSlip = ci.m_frictionSlip; | ||
480 | // m_steering = 0.f; | ||
481 | // m_engineForce = 0.f; | ||
482 | // m_rotation = 0.f; | ||
483 | // m_deltaRotation = 0.f; | ||
484 | // m_brake = 0.f; | ||
485 | // m_rollInfluence = 0.1f; | ||
486 | // m_bIsFrontWheel = ci.m_bIsFrontWheel; | ||
487 | |||
488 | //} | ||
489 | |||
490 | //void updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo); | ||
491 | |||
492 | //btScalar m_clippedInvContactDotSuspension; | ||
493 | //btScalar m_suspensionRelativeVelocity; | ||
494 | ////calculated by suspension | ||
495 | //btScalar m_wheelsSuspensionForce; | ||
496 | //btScalar m_skidInfo; | ||
497 | |||
498 | //}; | ||
499 | |||
500 | //struct RaycastInfo | ||
501 | //{ | ||
502 | // //set by raycaster | ||
503 | // btVector3 m_contactNormalWS;//contactnormal | ||
504 | // btVector3 m_contactPointWS;//raycast hitpoint | ||
505 | // btScalar m_suspensionLength; | ||
506 | // btVector3 m_hardPointWS;//raycast starting point | ||
507 | // btVector3 m_wheelDirectionWS; //direction in worldspace | ||
508 | // btVector3 m_wheelAxleWS; // axle in worldspace | ||
509 | // bool m_isInContact; | ||
510 | // void* m_groundObject; //could be general void* ptr | ||
511 | //}; | ||
512 | |||
513 | //struct btWheelInfoConstructionInfo | ||
514 | //{ | ||
515 | // btVector3 m_chassisConnectionCS; | ||
516 | // btVector3 m_wheelDirectionCS; | ||
517 | // btVector3 m_wheelAxleCS; | ||
518 | // btScalar m_suspensionRestLength; | ||
519 | // btScalar m_maxSuspensionTravelCm; | ||
520 | // btScalar m_wheelRadius; | ||
521 | |||
522 | // float m_suspensionStiffness; | ||
523 | // float m_wheelsDampingCompression; | ||
524 | // float m_wheelsDampingRelaxation; | ||
525 | // float m_frictionSlip; | ||
526 | // bool m_bIsFrontWheel; | ||
527 | |||
528 | //}; | ||
529 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs new file mode 100644 index 0000000..abd4885 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs | |||
@@ -0,0 +1,53 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using System.Runtime.Serialization; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class BulletException : Exception | ||
30 | { | ||
31 | internal BulletException() | ||
32 | : base("Bullet Physics Library has thrown an exception.") | ||
33 | { | ||
34 | } | ||
35 | |||
36 | internal BulletException(string message) | ||
37 | : base(message) | ||
38 | { | ||
39 | } | ||
40 | |||
41 | internal BulletException(string message, Exception innerException) | ||
42 | : base(message, innerException) | ||
43 | { | ||
44 | } | ||
45 | |||
46 | #if !XBOX | ||
47 | internal BulletException(SerializationInfo info, StreamingContext context) | ||
48 | : base(info, context) | ||
49 | { | ||
50 | } | ||
51 | #endif | ||
52 | } | ||
53 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs b/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs new file mode 100644 index 0000000..ef5b5dc --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs | |||
@@ -0,0 +1,60 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public enum DebugDrawModes | ||
30 | { | ||
31 | NoDebug = 0, | ||
32 | DrawWireframe = 1, | ||
33 | DrawAabb = 2, | ||
34 | DrawFeaturesText = 4, | ||
35 | DrawContactPoints = 8, | ||
36 | NoDeactivation = 16, | ||
37 | NoHelpText = 32, | ||
38 | DrawText = 64, | ||
39 | ProfileTimings = 128, | ||
40 | EnableSatComparison = 256, | ||
41 | DisableBulletLcp = 512, | ||
42 | EnableCcd = 1024, | ||
43 | MaxDebugDrawMode | ||
44 | } | ||
45 | |||
46 | public interface IDebugDraw | ||
47 | { | ||
48 | void DrawLine(Vector3 from, Vector3 to, Vector3 color); | ||
49 | |||
50 | void DrawContactPoint( | ||
51 | Vector3 pointOnB, | ||
52 | Vector3 normalOnB, | ||
53 | float distance, | ||
54 | int lifeTime, | ||
55 | Vector3 color | ||
56 | ); | ||
57 | |||
58 | DebugDrawModes DebugMode { get; set; } | ||
59 | } | ||
60 | } \ No newline at end of file | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs new file mode 100644 index 0000000..2a45a20 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs | |||
@@ -0,0 +1,66 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | // DefaultMotionState provides a common implementation to synchronize world transforms with offsets | ||
30 | public class DefaultMotionState : MotionState | ||
31 | { | ||
32 | private Matrix _graphicsWorldTransform; | ||
33 | private Matrix _centerOfMassOffset; | ||
34 | private Matrix _startWorldTransform; | ||
35 | private object _userData; | ||
36 | |||
37 | public DefaultMotionState() | ||
38 | : this(Matrix.Identity, Matrix.Identity) { } | ||
39 | |||
40 | public DefaultMotionState(Matrix startTransform, Matrix centerOfMassOffset) | ||
41 | { | ||
42 | _graphicsWorldTransform = startTransform; | ||
43 | _centerOfMassOffset = centerOfMassOffset; | ||
44 | _startWorldTransform = startTransform; | ||
45 | } | ||
46 | |||
47 | public Matrix GraphicsWorldTransform { get { return _graphicsWorldTransform; } set { _graphicsWorldTransform = value; } } | ||
48 | public Matrix CenterOfMassOffset { get { return _centerOfMassOffset; } set { _centerOfMassOffset = value; } } | ||
49 | public Matrix StartWorldTransform { get { return _startWorldTransform; } set { _startWorldTransform = value; } } | ||
50 | public object UserData { get { return _userData; } set { _userData = value; } } | ||
51 | |||
52 | // synchronizes world transform from user to physics | ||
53 | public override void GetWorldTransform(out Matrix centerOfMassWorldTrans) | ||
54 | { | ||
55 | centerOfMassWorldTrans = MathHelper.InvertMatrix(_centerOfMassOffset) * _graphicsWorldTransform; | ||
56 | } | ||
57 | |||
58 | // synchronizes world transform from physics to user | ||
59 | // Bullet only calls the update of worldtransform for active objects | ||
60 | public override void SetWorldTransform(Matrix centerOfMassWorldTrans) | ||
61 | { | ||
62 | _graphicsWorldTransform = MatrixOperations.Multiply(centerOfMassWorldTrans, _centerOfMassOffset); | ||
63 | _graphicsWorldTransform.Translation = centerOfMassWorldTrans.Translation; | ||
64 | } | ||
65 | } | ||
66 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs new file mode 100644 index 0000000..2e3f258 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs | |||
@@ -0,0 +1,581 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | using MonoXnaCompactMaths; | ||
27 | |||
28 | namespace XnaDevRu.BulletX | ||
29 | { | ||
30 | public static class MathHelper | ||
31 | { | ||
32 | internal const float Sqrt12 = 0.7071067811865475244008443621048490f; | ||
33 | internal const float Infinity = 3.402823466e+38f; | ||
34 | internal const float Epsilon = 1.192092896e-07f; | ||
35 | |||
36 | public static Vector3 MatrixToVector(Matrix m, Vector3 v) | ||
37 | { | ||
38 | return new Vector3( | ||
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, | ||
41 | Vector3.Dot(new Vector3(m.M31, m.M32, m.M33), v) + m.Translation.Z | ||
42 | ); | ||
43 | } | ||
44 | |||
45 | internal static int ClosestAxis(Vector4 v) | ||
46 | { | ||
47 | return MaxAxis(Absolute(v)); | ||
48 | } | ||
49 | |||
50 | internal static Vector4 Absolute(Vector4 v) | ||
51 | { | ||
52 | return new Vector4(Math.Abs(v.X), Math.Abs(v.Y), Math.Abs(v.Z), Math.Abs(v.W)); | ||
53 | } | ||
54 | |||
55 | internal static int MaxAxis(Vector4 v) | ||
56 | { | ||
57 | int maxIndex = -1; | ||
58 | float maxVal = float.MinValue; | ||
59 | if (v.X > maxVal) | ||
60 | { | ||
61 | maxIndex = 0; | ||
62 | maxVal = v.X; | ||
63 | } | ||
64 | if (v.Y > maxVal) | ||
65 | { | ||
66 | maxIndex = 1; | ||
67 | maxVal = v.Y; | ||
68 | } | ||
69 | if (v.Z > maxVal) | ||
70 | { | ||
71 | maxIndex = 2; | ||
72 | maxVal = v.Z; | ||
73 | } | ||
74 | if (v.W > maxVal) | ||
75 | { | ||
76 | maxIndex = 3; | ||
77 | maxVal = v.W; | ||
78 | } | ||
79 | |||
80 | return maxIndex; | ||
81 | } | ||
82 | |||
83 | internal static int MaxAxis(Vector3 v) | ||
84 | { | ||
85 | return v.X < v.Y ? (v.Y < v.Z ? 2 : 1) : (v.X < v.Z ? 2 : 0); | ||
86 | } | ||
87 | |||
88 | // conservative test for overlap between two aabbs | ||
89 | internal static bool TestAabbAgainstAabb2(Vector3 aabbMinA, Vector3 aabbMaxA, Vector3 aabbMinB, Vector3 aabbMaxB) | ||
90 | { | ||
91 | bool overlap = true; | ||
92 | overlap = (aabbMinA.X > aabbMaxB.X || aabbMaxA.X < aabbMinB.X) ? 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; | ||
95 | return overlap; | ||
96 | } | ||
97 | |||
98 | internal static bool TestTriangleAgainstAabb2(Vector3[] vertices, Vector3 aabbMin, Vector3 aabbMax) | ||
99 | { | ||
100 | Vector3 p1 = vertices[0]; | ||
101 | Vector3 p2 = vertices[1]; | ||
102 | Vector3 p3 = vertices[2]; | ||
103 | |||
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; | ||
106 | |||
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; | ||
109 | |||
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; | ||
112 | return true; | ||
113 | } | ||
114 | |||
115 | internal static void SetInterpolate3(Vector3 vA, Vector3 vB, float rt, ref Vector3 interpolated) | ||
116 | { | ||
117 | float s = 1.0f - rt; | ||
118 | interpolated.X = s * vA.X + rt * vB.X; | ||
119 | interpolated.Y = s * vA.Y + rt * vB.Y; | ||
120 | interpolated.Z = s * vA.Z + rt * vB.Z; | ||
121 | } | ||
122 | |||
123 | internal static void PlaneSpace1(Vector3 n, ref Vector3 p, ref Vector3 q) | ||
124 | { | ||
125 | if (Math.Abs(n.Z) > Sqrt12) | ||
126 | { | ||
127 | // choose p in y-z plane | ||
128 | float a = n.Y * n.Y + n.Z * n.Z; | ||
129 | float k = 1f / (float)Math.Sqrt(a); | ||
130 | p.X = 0; | ||
131 | p.Y = -n.Z * k; | ||
132 | p.Z = n.Y * k; | ||
133 | // set q = n x p | ||
134 | q.X = a * k; | ||
135 | q.Y = -n.X * p.Z; | ||
136 | q.Z = n.X * p.Y; | ||
137 | } | ||
138 | else | ||
139 | { | ||
140 | // choose p in x-y plane | ||
141 | float a = n.X * n.X + n.Y * n.Y; | ||
142 | float k = 1f / (float)Math.Sqrt(a); | ||
143 | p.X = -n.Y * k; | ||
144 | p.Y = n.X * k; | ||
145 | p.Z = 0; | ||
146 | // set q = n x p | ||
147 | q.X = -n.Z * p.Y; | ||
148 | q.Y = n.Z * p.X; | ||
149 | q.Z = a * k; | ||
150 | } | ||
151 | } | ||
152 | |||
153 | internal static bool RayAabb(Vector3 rayFrom, | ||
154 | Vector3 rayTo, | ||
155 | Vector3 aabbMin, | ||
156 | Vector3 aabbMax, | ||
157 | float param, Vector3 normal) | ||
158 | { | ||
159 | Vector3 aabbHalfExtent = (aabbMax - aabbMin) * 0.5f; | ||
160 | Vector3 aabbCenter = (aabbMax + aabbMin) * 0.5f; | ||
161 | Vector3 source = rayFrom - aabbCenter; | ||
162 | Vector3 target = rayTo - aabbCenter; | ||
163 | int sourceOutcode = Outcode(source, aabbHalfExtent); | ||
164 | int targetOutcode = Outcode(target, aabbHalfExtent); | ||
165 | if ((sourceOutcode & targetOutcode) == 0x0) | ||
166 | { | ||
167 | float lambda_enter = 0; | ||
168 | float lambda_exit = param; | ||
169 | Vector3 r = target - source; | ||
170 | float normSign = 1; | ||
171 | Vector3 hitNormal = new Vector3(); | ||
172 | int bit = 1; | ||
173 | |||
174 | for (int j = 0; j < 2; j++) | ||
175 | { | ||
176 | { | ||
177 | if ((sourceOutcode & bit) != 0) | ||
178 | { | ||
179 | float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X; | ||
180 | if (lambda_enter <= lambda) | ||
181 | { | ||
182 | lambda_enter = lambda; | ||
183 | hitNormal = new Vector3(); | ||
184 | hitNormal.X = normSign; | ||
185 | } | ||
186 | } | ||
187 | else if ((targetOutcode & bit) != 0) | ||
188 | { | ||
189 | float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X; | ||
190 | SetMin(ref lambda_exit, lambda); | ||
191 | } | ||
192 | bit <<= 1; | ||
193 | } | ||
194 | { | ||
195 | if ((sourceOutcode & bit) != 0) | ||
196 | { | ||
197 | float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y; | ||
198 | if (lambda_enter <= lambda) | ||
199 | { | ||
200 | lambda_enter = lambda; | ||
201 | hitNormal = new Vector3(); | ||
202 | hitNormal.Y = normSign; | ||
203 | } | ||
204 | } | ||
205 | else if ((targetOutcode & bit) != 0) | ||
206 | { | ||
207 | float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y; | ||
208 | SetMin(ref lambda_exit, lambda); | ||
209 | } | ||
210 | bit <<= 1; | ||
211 | } | ||
212 | { | ||
213 | if ((sourceOutcode & bit) != 0) | ||
214 | { | ||
215 | float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z; | ||
216 | if (lambda_enter <= lambda) | ||
217 | { | ||
218 | lambda_enter = lambda; | ||
219 | hitNormal = new Vector3(); | ||
220 | hitNormal.Z = normSign; | ||
221 | } | ||
222 | } | ||
223 | else if ((targetOutcode & bit) != 0) | ||
224 | { | ||
225 | float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z; | ||
226 | SetMin(ref lambda_exit, lambda); | ||
227 | } | ||
228 | bit <<= 1; | ||
229 | } | ||
230 | normSign = -1; | ||
231 | } | ||
232 | if (lambda_enter <= lambda_exit) | ||
233 | { | ||
234 | param = lambda_enter; | ||
235 | normal = hitNormal; | ||
236 | return true; | ||
237 | } | ||
238 | } | ||
239 | return false; | ||
240 | } | ||
241 | |||
242 | internal static void SetMin(ref float a, float b) | ||
243 | { | ||
244 | if (a > b) | ||
245 | a = b; | ||
246 | } | ||
247 | |||
248 | internal static void SetMax(ref float a, float b) | ||
249 | { | ||
250 | if (a < b) | ||
251 | a = b; | ||
252 | } | ||
253 | |||
254 | internal static void SetMax(ref Vector3 self, Vector3 other) | ||
255 | { | ||
256 | if (other.X > self.X) | ||
257 | self.X = other.X; | ||
258 | |||
259 | if (other.Y > self.Y) | ||
260 | self.Y = other.Y; | ||
261 | |||
262 | if (other.Z > self.Z) | ||
263 | self.Z = other.Z; | ||
264 | } | ||
265 | |||
266 | internal static Vector3 SetMax(Vector3 self, Vector3 other) | ||
267 | { | ||
268 | if (other.X > self.X) | ||
269 | self.X = other.X; | ||
270 | |||
271 | if (other.Y > self.Y) | ||
272 | self.Y = other.Y; | ||
273 | |||
274 | if (other.Z > self.Z) | ||
275 | self.Z = other.Z; | ||
276 | |||
277 | return self; | ||
278 | } | ||
279 | |||
280 | internal static void SetMin(ref Vector3 self, Vector3 other) | ||
281 | { | ||
282 | if (other.X < self.X) | ||
283 | self.X = other.X; | ||
284 | |||
285 | if (other.Y < self.Y) | ||
286 | self.Y = other.Y; | ||
287 | |||
288 | if (other.Z < self.Z) | ||
289 | self.Z = other.Z; | ||
290 | } | ||
291 | |||
292 | internal static Vector3 SetMin(Vector3 self, Vector3 other) | ||
293 | { | ||
294 | if (other.X < self.X) | ||
295 | self.X = other.X; | ||
296 | |||
297 | if (other.Y < self.Y) | ||
298 | self.Y = other.Y; | ||
299 | |||
300 | if (other.Z < self.Z) | ||
301 | self.Z = other.Z; | ||
302 | |||
303 | return self; | ||
304 | } | ||
305 | |||
306 | internal static int Outcode(Vector3 p, Vector3 halfExtent) | ||
307 | { | ||
308 | return (p.X < -halfExtent.X ? 0x01 : 0x0) | | ||
309 | (p.X > halfExtent.X ? 0x08 : 0x0) | | ||
310 | (p.Y < -halfExtent.Y ? 0x02 : 0x0) | | ||
311 | (p.Y > halfExtent.Y ? 0x10 : 0x0) | | ||
312 | (p.Z < -halfExtent.Z ? 0x4 : 0x0) | | ||
313 | (p.Z > halfExtent.Z ? 0x20 : 0x0); | ||
314 | } | ||
315 | |||
316 | internal static Matrix Absolute(Matrix m) | ||
317 | { | ||
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), | ||
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)); | ||
322 | } | ||
323 | |||
324 | internal static void SetValueByIndex(ref Vector3 v, int i, float value) | ||
325 | { | ||
326 | if (i == 0) | ||
327 | v.X = value; | ||
328 | else if (i == 1) | ||
329 | v.Y = value; | ||
330 | else | ||
331 | v.Z = value; | ||
332 | } | ||
333 | |||
334 | internal static float GetValueByIndex(Vector3 v, int i) | ||
335 | { | ||
336 | if (i == 0) | ||
337 | return v.X; | ||
338 | else if (i == 1) | ||
339 | return v.Y; | ||
340 | else | ||
341 | return v.Z; | ||
342 | } | ||
343 | |||
344 | internal static Vector3 InvXForm(Matrix m, Vector3 v) | ||
345 | { | ||
346 | v -= m.Translation; | ||
347 | m.Translation = new Vector3(); | ||
348 | return MathHelper.Transform(v, Matrix.Transpose(m)); | ||
349 | } | ||
350 | |||
351 | internal static Matrix InverseTimes(Matrix m, Matrix t) | ||
352 | { | ||
353 | Vector3 v = t.Translation - m.Translation; | ||
354 | |||
355 | Matrix mat = TransposeTimes(m, t); | ||
356 | mat.Translation = Vector3.Transform(v, m); | ||
357 | return mat; | ||
358 | } | ||
359 | |||
360 | internal static Matrix TransposeTimes(Matrix mA, Matrix mB) | ||
361 | { | ||
362 | return new Matrix( | ||
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, | ||
365 | mA.M11 * mB.M13 + mA.M21 * mB.M23 + mA.M31 * mB.M33, | ||
366 | 0, | ||
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, | ||
369 | mA.M12 * mB.M13 + mA.M22 * mB.M23 + mA.M32 * mB.M33, | ||
370 | 0, | ||
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, | ||
373 | mA.M13 * mB.M13 + mA.M23 * mB.M23 + mA.M33 * mB.M33, | ||
374 | 0, 0, 0, 0, 1); | ||
375 | } | ||
376 | |||
377 | internal static Vector3 GetColumn(Matrix m, int column) | ||
378 | { | ||
379 | switch (column) | ||
380 | { | ||
381 | case 1: | ||
382 | return new Vector3(m.M11, m.M21, m.M31); | ||
383 | case 2: | ||
384 | return new Vector3(m.M12, m.M22, m.M32); | ||
385 | case 3: | ||
386 | return new Vector3(m.M13, m.M23, m.M33); | ||
387 | default: | ||
388 | throw new ArgumentOutOfRangeException("column"); | ||
389 | } | ||
390 | } | ||
391 | |||
392 | internal static Vector3 GetRow(Matrix m, int row) | ||
393 | { | ||
394 | switch (row) | ||
395 | { | ||
396 | case 1: | ||
397 | return new Vector3(m.M11, m.M12, m.M13); | ||
398 | case 2: | ||
399 | return new Vector3(m.M21, m.M22, m.M23); | ||
400 | case 3: | ||
401 | return new Vector3(m.M31, m.M32, m.M33); | ||
402 | default: | ||
403 | throw new ArgumentOutOfRangeException("row"); | ||
404 | } | ||
405 | } | ||
406 | |||
407 | internal static Quaternion GetRotation(Matrix m) | ||
408 | { | ||
409 | float trace = m.M11 + m.M22 + m.M33; | ||
410 | Quaternion q = new Quaternion(); | ||
411 | |||
412 | if (trace > 0) | ||
413 | { | ||
414 | float s = (float)Math.Sqrt(trace + 1.0f); | ||
415 | q.W = s * 0.5f; | ||
416 | s = 0.5f / s; | ||
417 | |||
418 | q.X = (m.M32 - m.M23) * s; | ||
419 | q.Y = (m.M13 - m.M31) * s; | ||
420 | q.Z = (m.M21 - m.M12) * s; | ||
421 | } | ||
422 | else | ||
423 | { | ||
424 | int i = m.M11 < m.M22 ? | ||
425 | (m.M22 < m.M33 ? 2 : 1) : | ||
426 | (m.M11 < m.M33 ? 2 : 0); | ||
427 | int j = (i + 1) % 3; | ||
428 | int k = (i + 2) % 3; | ||
429 | |||
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); | ||
432 | s = 0.5f / s; | ||
433 | |||
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); | ||
436 | SetElement(ref q, k, (GetElement(m, k, i) + GetElement(m, i, k)) * s); | ||
437 | } | ||
438 | return q; | ||
439 | } | ||
440 | |||
441 | internal static float SetElement(ref Quaternion q, int index, float value) | ||
442 | { | ||
443 | switch (index) | ||
444 | { | ||
445 | case 0: | ||
446 | q.X = value; break; | ||
447 | case 1: | ||
448 | q.Y = value; break; | ||
449 | case 2: | ||
450 | q.Z = value; break; | ||
451 | case 3: | ||
452 | q.W = value; break; | ||
453 | } | ||
454 | |||
455 | return 0; | ||
456 | } | ||
457 | |||
458 | internal static float GetElement(Quaternion q, int index) | ||
459 | { | ||
460 | switch (index) | ||
461 | { | ||
462 | case 0: | ||
463 | return q.X; | ||
464 | case 1: | ||
465 | return q.Y; | ||
466 | case 2: | ||
467 | return q.Z; | ||
468 | default: | ||
469 | return q.W; | ||
470 | } | ||
471 | } | ||
472 | |||
473 | internal static float GetElement(Matrix mat, int index) | ||
474 | { | ||
475 | int row = index % 3; | ||
476 | int col = index / 3; | ||
477 | |||
478 | return GetElement(mat, row, col); | ||
479 | } | ||
480 | |||
481 | internal static float GetElement(Matrix mat, int row, int col) | ||
482 | { | ||
483 | switch (row) | ||
484 | { | ||
485 | case 0: | ||
486 | switch (col) | ||
487 | { | ||
488 | case 0: | ||
489 | return mat.M11; | ||
490 | case 1: | ||
491 | return mat.M12; | ||
492 | case 2: | ||
493 | return mat.M13; | ||
494 | } break; | ||
495 | case 1: | ||
496 | switch (col) | ||
497 | { | ||
498 | case 0: | ||
499 | return mat.M21; | ||
500 | case 1: | ||
501 | return mat.M22; | ||
502 | case 2: | ||
503 | return mat.M23; | ||
504 | } break; | ||
505 | case 2: | ||
506 | switch (col) | ||
507 | { | ||
508 | case 0: | ||
509 | return mat.M31; | ||
510 | case 1: | ||
511 | return mat.M32; | ||
512 | case 2: | ||
513 | return mat.M33; | ||
514 | } break; | ||
515 | } | ||
516 | |||
517 | return 0; | ||
518 | } | ||
519 | |||
520 | internal static float GetElement(Vector3 v, int index) | ||
521 | { | ||
522 | if (index == 0) | ||
523 | return v.X; | ||
524 | if (index == 1) | ||
525 | return v.Y; | ||
526 | if (index == 2) | ||
527 | return v.Z; | ||
528 | |||
529 | throw new ArgumentOutOfRangeException("index"); | ||
530 | } | ||
531 | |||
532 | internal static void SetElement(ref Vector3 v, int index, float value) | ||
533 | { | ||
534 | if (index == 0) | ||
535 | v.X = value; | ||
536 | else if (index == 1) | ||
537 | v.Y = value; | ||
538 | else if (index == 2) | ||
539 | v.Z = value; | ||
540 | else | ||
541 | throw new ArgumentOutOfRangeException("index"); | ||
542 | } | ||
543 | |||
544 | public static Matrix InvertMatrix(Matrix m) | ||
545 | { | ||
546 | Vector3 pos = m.Translation; | ||
547 | m.Translation = Vector3.Zero; | ||
548 | Matrix inv = Matrix.Transpose(m); | ||
549 | pos = Vector3.Transform(-pos, m); | ||
550 | inv.Translation = pos; | ||
551 | return inv; | ||
552 | } | ||
553 | |||
554 | public static Matrix GetDisplayMatrix(Matrix m) | ||
555 | { | ||
556 | Matrix displayMatrix = m; | ||
557 | displayMatrix.Translation = Vector3.Zero; | ||
558 | displayMatrix = Matrix.Transpose(displayMatrix); | ||
559 | displayMatrix.Translation = m.Translation; | ||
560 | return displayMatrix; | ||
561 | } | ||
562 | |||
563 | internal static Vector3 Transform(Vector3 position, Matrix matrix) | ||
564 | { | ||
565 | Vector3 vector = new Vector3(); | ||
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; | ||
568 | vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)) + matrix.M43; | ||
569 | return vector; | ||
570 | } | ||
571 | |||
572 | internal static Vector3 TransformNormal(Vector3 position, Matrix matrix) | ||
573 | { | ||
574 | Vector3 vector = new Vector3(); | ||
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)); | ||
577 | vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)); | ||
578 | return vector; | ||
579 | } | ||
580 | } | ||
581 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs new file mode 100644 index 0000000..04e42d9 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs | |||
@@ -0,0 +1,116 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | internal static class MatrixOperations | ||
30 | { | ||
31 | public static void SetRotation(ref Matrix m, Quaternion q) | ||
32 | { | ||
33 | float d = q.LengthSquared(); | ||
34 | BulletDebug.Assert(d != 0); | ||
35 | float s = 2f / d; | ||
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; | ||
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; | ||
40 | m = new Matrix(1 - (yy + zz), xy - wz, xz + wy, 0, | ||
41 | xy + wz, 1 - (xx + zz), yz - wx, 0, | ||
42 | xz - wy, yz + wx, 1 - (xx + yy), 0, | ||
43 | m.M41, m.M42, m.M43, 1); | ||
44 | } | ||
45 | |||
46 | public static Quaternion GetRotation(Matrix m) | ||
47 | { | ||
48 | Quaternion q = new Quaternion(); | ||
49 | |||
50 | float trace = m.M11 + m.M22 + m.M33; | ||
51 | |||
52 | if (trace > 0) | ||
53 | { | ||
54 | float s = (float)Math.Sqrt(trace + 1); | ||
55 | q.W = s * 0.5f; | ||
56 | s = 0.5f / s; | ||
57 | |||
58 | q.X = (m.M32 - m.M23) * s; | ||
59 | q.Y = (m.M13 - m.M31) * s; | ||
60 | q.Z = (m.M21 - m.M12) * s; | ||
61 | } | ||
62 | else | ||
63 | { | ||
64 | int i = m.M11 < m.M22 ? | ||
65 | (m.M22 < m.M33 ? 2 : 1) : | ||
66 | (m.M11 < m.M33 ? 2 : 0); | ||
67 | int j = (i + 1) % 3; | ||
68 | int k = (i + 2) % 3; | ||
69 | |||
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); | ||
72 | s = 0.5f / s; | ||
73 | |||
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); | ||
76 | MathHelper.SetElement(ref q, k, (MathHelper.GetElement(m, k, i) + MathHelper.GetElement(m, i, k)) * s); | ||
77 | } | ||
78 | |||
79 | return q; | ||
80 | } | ||
81 | |||
82 | public static Matrix Scaled(Matrix m, Vector3 v) | ||
83 | { | ||
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, | ||
86 | m.M31 * v.X, m.M32 * v.Y, m.M33 * v.Z, 0, | ||
87 | 0, 0, 0, 1); | ||
88 | } | ||
89 | |||
90 | public static Matrix Multiply(Matrix a, Matrix b) | ||
91 | { | ||
92 | /*return btMatrix3x3( | ||
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]), | ||
95 | m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));*/ | ||
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, | ||
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, | ||
100 | 0, 0, 0, 1); | ||
101 | } | ||
102 | |||
103 | public static float Dot(Matrix m, int c, Vector3 v) | ||
104 | { | ||
105 | return MathHelper.GetElement(m, 0, c) * v.X + MathHelper.GetElement(m, 1, c) * v.Y + MathHelper.GetElement(m, 2, c) * v.Z; | ||
106 | } | ||
107 | |||
108 | public static Matrix Transpose(Matrix m) | ||
109 | { | ||
110 | return new Matrix( m.M11, m.M21, m.M31, 0, | ||
111 | m.M12, m.M22, m.M32, 0, | ||
112 | m.M13, m.M23, m.M33, 0, | ||
113 | 0, 0, 0, 1); | ||
114 | } | ||
115 | } | ||
116 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs new file mode 100644 index 0000000..4039177 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs | |||
@@ -0,0 +1,44 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public abstract class MotionState | ||
30 | { | ||
31 | /*public abstract Vector3 getWorldPosition(); | ||
32 | public abstract Vector3 getWorldScaling(); | ||
33 | public abstract Quaternion getWorldOrientation(); | ||
34 | |||
35 | public abstract void setWorldPosition(Vector3 worldPos); | ||
36 | public abstract void setWorldOrientation(Quaternion orn); | ||
37 | |||
38 | public abstract void calculateWorldTransformations();*/ | ||
39 | |||
40 | public abstract void GetWorldTransform(out Matrix worldTransform); | ||
41 | //Bullet only calls the update of worldtransform for active objects | ||
42 | public abstract void SetWorldTransform(Matrix worldTransform); | ||
43 | } | ||
44 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs new file mode 100644 index 0000000..35c9fed --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs | |||
@@ -0,0 +1,88 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX.LinearMath | ||
27 | { | ||
28 | internal abstract class QuadWord | ||
29 | { | ||
30 | private float x; | ||
31 | private float y; | ||
32 | private float z; | ||
33 | private float w; | ||
34 | |||
35 | public float X { get { return x; } set { x = value; } } | ||
36 | public float Y { get { return y; } set { y = value; } } | ||
37 | public float Z { get { return z; } set { z = value; } } | ||
38 | public float W { get { return w; } set { w = value; } } | ||
39 | |||
40 | public QuadWord() { } | ||
41 | |||
42 | public QuadWord(float x, float y, float z, float w) | ||
43 | { | ||
44 | X = x; | ||
45 | Y = y; | ||
46 | Z = z; | ||
47 | W = w; | ||
48 | } | ||
49 | |||
50 | public QuadWord(float x, float y, float z) | ||
51 | { | ||
52 | X = x; | ||
53 | Y = y; | ||
54 | Z = z; | ||
55 | W = 0; | ||
56 | } | ||
57 | |||
58 | public void SetMax(QuadWord other) | ||
59 | { | ||
60 | if (other.X > X) | ||
61 | X = other.X; | ||
62 | |||
63 | if (other.Y > Y) | ||
64 | Y = other.Y; | ||
65 | |||
66 | if (other.Z > Z) | ||
67 | Z = other.Z; | ||
68 | |||
69 | if (other.W > W) | ||
70 | W = other.W; | ||
71 | } | ||
72 | |||
73 | public void SetMin(QuadWord other) | ||
74 | { | ||
75 | if (other.X < X) | ||
76 | X = other.X; | ||
77 | |||
78 | if (other.Y < Y) | ||
79 | Y = other.Y; | ||
80 | |||
81 | if (other.Z < Z) | ||
82 | Z = other.Z; | ||
83 | |||
84 | if (other.W < W) | ||
85 | W = other.W; | ||
86 | } | ||
87 | } | ||
88 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs new file mode 100644 index 0000000..5425a05 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs | |||
@@ -0,0 +1,198 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX.LinearMath | ||
27 | { | ||
28 | internal class Quaternion : QuadWord | ||
29 | { | ||
30 | public Quaternion() { } | ||
31 | |||
32 | public Quaternion(float x, float y, float z, float w) | ||
33 | : base(x, y, z, w) { } | ||
34 | |||
35 | public Quaternion(Vector3 axis, float angle) | ||
36 | { | ||
37 | SetRotation(axis, angle); | ||
38 | } | ||
39 | |||
40 | public Quaternion(float yaw, float pitch, float roll) | ||
41 | { | ||
42 | SetEuler(yaw, pitch, roll); | ||
43 | } | ||
44 | |||
45 | public void SetRotation(Vector3 axis, float angle) | ||
46 | { | ||
47 | float d = axis.Length(); | ||
48 | if (d == 0) throw new DivideByZeroException(); | ||
49 | float s = (float)Math.Sin(angle * 0.5f) / d; | ||
50 | X = axis.X * s; | ||
51 | Y = axis.Y * s; | ||
52 | Z = axis.Z * s; | ||
53 | W = (float)Math.Cos(angle * 0.5f); | ||
54 | } | ||
55 | |||
56 | public void SetEuler(float yaw, float pitch, float roll) | ||
57 | { | ||
58 | float halfYaw = yaw * 0.5f; | ||
59 | float halfPitch = pitch * 0.5f; | ||
60 | float halfRoll = roll * 0.5f; | ||
61 | float cosYaw = (float)Math.Cos(halfYaw); | ||
62 | float sinYaw = (float)Math.Sin(halfYaw); | ||
63 | float cosPitch = (float)Math.Cos(halfPitch); | ||
64 | float sinPitch = (float)Math.Sin(halfPitch); | ||
65 | float cosRoll = (float)Math.Cos(halfRoll); | ||
66 | float sinRoll = (float)Math.Sin(halfRoll); | ||
67 | X = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; | ||
68 | Y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw; | ||
69 | Z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw; | ||
70 | W = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw; | ||
71 | } | ||
72 | |||
73 | public float LengthSquared() | ||
74 | { | ||
75 | return Dot(this, this); | ||
76 | } | ||
77 | |||
78 | public float Length() | ||
79 | { | ||
80 | return (float)Math.Sqrt(LengthSquared()); | ||
81 | } | ||
82 | |||
83 | public float Angle() | ||
84 | { | ||
85 | return 2f * (float)Math.Acos(W); | ||
86 | } | ||
87 | |||
88 | public static float Angle(Quaternion a, Quaternion b) | ||
89 | { | ||
90 | float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared()); | ||
91 | if (s == 0) throw new DivideByZeroException(); | ||
92 | return (float)Math.Acos(Dot(a, b) / s); | ||
93 | } | ||
94 | |||
95 | public static Quaternion Farthest(Quaternion a, Quaternion b) | ||
96 | { | ||
97 | Quaternion diff, sum; | ||
98 | diff = a - b; | ||
99 | sum = a + b; | ||
100 | if (Dot(diff, diff) > Dot(sum, sum)) | ||
101 | return b; | ||
102 | return -b; | ||
103 | } | ||
104 | |||
105 | public static Quaternion Slerp(Quaternion a, Quaternion b, float c) | ||
106 | { | ||
107 | float theta = Angle(a, b); | ||
108 | if (theta != 0) | ||
109 | { | ||
110 | float d = 1f / (float)Math.Sin(theta); | ||
111 | float s0 = (float)Math.Sin((1f - c) * theta); | ||
112 | float s1 = (float)Math.Sin(c * theta); | ||
113 | return new Quaternion( | ||
114 | (a.X * s0 + b.X * s1) * d, | ||
115 | (a.Y * s0 + b.Y * s1) * d, | ||
116 | (a.Z * s0 + b.Z * s1) * d, | ||
117 | (a.W * s0 + b.W * s1) * d); | ||
118 | } | ||
119 | else | ||
120 | { | ||
121 | return a; | ||
122 | } | ||
123 | } | ||
124 | |||
125 | public static Quaternion Inverse(Quaternion a) | ||
126 | { | ||
127 | return new Quaternion(a.X, a.Y, a.Z, -a.W); | ||
128 | } | ||
129 | |||
130 | public static Quaternion Normalize(Quaternion a) | ||
131 | { | ||
132 | return a / a.Length(); | ||
133 | } | ||
134 | |||
135 | public static float Dot(Quaternion a, Quaternion b) | ||
136 | { | ||
137 | return a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W; | ||
138 | } | ||
139 | |||
140 | public static Quaternion operator +(Quaternion a, Quaternion b) | ||
141 | { | ||
142 | return new Quaternion(a.X + b.X, a.Y + b.Y, a.Z + b.Z, a.W + b.W); | ||
143 | } | ||
144 | |||
145 | public static Quaternion operator -(Quaternion a, Quaternion b) | ||
146 | { | ||
147 | return new Quaternion(a.X - b.X, a.Y - b.Y, a.Z - b.Z, a.W - b.W); | ||
148 | } | ||
149 | |||
150 | public static Quaternion operator -(Quaternion a) | ||
151 | { | ||
152 | return new Quaternion(-a.X, -a.Y, -a.Z, -a.W); | ||
153 | } | ||
154 | |||
155 | public static Quaternion operator *(Quaternion a, float b) | ||
156 | { | ||
157 | return new Quaternion(a.X * b, a.Y * b, a.Z * b, a.W * b); | ||
158 | } | ||
159 | |||
160 | public static Quaternion operator *(Quaternion a, Quaternion b) | ||
161 | { | ||
162 | return new Quaternion( | ||
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, | ||
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); | ||
167 | } | ||
168 | |||
169 | public static Quaternion operator *(Quaternion a, Vector3 b) | ||
170 | { | ||
171 | return new Quaternion( | ||
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, | ||
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); | ||
176 | } | ||
177 | |||
178 | public static Quaternion operator *(Vector3 w, Quaternion q) | ||
179 | { | ||
180 | return new Quaternion( | ||
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, | ||
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); | ||
185 | } | ||
186 | |||
187 | public static Quaternion operator /(Quaternion a, float b) | ||
188 | { | ||
189 | if (b == 0) throw new DivideByZeroException(); | ||
190 | return new Quaternion(a.X / b, a.Y / b, a.Z / b, a.W / b); | ||
191 | } | ||
192 | |||
193 | public static explicit operator MonoXnaCompactMaths.Quaternion(Quaternion a) | ||
194 | { | ||
195 | return new MonoXnaCompactMaths.Quaternion(a.X, a.Y, a.Z, a.W); | ||
196 | } | ||
197 | } | ||
198 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs new file mode 100644 index 0000000..031faab --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs | |||
@@ -0,0 +1,102 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public static class TransformUtil | ||
30 | { | ||
31 | //const float AngularMotionTreshold = 0.5f * MonoXnaCompactMaths.MathHelper.PiOver2; | ||
32 | const float AngularMotionTreshold = 0.5f * (float)Math.PI / 2.0f; | ||
33 | |||
34 | public static void IntegrateTransform(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, ref Matrix predictedTransform) | ||
35 | { | ||
36 | predictedTransform.Translation = currentTransform.Translation + linearVelocity * timeStep; | ||
37 | //exponential map | ||
38 | Vector3 axis; | ||
39 | float angle = angularVelocity.Length(); | ||
40 | //limit the angular motion | ||
41 | if (angle * timeStep > AngularMotionTreshold) | ||
42 | { | ||
43 | angle = AngularMotionTreshold / timeStep; | ||
44 | } | ||
45 | |||
46 | if (angle < 0.001f) | ||
47 | { | ||
48 | // use Taylor's expansions of sync function | ||
49 | axis = angularVelocity * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * angle * angle); | ||
50 | } | ||
51 | else | ||
52 | { | ||
53 | // sync(fAngle) = sin(c*fAngle)/t | ||
54 | axis = angularVelocity * ((float)Math.Sin(0.5f * angle * timeStep) / angle); | ||
55 | } | ||
56 | Quaternion dorn = new Quaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * timeStep * 0.5f)); | ||
57 | Quaternion ornA = MatrixOperations.GetRotation(currentTransform); | ||
58 | |||
59 | Quaternion predictedOrn = dorn * ornA; | ||
60 | predictedOrn.Normalize(); | ||
61 | |||
62 | MatrixOperations.SetRotation(ref predictedTransform, predictedOrn); | ||
63 | |||
64 | Matrix test = Matrix.CreateFromQuaternion(predictedOrn); | ||
65 | } | ||
66 | |||
67 | public static void CalculateVelocity(Matrix transformA, Matrix transformB, float timeStep, ref Vector3 linearVelocity, ref Vector3 angularVelocity) | ||
68 | { | ||
69 | linearVelocity = (transformB.Translation - transformA.Translation) / timeStep; | ||
70 | Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); | ||
71 | Quaternion dorn = Quaternion.CreateFromRotationMatrix(dmat); | ||
72 | |||
73 | Vector3 axis; | ||
74 | float angle = 2 * (float)Math.Acos(dorn.W); | ||
75 | axis = new Vector3(dorn.X, dorn.Y, dorn.Z); | ||
76 | //axis[3] = 0.f; | ||
77 | //check for axis length | ||
78 | float len = axis.LengthSquared(); | ||
79 | if (len < MathHelper.Epsilon * MathHelper.Epsilon) | ||
80 | axis = new Vector3(1f, 0f, 0f); | ||
81 | else | ||
82 | axis /= (float)Math.Sqrt(len); | ||
83 | |||
84 | angularVelocity = axis * angle / timeStep; | ||
85 | } | ||
86 | |||
87 | public static void CalculateDiffAxisAngle(Matrix transformA, Matrix transformB, out Vector3 axis, out float angle) | ||
88 | { | ||
89 | Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); | ||
90 | Quaternion dorn = MathHelper.GetRotation(dmat); | ||
91 | |||
92 | angle = 2f * (float)Math.Acos(dorn.W); | ||
93 | axis = new Vector3(dorn.X, dorn.Y, dorn.Z); | ||
94 | //check for axis length | ||
95 | float len = axis.LengthSquared(); | ||
96 | if (len < MathHelper.Epsilon * MathHelper.Epsilon) | ||
97 | axis = new Vector3(1f, 0f, 0f); | ||
98 | else | ||
99 | axis /= (float)Math.Sqrt(len); | ||
100 | } | ||
101 | } | ||
102 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs new file mode 100644 index 0000000..79b1262 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs | |||
@@ -0,0 +1,221 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX.LinearMath | ||
27 | { | ||
28 | internal class Vector3 : QuadWord | ||
29 | { | ||
30 | public Vector3() { } | ||
31 | |||
32 | public Vector3(float x, float y, float z) | ||
33 | : base(x, y, z) { } | ||
34 | |||
35 | public void SetInterpolate3(Vector3 a, Vector3 b, float c) | ||
36 | { | ||
37 | float s = 1.0f - c; | ||
38 | X = s * a.X + c * b.X; | ||
39 | Y = s * a.Y + c * b.Y; | ||
40 | Z = s * a.Z + c * b.Z; | ||
41 | } | ||
42 | |||
43 | public float LengthSquared() | ||
44 | { | ||
45 | return Dot(this, this); | ||
46 | } | ||
47 | |||
48 | public float Length() | ||
49 | { | ||
50 | return (float)Math.Sqrt(LengthSquared()); | ||
51 | } | ||
52 | |||
53 | public int MinAxis() | ||
54 | { | ||
55 | return X < Y ? (X < Z ? 0 : 2) : (Y < Z ? 1 : 2); | ||
56 | } | ||
57 | |||
58 | public int MaxAxis() | ||
59 | { | ||
60 | return X < Y ? (Y < Z ? 2 : 1) : (X < Z ? 2 : 0); | ||
61 | } | ||
62 | |||
63 | public int FurthestAxis() | ||
64 | { | ||
65 | return Absolute(this).MinAxis(); | ||
66 | } | ||
67 | |||
68 | public int ClosestAxis() | ||
69 | { | ||
70 | return Absolute(this).MaxAxis(); | ||
71 | } | ||
72 | |||
73 | public Vector3 Rotate(Vector3 axis, float angle) | ||
74 | { | ||
75 | Vector3 o = axis * Dot(axis, this); | ||
76 | Vector3 x = this - o; | ||
77 | Vector3 y = Cross(axis, this); | ||
78 | |||
79 | return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle)); | ||
80 | } | ||
81 | |||
82 | public static Vector3 Lerp(Vector3 a, Vector3 b, float c) | ||
83 | { | ||
84 | return new Vector3( | ||
85 | a.X + (b.X - a.X) * c, | ||
86 | a.Y + (b.Y - a.Y) * c, | ||
87 | a.Z + (b.Z - a.Z) * c); | ||
88 | } | ||
89 | |||
90 | public static float Angle(Vector3 a, Vector3 b) | ||
91 | { | ||
92 | float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared()); | ||
93 | if (s == 0) throw new DivideByZeroException(); | ||
94 | return (float)Math.Acos(Dot(a, b) / s); | ||
95 | } | ||
96 | |||
97 | public static Vector3 Absolute(Vector3 a) | ||
98 | { | ||
99 | return new Vector3( | ||
100 | Math.Abs(a.X), | ||
101 | Math.Abs(a.Y), | ||
102 | Math.Abs(a.Z)); | ||
103 | } | ||
104 | |||
105 | public static Vector3 Normalize(Vector3 a) | ||
106 | { | ||
107 | return a / a.Length(); | ||
108 | } | ||
109 | |||
110 | public static Vector3 Cross(Vector3 a, Vector3 b) | ||
111 | { | ||
112 | return new Vector3( | ||
113 | a.Y * b.Z - a.Z * b.Y, | ||
114 | a.Z * b.X - a.X * b.Z, | ||
115 | a.X * b.Y - a.Y * b.X); | ||
116 | } | ||
117 | |||
118 | public static float Dot(Vector3 a, Vector3 b) | ||
119 | { | ||
120 | return a.X * b.X + a.Y * b.Y + a.Z * b.Z; | ||
121 | } | ||
122 | |||
123 | public static float Triple(Vector3 a, Vector3 b, Vector3 c) | ||
124 | { | ||
125 | return a.X * (b.Y * c.Z - b.Z * c.Y) + | ||
126 | a.Y * (b.Z * c.X - b.X * c.Z) + | ||
127 | a.Z * (b.X * c.Y - b.Y * c.X); | ||
128 | } | ||
129 | |||
130 | public static float Distance(Vector3 a, Vector3 b) | ||
131 | { | ||
132 | return (b - a).Length(); | ||
133 | } | ||
134 | |||
135 | public static float DistanceSquared(Vector3 a, Vector3 b) | ||
136 | { | ||
137 | return (b - a).LengthSquared(); | ||
138 | } | ||
139 | |||
140 | public static Vector3 Rotate(Vector3 a, Vector3 axis, float angle) | ||
141 | { | ||
142 | Vector3 o = axis * Dot(axis, a); | ||
143 | Vector3 x = a - o; | ||
144 | Vector3 y = Cross(axis, a); | ||
145 | |||
146 | return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle)); | ||
147 | } | ||
148 | |||
149 | public static Vector3 operator +(Vector3 a, Vector3 b) | ||
150 | { | ||
151 | return new Vector3(a.X + b.X, a.Y + b.Y, a.Z + b.Z); | ||
152 | } | ||
153 | |||
154 | public static Vector3 operator -(Vector3 a, Vector3 b) | ||
155 | { | ||
156 | return new Vector3(a.X - b.X, a.Y - b.Y, a.Z - b.Z); | ||
157 | } | ||
158 | |||
159 | public static Vector3 operator -(Vector3 a) | ||
160 | { | ||
161 | return new Vector3(-a.X, -a.Y, -a.Z); | ||
162 | } | ||
163 | |||
164 | public static Vector3 operator *(float b, Vector3 a) | ||
165 | { | ||
166 | return new Vector3(a.X * b, a.Y * b, a.Z * b); | ||
167 | } | ||
168 | |||
169 | public static Vector3 operator *(Vector3 a, float b) | ||
170 | { | ||
171 | return new Vector3(a.X * b, a.Y * b, a.Z * b); | ||
172 | } | ||
173 | |||
174 | public static Vector3 operator *(Vector3 a, Vector3 b) | ||
175 | { | ||
176 | return new Vector3(a.X * b.X, a.Y * b.Y, a.Z * b.Z); | ||
177 | } | ||
178 | |||
179 | public static Vector3 operator /(Vector3 a, float b) | ||
180 | { | ||
181 | if (b == 0) throw new DivideByZeroException(); | ||
182 | return new Vector3(a.X / b, a.Y / b, a.Z / b); | ||
183 | } | ||
184 | |||
185 | public static Vector3 operator /(Vector3 a, Vector3 b) | ||
186 | { | ||
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); | ||
189 | } | ||
190 | |||
191 | public static bool operator ==(Vector3 a, Vector3 b) | ||
192 | { | ||
193 | return a.X == b.X && a.Y == b.Y && a.Z == b.Z; | ||
194 | } | ||
195 | |||
196 | public static bool operator !=(Vector3 a, Vector3 b) | ||
197 | { | ||
198 | return a.X != b.X || a.Y != b.Y || a.Z != b.Z; | ||
199 | } | ||
200 | |||
201 | public static explicit operator MonoXnaCompactMaths.Vector3(Vector3 a) | ||
202 | { | ||
203 | return new MonoXnaCompactMaths.Vector3(a.X, a.Y, a.Z); | ||
204 | } | ||
205 | |||
206 | public override bool Equals(object obj) | ||
207 | { | ||
208 | return object.Equals(this, obj); | ||
209 | } | ||
210 | |||
211 | public override int GetHashCode() | ||
212 | { | ||
213 | return X.GetHashCode() & Y.GetHashCode() & Z.GetHashCode(); | ||
214 | } | ||
215 | |||
216 | public override string ToString() | ||
217 | { | ||
218 | return string.Format("{0}, {1}, {2}", X, Y, Z); | ||
219 | } | ||
220 | } | ||
221 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs new file mode 100644 index 0000000..a64ce97 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs | |||
@@ -0,0 +1,110 @@ | |||
1 | /* | ||
2 | Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru | ||
3 | Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com | ||
4 | |||
5 | This software is provided 'as-is', without any express or implied | ||
6 | warranty. In no event will the authors be held liable for any damages | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
13 | 1. The origin of this software must not be misrepresented; you must not | ||
14 | claim that you wrote the original software. If you use this software | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX.LinearMath | ||
27 | { | ||
28 | internal class Vector4 : Vector3 | ||
29 | { | ||
30 | public Vector4() { } | ||
31 | |||
32 | public Vector4(float x, float y, float z, float w) | ||
33 | : base(x, y, z) { W = w; } | ||
34 | |||
35 | public static Vector4 Absolute4(Vector4 a) | ||
36 | { | ||
37 | return new Vector4( | ||
38 | Math.Abs(a.X), | ||
39 | Math.Abs(a.Y), | ||
40 | Math.Abs(a.Z), | ||
41 | Math.Abs(a.W)); | ||
42 | } | ||
43 | |||
44 | public int MaxAxis4() | ||
45 | { | ||
46 | int maxIndex = -1; | ||
47 | float maxVal = -1e30f; | ||
48 | if (X > maxVal) | ||
49 | { | ||
50 | maxIndex = 0; | ||
51 | maxVal = X; | ||
52 | } | ||
53 | if (Y > maxVal) | ||
54 | { | ||
55 | maxIndex = 1; | ||
56 | maxVal = Y; | ||
57 | } | ||
58 | if (Z > maxVal) | ||
59 | { | ||
60 | maxIndex = 2; | ||
61 | maxVal = Z; | ||
62 | } | ||
63 | if (W > maxVal) | ||
64 | { | ||
65 | maxIndex = 3; | ||
66 | maxVal = W; | ||
67 | } | ||
68 | |||
69 | return maxIndex; | ||
70 | } | ||
71 | |||
72 | public int MinAxis4() | ||
73 | { | ||
74 | int minIndex = -1; | ||
75 | float minVal = 1e30f; | ||
76 | if (X < minVal) | ||
77 | { | ||
78 | minIndex = 0; | ||
79 | minVal = X; | ||
80 | } | ||
81 | if (Y < minVal) | ||
82 | { | ||
83 | minIndex = 1; | ||
84 | minVal = Y; | ||
85 | } | ||
86 | if (Z < minVal) | ||
87 | { | ||
88 | minIndex = 2; | ||
89 | minVal = Z; | ||
90 | } | ||
91 | if (W < minVal) | ||
92 | { | ||
93 | minIndex = 3; | ||
94 | minVal = W; | ||
95 | } | ||
96 | |||
97 | return minIndex; | ||
98 | } | ||
99 | |||
100 | public int ClosestAxis4() | ||
101 | { | ||
102 | return Absolute4(this).MaxAxis4(); | ||
103 | } | ||
104 | |||
105 | public static explicit operator MonoXnaCompactMaths.Vector4(Vector4 a) | ||
106 | { | ||
107 | return new MonoXnaCompactMaths.Vector4(a.X, a.Y, a.Z, a.W); | ||
108 | } | ||
109 | } | ||
110 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj new file mode 100644 index 0000000..bed8d19 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj | |||
@@ -0,0 +1,168 @@ | |||
1 | <Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> | ||
2 | <PropertyGroup> | ||
3 | <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration> | ||
4 | <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform> | ||
5 | <ProductVersion>8.0.50727</ProductVersion> | ||
6 | <SchemaVersion>2.0</SchemaVersion> | ||
7 | <ProjectGuid>{44270344-ACA7-4875-B585-81D5C06D0489}</ProjectGuid> | ||
8 | <OutputType>Library</OutputType> | ||
9 | <AppDesignerFolder>Properties</AppDesignerFolder> | ||
10 | <RootNamespace>XnaDevRu.BulletX</RootNamespace> | ||
11 | <AssemblyName>Modified.XnaDevRu.BulletX</AssemblyName> | ||
12 | </PropertyGroup> | ||
13 | <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' "> | ||
14 | <DebugSymbols>true</DebugSymbols> | ||
15 | <DebugType>full</DebugType> | ||
16 | <Optimize>false</Optimize> | ||
17 | <OutputPath>bin\Debug\</OutputPath> | ||
18 | <DefineConstants>DEBUG;TRACE</DefineConstants> | ||
19 | <ErrorReport>prompt</ErrorReport> | ||
20 | <WarningLevel>4</WarningLevel> | ||
21 | </PropertyGroup> | ||
22 | <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' "> | ||
23 | <DebugType>pdbonly</DebugType> | ||
24 | <Optimize>true</Optimize> | ||
25 | <OutputPath>bin\Release\</OutputPath> | ||
26 | <DefineConstants>TRACE</DefineConstants> | ||
27 | <ErrorReport>prompt</ErrorReport> | ||
28 | <WarningLevel>4</WarningLevel> | ||
29 | </PropertyGroup> | ||
30 | <ItemGroup> | ||
31 | <Reference Include="System" /> | ||
32 | </ItemGroup> | ||
33 | <ItemGroup> | ||
34 | <Compile Include="BulletDebug.cs" /> | ||
35 | <Compile Include="Collision\BroadphaseCollision\AxisSweep3.cs" /> | ||
36 | <Compile Include="Collision\BroadphaseCollision\BroadphaseNativeTypes.cs" /> | ||
37 | <Compile Include="Collision\BroadphaseCollision\BroadphasePair.cs" /> | ||
38 | <Compile Include="Collision\BroadphaseCollision\BroadphaseProxy.cs" /> | ||
39 | <Compile Include="Collision\BroadphaseCollision\CollisionAlgorithm.cs" /> | ||
40 | <Compile Include="Collision\BroadphaseCollision\CollisionAlgorithmConstructionInfo.cs" /> | ||
41 | <Compile Include="Collision\BroadphaseCollision\DispatcherInfo.cs" /> | ||
42 | <Compile Include="Collision\BroadphaseCollision\IBroadphase.cs" /> | ||
43 | <Compile Include="Collision\BroadphaseCollision\IDispatcher.cs" /> | ||
44 | <Compile Include="Collision\BroadphaseCollision\IOverlapCallback.cs" /> | ||
45 | <Compile Include="Collision\BroadphaseCollision\OverlappingPairCache.cs" /> | ||
46 | <Compile Include="Collision\BroadphaseCollision\SimpleBroadphase.cs" /> | ||
47 | <Compile Include="Collision\BroadphaseCollision\SimpleBroadphaseProxy.cs" /> | ||
48 | <Compile Include="Collision\CollisionDispatch\BridgeTriangleRaycastCallback.cs" /> | ||
49 | <Compile Include="Collision\CollisionDispatch\CollisionAlgorithmCreateFunc.cs" /> | ||
50 | <Compile Include="Collision\CollisionDispatch\CollisionDispatcher.cs" /> | ||
51 | <Compile Include="Collision\CollisionDispatch\CollisionObject.cs" /> | ||
52 | <Compile Include="Collision\CollisionDispatch\CollisionPairCallback.cs" /> | ||
53 | <Compile Include="Collision\CollisionDispatch\CollisionWorld.cs" /> | ||
54 | <Compile Include="Collision\CollisionDispatch\CompoundCollisionAlgorithm.cs" /> | ||
55 | <Compile Include="Collision\CollisionDispatch\ConvexConcaveCollisionAlgorithm.cs" /> | ||
56 | <Compile Include="Collision\CollisionDispatch\ConvexConvexCollisionAlgorithm.cs" /> | ||
57 | <Compile Include="Collision\CollisionDispatch\ConvexTriangleCallback.cs" /> | ||
58 | <Compile Include="Collision\CollisionDispatch\EmptyAlgorithm.cs" /> | ||
59 | <Compile Include="Collision\CollisionDispatch\ManifoldResult.cs" /> | ||
60 | <Compile Include="Collision\CollisionDispatch\SimulationIslandManager.cs" /> | ||
61 | <Compile Include="Collision\CollisionDispatch\SphereBoxCollisionAlgorithm.cs" /> | ||
62 | <Compile Include="Collision\CollisionDispatch\SphereSphereCollisionAlgorithm.cs" /> | ||
63 | <Compile Include="Collision\CollisionDispatch\SphereTriangleCollisionAlgorithm.cs" /> | ||
64 | <Compile Include="Collision\CollisionDispatch\SphereTriangleDetector.cs" /> | ||
65 | <Compile Include="Collision\CollisionDispatch\UnionFind.cs" /> | ||
66 | <Compile Include="Collision\CollisionShapes\BoxShape.cs" /> | ||
67 | <Compile Include="Collision\CollisionShapes\BUSimplex1to4.cs" /> | ||
68 | <Compile Include="Collision\CollisionShapes\BvhTriangleMeshShape.cs" /> | ||
69 | <Compile Include="Collision\CollisionShapes\CollisionShape.cs" /> | ||
70 | <Compile Include="Collision\CollisionShapes\CompoundShape.cs" /> | ||
71 | <Compile Include="Collision\CollisionShapes\ConcaveShape.cs" /> | ||
72 | <Compile Include="Collision\CollisionShapes\ConeShape.cs" /> | ||
73 | <Compile Include="Collision\CollisionShapes\ConvexHullShape.cs" /> | ||
74 | <Compile Include="Collision\CollisionShapes\ConvexShape.cs" /> | ||
75 | <Compile Include="Collision\CollisionShapes\ConvexTriangleMeshShape.cs" /> | ||
76 | <Compile Include="Collision\CollisionShapes\CylinderShape.cs" /> | ||
77 | <Compile Include="Collision\CollisionShapes\CylinderShapeX.cs" /> | ||
78 | <Compile Include="Collision\CollisionShapes\CylinderShapeZ.cs" /> | ||
79 | <Compile Include="Collision\CollisionShapes\EmptyShape.cs" /> | ||
80 | <Compile Include="Collision\CollisionShapes\FilteredCallback.cs" /> | ||
81 | <Compile Include="Collision\CollisionShapes\InternalTriangleIndexCallback.cs" /> | ||
82 | <Compile Include="Collision\CollisionShapes\LocalSupportVertexCallback.cs" /> | ||
83 | <Compile Include="Collision\CollisionShapes\MinkowskiSumShape.cs" /> | ||
84 | <Compile Include="Collision\CollisionShapes\MultiSphereShape.cs" /> | ||
85 | <Compile Include="Collision\CollisionShapes\NodeOverlapCallback.cs" /> | ||
86 | <Compile Include="Collision\CollisionShapes\OptimizedBvh.cs" /> | ||
87 | <Compile Include="Collision\CollisionShapes\OptimizedBvhNode.cs" /> | ||
88 | <Compile Include="Collision\CollisionShapes\PolyhedralConvexShape.cs" /> | ||
89 | <Compile Include="Collision\CollisionShapes\SphereShape.cs" /> | ||
90 | <Compile Include="Collision\CollisionShapes\StaticPlaneShape.cs" /> | ||
91 | <Compile Include="Collision\CollisionShapes\StridingMeshInterface.cs" /> | ||
92 | <Compile Include="Collision\CollisionShapes\SupportVertexCallback.cs" /> | ||
93 | <Compile Include="Collision\CollisionShapes\TriangleBuffer.cs" /> | ||
94 | <Compile Include="Collision\CollisionShapes\TriangleCallback.cs" /> | ||
95 | <Compile Include="Collision\CollisionShapes\TriangleIndexVertexArray.cs" /> | ||
96 | <Compile Include="Collision\CollisionShapes\TriangleMesh.cs" /> | ||
97 | <Compile Include="Collision\CollisionShapes\TriangleMeshShape.cs" /> | ||
98 | <Compile Include="Collision\CollisionShapes\TriangleShape.cs" /> | ||
99 | <Compile Include="Collision\NarrowPhaseCollision\ContinuousConvexCollision.cs" /> | ||
100 | <Compile Include="Collision\NarrowPhaseCollision\ConvexCast.cs" /> | ||
101 | <Compile Include="Collision\NarrowPhaseCollision\DiscreteCollisionDetectorInterface.cs" /> | ||
102 | <Compile Include="Collision\NarrowPhaseCollision\GjkConvexCast.cs" /> | ||
103 | <Compile Include="Collision\NarrowPhaseCollision\GjkEpa.cs" /> | ||
104 | <Compile Include="Collision\NarrowPhaseCollision\GjkEpaPenetrationDepthSolver.cs" /> | ||
105 | <Compile Include="Collision\NarrowPhaseCollision\GjkEpaSolver.cs" /> | ||
106 | <Compile Include="Collision\NarrowPhaseCollision\GjkPairDetector.cs" /> | ||
107 | <Compile Include="Collision\NarrowPhaseCollision\IConvexPenetrationDepthSolver.cs" /> | ||
108 | <Compile Include="Collision\NarrowPhaseCollision\ISimplexSolver.cs" /> | ||
109 | <Compile Include="Collision\NarrowPhaseCollision\ManifoldPoint.cs" /> | ||
110 | <Compile Include="Collision\NarrowPhaseCollision\MinkowskiPenetrationDepthSolver.cs" /> | ||
111 | <Compile Include="Collision\NarrowPhaseCollision\PersistentManifold.cs" /> | ||
112 | <Compile Include="Collision\NarrowPhaseCollision\PointCollector.cs" /> | ||
113 | <Compile Include="Collision\NarrowPhaseCollision\SubsimplexConvexCast.cs" /> | ||
114 | <Compile Include="Collision\NarrowPhaseCollision\TriangleRaycastCallback.cs" /> | ||
115 | <Compile Include="Collision\NarrowPhaseCollision\VoronoiSimplexSolver.cs" /> | ||
116 | <Compile Include="Dynamics\ConstraintSolver\ContactConstraint.cs" /> | ||
117 | <Compile Include="Dynamics\ConstraintSolver\ContactSolverInfo.cs" /> | ||
118 | <Compile Include="Dynamics\ConstraintSolver\Generic6DofConstraint.cs" /> | ||
119 | <Compile Include="Dynamics\ConstraintSolver\HingeConstraint.cs" /> | ||
120 | <Compile Include="Dynamics\ConstraintSolver\IConstraintSolver.cs" /> | ||
121 | <Compile Include="Dynamics\ConstraintSolver\JacobianEntry.cs" /> | ||
122 | <Compile Include="Dynamics\ConstraintSolver\Point2PointConstraint.cs" /> | ||
123 | <Compile Include="Dynamics\ConstraintSolver\SequentialImpulseConstraintSolver.cs" /> | ||
124 | <Compile Include="Dynamics\ConstraintSolver\Solve2LinearConstraint.cs" /> | ||
125 | <Compile Include="Dynamics\ConstraintSolver\SolverBody.cs" /> | ||
126 | <Compile Include="Dynamics\ConstraintSolver\SolverConstraint.cs" /> | ||
127 | <Compile Include="Dynamics\ConstraintSolver\TypedConstraint.cs" /> | ||
128 | <Compile Include="Dynamics\DiscreteDynamicsWorld.cs" /> | ||
129 | <Compile Include="Dynamics\DynamicsWorld.cs" /> | ||
130 | <Compile Include="Dynamics\RigidBody.cs" /> | ||
131 | <Compile Include="Dynamics\SimpleDynamicsWorld.cs" /> | ||
132 | <Compile Include="Dynamics\Vehicle\RaycastVehicle.cs" /> | ||
133 | <Compile Include="Dynamics\Vehicle\VehicleRaycaster.cs" /> | ||
134 | <Compile Include="Dynamics\Vehicle\WheelInfo.cs" /> | ||
135 | <Compile Include="Exceptions\BulletException.cs" /> | ||
136 | <Compile Include="IDebugDraw.cs" /> | ||
137 | <Compile Include="LinearMath\DefaultMotionState.cs" /> | ||
138 | <Compile Include="LinearMath\MathHelper.cs" /> | ||
139 | <Compile Include="LinearMath\MatrixOperations.cs" /> | ||
140 | <Compile Include="LinearMath\MotionState.cs" /> | ||
141 | <Compile Include="LinearMath\QuadWord.cs" /> | ||
142 | <Compile Include="LinearMath\Quaternion.cs" /> | ||
143 | <Compile Include="LinearMath\TransformUtil.cs" /> | ||
144 | <Compile Include="LinearMath\Vector3.cs" /> | ||
145 | <Compile Include="LinearMath\Vector4.cs" /> | ||
146 | <Compile Include="Properties\AssemblyInfo.cs" /> | ||
147 | </ItemGroup> | ||
148 | <ItemGroup> | ||
149 | <ProjectReference Include="..\MonoXnaCompactMaths\MonoXnaCompactMaths.csproj"> | ||
150 | <Project>{121147BC-B06B-406C-84E9-907F268CF0EB}</Project> | ||
151 | <Name>MonoXnaCompactMaths</Name> | ||
152 | </ProjectReference> | ||
153 | </ItemGroup> | ||
154 | <ItemGroup> | ||
155 | <None Include="BulletX.snk" /> | ||
156 | </ItemGroup> | ||
157 | <ItemGroup> | ||
158 | <Folder Include="Content\" /> | ||
159 | </ItemGroup> | ||
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. | ||
162 | Other similar extension points exist, see Microsoft.Common.targets. | ||
163 | <Target Name="BeforeBuild"> | ||
164 | </Target> | ||
165 | <Target Name="AfterBuild"> | ||
166 | </Target> | ||
167 | --> | ||
168 | </Project> \ No newline at end of file | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs new file mode 100644 index 0000000..efc8fd6 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs | |||
@@ -0,0 +1,35 @@ | |||
1 | using System.Reflection; | ||
2 | using System.Runtime.CompilerServices; | ||
3 | using System.Runtime.InteropServices; | ||
4 | |||
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 | ||
7 | // asociada con un ensamblado. | ||
8 | [assembly: AssemblyTitle("Modified.XnaDevRu.BulletX")] | ||
9 | [assembly: AssemblyDescription("Modified Bullet for XNA")] | ||
10 | [assembly: AssemblyConfiguration("")] | ||
11 | [assembly: AssemblyCompany("XNADev.ru")] | ||
12 | [assembly: AssemblyProduct("Bullet for XNA")] | ||
13 | [assembly: AssemblyCopyright("Copyright © 2007 XNADev.ru")] | ||
14 | [assembly: AssemblyTrademark("")] | ||
15 | [assembly: AssemblyCulture("")] | ||
16 | |||
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 | ||
19 | // COM, establezca el atributo ComVisible como true en este tipo. | ||
20 | [assembly: ComVisible(false)] | ||
21 | |||
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")] | ||
24 | |||
25 | // La información de versión de un ensamblado consta de los cuatro valores siguientes: | ||
26 | // | ||
27 | // Versión principal | ||
28 | // Versión secundaria | ||
29 | // Número de versión de compilación | ||
30 | // Revisión | ||
31 | // | ||
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: | ||
34 | [assembly: AssemblyVersion("1.0.0.0")] | ||
35 | [assembly: AssemblyFileVersion("1.0.0.0")] | ||
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs new file mode 100644 index 0000000..a104d89 --- /dev/null +++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs | |||
@@ -0,0 +1,683 @@ | |||
1 | #region License | ||
2 | /* | ||
3 | MIT License | ||
4 | Copyright © 2006 The Mono.Xna Team | ||
5 | |||
6 | All rights reserved. | ||
7 | |||
8 | Permission is hereby granted, free of charge, to any person obtaining a copy | ||
9 | of this software and associated documentation files (the "Software"), to deal | ||
10 | in the Software without restriction, including without limitation the rights | ||
11 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
12 | copies of the Software, and to permit persons to whom the Software is | ||
13 | furnished to do so, subject to the following conditions: | ||
14 | |||
15 | The above copyright notice and this permission notice shall be included in all | ||
16 | copies or substantial portions of the Software. | ||
17 | |||
18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
19 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
20 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
21 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
22 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
23 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE | ||
24 | SOFTWARE. | ||
25 | */ | ||
26 | #endregion License | ||
27 | |||
28 | using System; | ||
29 | using System.ComponentModel; | ||
30 | using System.Runtime.InteropServices; | ||
31 | |||
32 | namespace MonoXnaCompactMaths | ||
33 | { | ||
34 | [Serializable] | ||
35 | [StructLayout(LayoutKind.Sequential)] | ||
36 | //[TypeConverter(typeof(MatrixConverter))] | ||
37 | public struct Matrix : IEquatable<Matrix> | ||
38 | { | ||
39 | #region Public Constructors | ||
40 | |||
41 | public Matrix(float m11, float m12, float m13, float m14, float m21, float m22, float m23, float m24, float m31, | ||
42 | float m32, float m33, float m34, float m41, float m42, float m43, float m44) | ||
43 | { | ||
44 | this.M11 = m11; | ||
45 | this.M12 = m12; | ||
46 | this.M13 = m13; | ||
47 | this.M14 = m14; | ||
48 | this.M21 = m21; | ||
49 | this.M22 = m22; | ||
50 | this.M23 = m23; | ||
51 | this.M24 = m24; | ||
52 | this.M31 = m31; | ||
53 | this.M32 = m32; | ||
54 | this.M33 = m33; | ||
55 | this.M34 = m34; | ||
56 | this.M41 = m41; | ||
57 | this.M42 = m42; | ||
58 | this.M43 = m43; | ||
59 | this.M44 = m44; | ||
60 | } | ||
61 | |||
62 | #endregion Public Constructors | ||
63 | |||
64 | |||
65 | #region Public Fields | ||
66 | |||
67 | public float M11; | ||
68 | public float M12; | ||
69 | public float M13; | ||
70 | public float M14; | ||
71 | public float M21; | ||
72 | public float M22; | ||
73 | public float M23; | ||
74 | public float M24; | ||
75 | public float M31; | ||
76 | public float M32; | ||
77 | public float M33; | ||
78 | public float M34; | ||
79 | public float M41; | ||
80 | public float M42; | ||
81 | public float M43; | ||
82 | public float M44; | ||
83 | |||
84 | #endregion Public Fields | ||
85 | |||
86 | |||
87 | #region Private Members | ||
88 | private static Matrix identity = new Matrix(1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f); | ||
89 | #endregion Private Members | ||
90 | |||
91 | |||
92 | #region Public Properties | ||
93 | |||
94 | public Vector3 Backward | ||
95 | { | ||
96 | get | ||
97 | { | ||
98 | return new Vector3(this.M31, this.M32, this.M33); | ||
99 | } | ||
100 | set | ||
101 | { | ||
102 | this.M31 = value.X; | ||
103 | this.M32 = value.Y; | ||
104 | this.M33 = value.Z; | ||
105 | } | ||
106 | } | ||
107 | |||
108 | |||
109 | public Vector3 Down | ||
110 | { | ||
111 | get | ||
112 | { | ||
113 | return new Vector3(-this.M21, -this.M22, -this.M23); | ||
114 | } | ||
115 | set | ||
116 | { | ||
117 | this.M21 = -value.X; | ||
118 | this.M22 = -value.Y; | ||
119 | this.M23 = -value.Z; | ||
120 | } | ||
121 | } | ||
122 | |||
123 | |||
124 | public Vector3 Forward | ||
125 | { | ||
126 | get | ||
127 | { | ||
128 | return new Vector3(-this.M31, -this.M32, -this.M33); | ||
129 | } | ||
130 | set | ||
131 | { | ||
132 | this.M31 = -value.X; | ||
133 | this.M32 = -value.Y; | ||
134 | this.M33 = -value.Z; | ||
135 | } | ||
136 | } | ||
137 | |||
138 | |||
139 | public static Matrix Identity | ||
140 | { | ||
141 | get { return identity; } | ||
142 | } | ||
143 | |||
144 | |||
145 | public Vector3 Left | ||
146 | { | ||
147 | get | ||
148 | { | ||
149 | return new Vector3(-this.M11, -this.M12, -this.M13); | ||
150 | } | ||
151 | set | ||
152 | { | ||
153 | this.M11 = -value.X; | ||
154 | this.M12 = -value.Y; | ||
155 | this.M13 = -value.Z; | ||
156 | } | ||
157 | } | ||
158 | |||
159 | |||
160 | public Vector3 Right | ||
161 | { | ||
162 | get | ||
163 | { | ||
164 | return new Vector3(this.M11, this.M12, this.M13); | ||
165 | } | ||
166 | set | ||
167 | { | ||
168 | this.M11 = value.X; | ||
169 | this.M12 = value.Y; | ||
170 | this.M13 = value.Z; | ||
171 | } | ||
172 | } | ||
173 | |||
174 | |||
175 | public Vector3 Translation | ||
176 | { | ||
177 | get | ||
178 | { | ||
179 | return new Vector3(this.M41, this.M42, this.M43); | ||
180 | } | ||
181 | set | ||
182 | { | ||
183 | this.M41 = value.X; | ||
184 | this.M42 = value.Y; | ||
185 | this.M43 = value.Z; | ||
186 | } | ||
187 | } | ||
188 | |||
189 | |||
190 | public Vector3 Up | ||
191 | { | ||
192 | get | ||
193 | { | ||
194 | return new Vector3(this.M21, this.M22, this.M23); | ||
195 | } | ||
196 | set | ||
197 | { | ||
198 | this.M21 = value.X; | ||
199 | this.M22 = value.Y; | ||
200 | this.M23 = value.Z; | ||
201 | } | ||
202 | } | ||
203 | #endregion Public Properties | ||
204 | |||
205 | |||
206 | #region Public Methods | ||
207 | |||
208 | public static Matrix Add(Matrix matrix1, Matrix matrix2) | ||
209 | { | ||
210 | throw new NotImplementedException(); | ||
211 | } | ||
212 | |||
213 | |||
214 | public static void Add(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) | ||
215 | { | ||
216 | throw new NotImplementedException(); | ||
217 | } | ||
218 | |||
219 | |||
220 | public static Matrix CreateBillboard(Vector3 objectPosition, Vector3 cameraPosition, | ||
221 | Vector3 cameraUpVector, Nullable<Vector3> cameraForwardVector) | ||
222 | { | ||
223 | throw new NotImplementedException(); | ||
224 | } | ||
225 | |||
226 | |||
227 | public static void CreateBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition, | ||
228 | ref Vector3 cameraUpVector, Vector3? cameraForwardVector, out Matrix result) | ||
229 | { | ||
230 | throw new NotImplementedException(); | ||
231 | } | ||
232 | |||
233 | |||
234 | public static Matrix CreateConstrainedBillboard(Vector3 objectPosition, Vector3 cameraPosition, | ||
235 | Vector3 rotateAxis, Nullable<Vector3> cameraForwardVector, Nullable<Vector3> objectForwardVector) | ||
236 | { | ||
237 | throw new NotImplementedException(); | ||
238 | } | ||
239 | |||
240 | |||
241 | public static void CreateConstrainedBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition, | ||
242 | ref Vector3 rotateAxis, Vector3? cameraForwardVector, Vector3? objectForwardVector, out Matrix result) | ||
243 | { | ||
244 | throw new NotImplementedException(); | ||
245 | } | ||
246 | |||
247 | |||
248 | public static Matrix CreateFromAxisAngle(Vector3 axis, float angle) | ||
249 | { | ||
250 | throw new NotImplementedException(); | ||
251 | } | ||
252 | |||
253 | |||
254 | public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Matrix result) | ||
255 | { | ||
256 | throw new NotImplementedException(); | ||
257 | } | ||
258 | |||
259 | |||
260 | public static Matrix CreateFromQuaternion(Quaternion quaternion) | ||
261 | { | ||
262 | //--- | ||
263 | //http://lists.ximian.com/pipermail/mono-patches/2006-December/084667.html | ||
264 | float xx = quaternion.X * quaternion.X; | ||
265 | float xy = quaternion.X * quaternion.Y; | ||
266 | float xw = quaternion.X * quaternion.W; | ||
267 | float yy = quaternion.Y * quaternion.Y; | ||
268 | float yw = quaternion.Y * quaternion.W; | ||
269 | float yz = quaternion.Y * quaternion.Z; | ||
270 | float zx = quaternion.Z * quaternion.X; | ||
271 | float zw = quaternion.Z * quaternion.W; | ||
272 | float zz = quaternion.Z * quaternion.Z; | ||
273 | return new Matrix(1f - (2f * (yy + zz)), 2f * (xy + zw), 2f * (zx - yw), 0f, 2f * (xy - zw), | ||
274 | 1f - (2f * (zz + xx)), 2f * (yz + xw), 0f, 2f * (zx + yw), 2f * (yz - xw), | ||
275 | 1f - (2f * (yy + xx)), 0f, 0f, 0f, 0f, 1f); | ||
276 | //throw new NotImplementedException(); | ||
277 | } | ||
278 | |||
279 | |||
280 | public static void CreateFromQuaternion(ref Quaternion quaternion, out Matrix result) | ||
281 | { | ||
282 | throw new NotImplementedException(); | ||
283 | } | ||
284 | |||
285 | |||
286 | public static Matrix CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUpVector) | ||
287 | { | ||
288 | throw new NotImplementedException(); | ||
289 | } | ||
290 | |||
291 | |||
292 | public static void CreateLookAt(ref Vector3 cameraPosition, ref Vector3 cameraTarget, ref Vector3 cameraUpVector, out Matrix result) | ||
293 | { | ||
294 | throw new NotImplementedException(); | ||
295 | } | ||
296 | |||
297 | |||
298 | public static Matrix CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane) | ||
299 | { | ||
300 | throw new NotImplementedException(); | ||
301 | } | ||
302 | |||
303 | |||
304 | public static void CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane, out Matrix result) | ||
305 | { | ||
306 | throw new NotImplementedException(); | ||
307 | } | ||
308 | |||
309 | |||
310 | public static Matrix CreateOrthographicOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane) | ||
311 | { | ||
312 | throw new NotImplementedException(); | ||
313 | } | ||
314 | |||
315 | |||
316 | public static void CreateOrthographicOffCenter(float left, float right, float bottom, float top, | ||
317 | float zNearPlane, float zFarPlane, out Matrix result) | ||
318 | { | ||
319 | throw new NotImplementedException(); | ||
320 | } | ||
321 | |||
322 | |||
323 | public static Matrix CreatePerspective(float width, float height, float zNearPlane, float zFarPlane) | ||
324 | { | ||
325 | throw new NotImplementedException(); | ||
326 | } | ||
327 | |||
328 | |||
329 | public static void CreatePerspective(float width, float height, float zNearPlane, float zFarPlane, out Matrix result) | ||
330 | { | ||
331 | throw new NotImplementedException(); | ||
332 | } | ||
333 | |||
334 | |||
335 | public static Matrix CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float zNearPlane, float zFarPlane) | ||
336 | { | ||
337 | throw new NotImplementedException(); | ||
338 | } | ||
339 | |||
340 | |||
341 | public static void CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float nearPlaneDistance, float farPlaneDistance, out Matrix result) | ||
342 | { | ||
343 | throw new NotImplementedException(); | ||
344 | } | ||
345 | |||
346 | |||
347 | public static Matrix CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane) | ||
348 | { | ||
349 | throw new NotImplementedException(); | ||
350 | } | ||
351 | |||
352 | |||
353 | public static void CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float nearPlaneDistance, float farPlaneDistance, out Matrix result) | ||
354 | { | ||
355 | throw new NotImplementedException(); | ||
356 | } | ||
357 | |||
358 | |||
359 | public static Matrix CreateRotationX(float radians) | ||
360 | { | ||
361 | throw new NotImplementedException(); | ||
362 | } | ||
363 | |||
364 | |||
365 | public static void CreateRotationX(float radians, out Matrix result) | ||
366 | { | ||
367 | throw new NotImplementedException(); | ||
368 | } | ||
369 | |||
370 | |||
371 | public static Matrix CreateRotationY(float radians) | ||
372 | { | ||
373 | throw new NotImplementedException(); | ||
374 | } | ||
375 | |||
376 | |||
377 | public static void CreateRotationY(float radians, out Matrix result) | ||
378 | { | ||
379 | throw new NotImplementedException(); | ||
380 | } | ||
381 | |||
382 | |||
383 | public static Matrix CreateRotationZ(float radians) | ||
384 | { | ||
385 | throw new NotImplementedException(); | ||
386 | } | ||
387 | |||
388 | |||
389 | public static void CreateRotationZ(float radians, out Matrix result) | ||
390 | { | ||
391 | throw new NotImplementedException(); | ||
392 | } | ||
393 | |||
394 | |||
395 | public static Matrix CreateScale(float scale) | ||
396 | { | ||
397 | throw new NotImplementedException(); | ||
398 | } | ||
399 | |||
400 | |||
401 | public static void CreateScale(float scale, out Matrix result) | ||
402 | { | ||
403 | throw new NotImplementedException(); | ||
404 | } | ||
405 | |||
406 | |||
407 | public static Matrix CreateScale(float xScale, float yScale, float zScale) | ||
408 | { | ||
409 | throw new NotImplementedException(); | ||
410 | } | ||
411 | |||
412 | |||
413 | public static void CreateScale(float xScale, float yScale, float zScale, out Matrix result) | ||
414 | { | ||
415 | throw new NotImplementedException(); | ||
416 | } | ||
417 | |||
418 | |||
419 | public static Matrix CreateScale(Vector3 scales) | ||
420 | { | ||
421 | throw new NotImplementedException(); | ||
422 | } | ||
423 | |||
424 | |||
425 | public static void CreateScale(ref Vector3 scales, out Matrix result) | ||
426 | { | ||
427 | throw new NotImplementedException(); | ||
428 | } | ||
429 | |||
430 | |||
431 | public static Matrix CreateTranslation(float xPosition, float yPosition, float zPosition) | ||
432 | { | ||
433 | throw new NotImplementedException(); | ||
434 | } | ||
435 | |||
436 | |||
437 | public static void CreateTranslation(ref Vector3 position, out Matrix result) | ||
438 | { | ||
439 | throw new NotImplementedException(); | ||
440 | } | ||
441 | |||
442 | |||
443 | public static Matrix CreateTranslation(Vector3 position) | ||
444 | { | ||
445 | throw new NotImplementedException(); | ||
446 | } | ||
447 | |||
448 | |||
449 | public static void CreateTranslation(float xPosition, float yPosition, float zPosition, out Matrix result) | ||
450 | { | ||
451 | throw new NotImplementedException(); | ||
452 | } | ||
453 | |||
454 | |||
455 | public float Determinant() | ||
456 | { | ||
457 | throw new NotImplementedException(); | ||
458 | } | ||
459 | |||
460 | |||
461 | public static Matrix Divide(Matrix matrix1, Matrix matrix2) | ||
462 | { | ||
463 | throw new NotImplementedException(); | ||
464 | } | ||
465 | |||
466 | |||
467 | public static void Divide(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) | ||
468 | { | ||
469 | throw new NotImplementedException(); | ||
470 | } | ||
471 | |||
472 | |||
473 | public static Matrix Divide(Matrix matrix1, float divider) | ||
474 | { | ||
475 | throw new NotImplementedException(); | ||
476 | } | ||
477 | |||
478 | |||
479 | public static void Divide(ref Matrix matrix1, float divider, out Matrix result) | ||
480 | { | ||
481 | throw new NotImplementedException(); | ||
482 | } | ||
483 | |||
484 | |||
485 | public bool Equals(Matrix other) | ||
486 | { | ||
487 | throw new NotImplementedException(); | ||
488 | } | ||
489 | |||
490 | |||
491 | public override bool Equals(object obj) | ||
492 | { | ||
493 | throw new NotImplementedException(); | ||
494 | } | ||
495 | |||
496 | |||
497 | public override int GetHashCode() | ||
498 | { | ||
499 | throw new NotImplementedException(); | ||
500 | } | ||
501 | |||
502 | |||
503 | public static Matrix Invert(Matrix matrix) | ||
504 | { | ||
505 | throw new NotImplementedException(); | ||
506 | } | ||
507 | |||
508 | |||
509 | public static void Invert(ref Matrix matrix, out Matrix result) | ||
510 | { | ||
511 | throw new NotImplementedException(); | ||
512 | } | ||
513 | |||
514 | |||
515 | public static Matrix Lerp(Matrix matrix1, Matrix matrix2, float amount) | ||
516 | { | ||
517 | throw new NotImplementedException(); | ||
518 | } | ||
519 | |||
520 | |||
521 | public static void Lerp(ref Matrix matrix1, ref Matrix matrix2, float amount, out Matrix result) | ||
522 | { | ||
523 | throw new NotImplementedException(); | ||
524 | } | ||
525 | |||
526 | public static Matrix Multiply(Matrix matrix1, Matrix matrix2) | ||
527 | { | ||
528 | throw new NotImplementedException(); | ||
529 | } | ||
530 | |||
531 | |||
532 | public static void Multiply(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) | ||
533 | { | ||
534 | throw new NotImplementedException(); | ||
535 | } | ||
536 | |||
537 | |||
538 | public static Matrix Multiply(Matrix matrix1, float factor) | ||
539 | { | ||
540 | throw new NotImplementedException(); | ||
541 | } | ||
542 | |||
543 | |||
544 | public static void Multiply(ref Matrix matrix1, float factor, out Matrix result) | ||
545 | { | ||
546 | throw new NotImplementedException(); | ||
547 | } | ||
548 | |||
549 | |||
550 | public static Matrix Negate(Matrix matrix) | ||
551 | { | ||
552 | throw new NotImplementedException(); | ||
553 | } | ||
554 | |||
555 | |||
556 | public static void Negate(ref Matrix matrix, out Matrix result) | ||
557 | { | ||
558 | throw new NotImplementedException(); | ||
559 | } | ||
560 | |||
561 | |||
562 | public static Matrix operator +(Matrix matrix1, Matrix matrix2) | ||
563 | { | ||
564 | throw new NotImplementedException(); | ||
565 | } | ||
566 | |||
567 | |||
568 | public static Matrix operator /(Matrix matrix1, Matrix matrix2) | ||
569 | { | ||
570 | throw new NotImplementedException(); | ||
571 | } | ||
572 | |||
573 | |||
574 | public static Matrix operator /(Matrix matrix1, float divider) | ||
575 | { | ||
576 | throw new NotImplementedException(); | ||
577 | } | ||
578 | |||
579 | |||
580 | public static bool operator ==(Matrix matrix1, Matrix matrix2) | ||
581 | { | ||
582 | throw new NotImplementedException(); | ||
583 | } | ||
584 | |||
585 | |||
586 | public static bool operator !=(Matrix matrix1, Matrix matrix2) | ||
587 | { | ||
588 | throw new NotImplementedException(); | ||
589 | } | ||
590 | |||
591 | |||
592 | public static Matrix operator *(Matrix matrix1, Matrix matrix2) | ||
593 | { | ||
594 | //--- | ||
595 | float[, ] arrayMatrix1 = new float[4, 4]; | ||
596 | float[, ] arrayMatrix2 = new float[4, 4]; | ||
597 | float[, ] arrayMatrixProduct = new float[4, 4]; | ||
598 | arrayMatrix1[0, 0] = matrix1.M11; arrayMatrix1[0, 1] = matrix1.M12; arrayMatrix1[0, 2] = matrix1.M13; arrayMatrix1[0, 3] = matrix1.M14; | ||
599 | arrayMatrix1[1, 0] = matrix1.M21; arrayMatrix1[1, 1] = matrix1.M22; arrayMatrix1[1, 2] = matrix1.M23; arrayMatrix1[1, 3] = matrix1.M24; | ||
600 | arrayMatrix1[2, 0] = matrix1.M31; arrayMatrix1[2, 1] = matrix1.M32; arrayMatrix1[2, 2] = matrix1.M33; arrayMatrix1[2, 3] = matrix1.M34; | ||
601 | arrayMatrix1[3, 0] = matrix1.M41; arrayMatrix1[3, 1] = matrix1.M42; arrayMatrix1[3, 2] = matrix1.M43; arrayMatrix1[3, 3] = matrix1.M44; | ||
602 | |||
603 | arrayMatrix2[0, 0] = matrix2.M11; arrayMatrix2[0, 1] = matrix2.M12; arrayMatrix2[0, 2] = matrix2.M13; arrayMatrix2[0, 3] = matrix2.M14; | ||
604 | arrayMatrix2[1, 0] = matrix2.M21; arrayMatrix2[1, 1] = matrix2.M22; arrayMatrix2[1, 2] = matrix2.M23; arrayMatrix2[1, 3] = matrix2.M24; | ||
605 | arrayMatrix2[2, 0] = matrix2.M31; arrayMatrix2[2, 1] = matrix2.M32; arrayMatrix2[2, 2] = matrix2.M33; arrayMatrix2[2, 3] = matrix2.M34; | ||
606 | arrayMatrix2[3, 0] = matrix2.M41; arrayMatrix2[3, 1] = matrix2.M42; arrayMatrix2[3, 2] = matrix2.M43; arrayMatrix2[3, 3] = matrix2.M44; | ||
607 | |||
608 | int n = 4; | ||
609 | for (int i = 0; i < n; i++) | ||
610 | { | ||
611 | for (int j = 0; j < n; j++) | ||
612 | { | ||
613 | // (AB)[i,j] = Sum(k=0; k < 4; k++) { A[i,k] * B[k, j] } | ||
614 | for (int k = 0; k < n; k++) | ||
615 | { | ||
616 | arrayMatrixProduct[i, j] += arrayMatrix1[i, k] * arrayMatrix2[k, j]; | ||
617 | } | ||
618 | } | ||
619 | } | ||
620 | return new Matrix( arrayMatrixProduct[0, 0], arrayMatrixProduct[0, 1], arrayMatrixProduct[0, 2], arrayMatrixProduct[0, 3], | ||
621 | arrayMatrixProduct[1, 0], arrayMatrixProduct[1, 1], arrayMatrixProduct[1, 2], arrayMatrixProduct[1, 3], | ||
622 | arrayMatrixProduct[2, 0], arrayMatrixProduct[2, 1], arrayMatrixProduct[2, 2], arrayMatrixProduct[2, 3], | ||
623 | arrayMatrixProduct[3, 1], arrayMatrixProduct[3, 1], arrayMatrixProduct[3, 2], arrayMatrixProduct[3, 3]); | ||
624 | //--- | ||
625 | //throw new NotImplementedException(); | ||
626 | } | ||
627 | |||
628 | |||
629 | public static Matrix operator *(Matrix matrix, float scaleFactor) | ||
630 | { | ||
631 | throw new NotImplementedException(); | ||
632 | } | ||
633 | |||
634 | |||
635 | public static Matrix operator -(Matrix matrix1, Matrix matrix2) | ||
636 | { | ||
637 | throw new NotImplementedException(); | ||
638 | } | ||
639 | |||
640 | |||
641 | public static Matrix operator -(Matrix matrix1) | ||
642 | { | ||
643 | throw new NotImplementedException(); | ||
644 | } | ||
645 | |||
646 | |||
647 | public static Matrix Subtract(Matrix matrix1, Matrix matrix2) | ||
648 | { | ||
649 | throw new NotImplementedException(); | ||
650 | } | ||
651 | |||
652 | |||
653 | public static void Subtract(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) | ||
654 | { | ||
655 | throw new NotImplementedException(); | ||
656 | } | ||
657 | |||
658 | |||
659 | public override string ToString() | ||
660 | { | ||
661 | throw new NotImplementedException(); | ||
662 | } | ||
663 | |||
664 | |||
665 | public static Matrix Transpose(Matrix matrix) | ||
666 | { | ||
667 | //--- | ||
668 | return new Matrix( matrix.M11, matrix.M21, matrix.M31, matrix.M41, | ||
669 | matrix.M12, matrix.M22, matrix.M32, matrix.M42, | ||
670 | matrix.M13, matrix.M23, matrix.M33, matrix.M43, | ||
671 | matrix.M14, matrix.M24, matrix.M34, matrix.M44); | ||
672 | //--- | ||
673 | //throw new NotImplementedException(); | ||
674 | } | ||
675 | |||
676 | |||
677 | public static void Transpose(ref Matrix matrix, out Matrix result) | ||
678 | { | ||
679 | throw new NotImplementedException(); | ||
680 | } | ||
681 | #endregion Public Methods | ||
682 | } | ||
683 | } | ||
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj b/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj new file mode 100644 index 0000000..c3ec4dd --- /dev/null +++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj | |||
@@ -0,0 +1,51 @@ | |||
1 | <?xml version="1.0" encoding="utf-8"?> | ||
2 | <Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> | ||
3 | <PropertyGroup> | ||
4 | <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration> | ||
5 | <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform> | ||
6 | <ProductVersion>8.0.50727</ProductVersion> | ||
7 | <SchemaVersion>2.0</SchemaVersion> | ||
8 | <ProjectGuid>{121147BC-B06B-406C-84E9-907F268CF0EB}</ProjectGuid> | ||
9 | <OutputType>Library</OutputType> | ||
10 | <AppDesignerFolder>Properties</AppDesignerFolder> | ||
11 | <RootNamespace>MonoXnaCompactMaths</RootNamespace> | ||
12 | <AssemblyName>MonoXnaCompactMaths</AssemblyName> | ||
13 | <StartupObject> | ||
14 | </StartupObject> | ||
15 | </PropertyGroup> | ||
16 | <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' "> | ||
17 | <DebugSymbols>true</DebugSymbols> | ||
18 | <DebugType>full</DebugType> | ||
19 | <Optimize>false</Optimize> | ||
20 | <OutputPath>bin\Debug\</OutputPath> | ||
21 | <DefineConstants>DEBUG;TRACE</DefineConstants> | ||
22 | <ErrorReport>prompt</ErrorReport> | ||
23 | <WarningLevel>4</WarningLevel> | ||
24 | </PropertyGroup> | ||
25 | <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' "> | ||
26 | <DebugType>pdbonly</DebugType> | ||
27 | <Optimize>true</Optimize> | ||
28 | <OutputPath>bin\Release\</OutputPath> | ||
29 | <DefineConstants>TRACE</DefineConstants> | ||
30 | <ErrorReport>prompt</ErrorReport> | ||
31 | <WarningLevel>4</WarningLevel> | ||
32 | </PropertyGroup> | ||
33 | <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" /> | ||
34 | <!-- To modify your build process, add your task inside one of the targets below and uncomment it. | ||
35 | Other similar extension points exist, see Microsoft.Common.targets. | ||
36 | <Target Name="BeforeBuild"> | ||
37 | </Target> | ||
38 | <Target Name="AfterBuild"> | ||
39 | </Target> | ||
40 | --> | ||
41 | <ItemGroup> | ||
42 | <Compile Include="Matrix.cs" /> | ||
43 | <Compile Include="Properties\AssemblyInfo.cs" /> | ||
44 | <Compile Include="Quaternion.cs" /> | ||
45 | <Compile Include="Vector3.cs" /> | ||
46 | <Compile Include="Vector4.cs" /> | ||
47 | </ItemGroup> | ||
48 | <ItemGroup> | ||
49 | <Reference Include="System" /> | ||
50 | </ItemGroup> | ||
51 | </Project> \ No newline at end of file | ||
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs new file mode 100644 index 0000000..748b94a --- /dev/null +++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs | |||
@@ -0,0 +1,33 @@ | |||
1 | using System.Reflection; | ||
2 | using System.Runtime.CompilerServices; | ||
3 | using System.Runtime.InteropServices; | ||
4 | |||
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 | ||
7 | // asociada con un ensamblado. | ||
8 | [assembly: AssemblyTitle("MonoXnaCompactMaths")] | ||
9 | [assembly: AssemblyDescription("MonoXnaCompactMaths (Vector3, Vector4, Quaternion and Matrix)")] | ||
10 | [assembly: AssemblyConfiguration("")] | ||
11 | [assembly: AssemblyCompany("")] | ||
12 | [assembly: AssemblyProduct("")] | ||
13 | [assembly: AssemblyCopyright("Copyright 2006 - 2007 The Mono.Xna Team")] | ||
14 | [assembly: AssemblyTrademark("")] | ||
15 | [assembly: AssemblyCulture("")] | ||
16 | |||
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 | ||
19 | // COM, establezca el atributo ComVisible como true en este tipo. | ||
20 | [assembly: ComVisible(false)] | ||
21 | |||
22 | // El siguiente GUID sirve como identificador de la biblioteca de tipos si este proyecto se expone a COM | ||
23 | [assembly: Guid("79f9dfd1-60f8-44b8-ab08-6f6e341976b7")] | ||
24 | |||
25 | // La información de versión de un ensamblado consta de los cuatro valores siguientes: | ||
26 | // | ||
27 | // Versión principal | ||
28 | // Versión secundaria | ||
29 | // Número de versión de compilación | ||
30 | // Revisión | ||
31 | // | ||
32 | [assembly: AssemblyVersion("1.0.0.0")] | ||
33 | [assembly: AssemblyFileVersion("1.0.0.0")] | ||
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs new file mode 100644 index 0000000..6ed1443 --- /dev/null +++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs | |||
@@ -0,0 +1,346 @@ | |||
1 | #region License | ||
2 | /* | ||
3 | MIT License | ||
4 | Copyright © 2006 The Mono.Xna Team | ||
5 | |||
6 | All rights reserved. | ||
7 | |||
8 | Permission is hereby granted, free of charge, to any person obtaining a copy | ||
9 | of this software and associated documentation files (the "Software"), to deal | ||
10 | in the Software without restriction, including without limitation the rights | ||
11 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
12 | copies of the Software, and to permit persons to whom the Software is | ||
13 | furnished to do so, subject to the following conditions: | ||
14 | |||
15 | The above copyright notice and this permission notice shall be included in all | ||
16 | copies or substantial portions of the Software. | ||
17 | |||
18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
19 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
20 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
21 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
22 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
23 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE | ||
24 | SOFTWARE. | ||
25 | */ | ||
26 | #endregion License | ||
27 | |||
28 | using System; | ||
29 | using System.ComponentModel; | ||
30 | |||
31 | namespace MonoXnaCompactMaths | ||
32 | { | ||
33 | [Serializable] | ||
34 | //[TypeConverter(typeof(QuaternionConverter))] | ||
35 | public struct Quaternion : IEquatable<Quaternion> | ||
36 | { | ||
37 | public float X; | ||
38 | public float Y; | ||
39 | public float Z; | ||
40 | public float W; | ||
41 | static Quaternion identity = new Quaternion(0, 0, 0, 1); | ||
42 | |||
43 | |||
44 | public Quaternion(float x, float y, float z, float w) | ||
45 | { | ||
46 | this.X = x; | ||
47 | this.Y = y; | ||
48 | this.Z = z; | ||
49 | this.W = w; | ||
50 | } | ||
51 | |||
52 | |||
53 | public Quaternion(Vector3 vectorPart, float scalarPart) | ||
54 | { | ||
55 | this.X = vectorPart.X; | ||
56 | this.Y = vectorPart.Y; | ||
57 | this.Z = vectorPart.Z; | ||
58 | this.W = scalarPart; | ||
59 | } | ||
60 | |||
61 | public static Quaternion Identity | ||
62 | { | ||
63 | get{ return identity; } | ||
64 | } | ||
65 | |||
66 | |||
67 | public static Quaternion Add(Quaternion quaternion1, Quaternion quaternion2) | ||
68 | { | ||
69 | throw new NotImplementedException(); | ||
70 | } | ||
71 | |||
72 | |||
73 | public static void Add(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) | ||
74 | { | ||
75 | throw new NotImplementedException(); | ||
76 | } | ||
77 | |||
78 | |||
79 | public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle) | ||
80 | { | ||
81 | throw new NotImplementedException(); | ||
82 | } | ||
83 | |||
84 | |||
85 | public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Quaternion result) | ||
86 | { | ||
87 | throw new NotImplementedException(); | ||
88 | } | ||
89 | |||
90 | |||
91 | public static Quaternion CreateFromRotationMatrix(Matrix matrix) | ||
92 | { | ||
93 | throw new NotImplementedException(); | ||
94 | } | ||
95 | |||
96 | |||
97 | public static void CreateFromRotationMatrix(ref Matrix matrix, out Quaternion result) | ||
98 | { | ||
99 | throw new NotImplementedException(); | ||
100 | } | ||
101 | |||
102 | |||
103 | public static Quaternion Divide(Quaternion quaternion1, Quaternion quaternion2) | ||
104 | { | ||
105 | throw new NotImplementedException(); | ||
106 | } | ||
107 | |||
108 | |||
109 | public static void Divide(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) | ||
110 | { | ||
111 | throw new NotImplementedException(); | ||
112 | } | ||
113 | |||
114 | |||
115 | public static float Dot(Quaternion quaternion1, Quaternion quaternion2) | ||
116 | { | ||
117 | throw new NotImplementedException(); | ||
118 | } | ||
119 | |||
120 | |||
121 | public static void Dot(ref Quaternion quaternion1, ref Quaternion quaternion2, out float result) | ||
122 | { | ||
123 | throw new NotImplementedException(); | ||
124 | } | ||
125 | |||
126 | |||
127 | public override bool Equals(object obj) | ||
128 | { | ||
129 | throw new NotImplementedException(); | ||
130 | } | ||
131 | |||
132 | |||
133 | public bool Equals(Quaternion other) | ||
134 | { | ||
135 | throw new NotImplementedException(); | ||
136 | } | ||
137 | |||
138 | |||
139 | public override int GetHashCode() | ||
140 | { | ||
141 | throw new NotImplementedException(); | ||
142 | } | ||
143 | |||
144 | |||
145 | public static Quaternion Inverse(Quaternion quaternion) | ||
146 | { | ||
147 | throw new NotImplementedException(); | ||
148 | } | ||
149 | |||
150 | |||
151 | public static void Inverse(ref Quaternion quaternion, out Quaternion result) | ||
152 | { | ||
153 | throw new NotImplementedException(); | ||
154 | } | ||
155 | |||
156 | |||
157 | public float Length() | ||
158 | { | ||
159 | //--- | ||
160 | return (float)Math.Sqrt(Math.Pow(this.W, 2.0) + Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0) + Math.Pow(this.Z, 2.0)); | ||
161 | //--- | ||
162 | //throw new NotImplementedException(); | ||
163 | } | ||
164 | |||
165 | |||
166 | public float LengthSquared() | ||
167 | { | ||
168 | //--- | ||
169 | return (float)(Math.Pow(this.W, 2.0) + Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0) + Math.Pow(this.Z, 2.0)); | ||
170 | //--- | ||
171 | //throw new NotImplementedException(); | ||
172 | } | ||
173 | |||
174 | |||
175 | public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, float amount) | ||
176 | { | ||
177 | throw new NotImplementedException(); | ||
178 | } | ||
179 | |||
180 | |||
181 | public static void Lerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result) | ||
182 | { | ||
183 | throw new NotImplementedException(); | ||
184 | } | ||
185 | |||
186 | |||
187 | public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float amount) | ||
188 | { | ||
189 | throw new NotImplementedException(); | ||
190 | } | ||
191 | |||
192 | |||
193 | public static void Slerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result) | ||
194 | { | ||
195 | throw new NotImplementedException(); | ||
196 | } | ||
197 | |||
198 | |||
199 | public static Quaternion Subtract(Quaternion quaternion1, Quaternion quaternion2) | ||
200 | { | ||
201 | throw new NotImplementedException(); | ||
202 | } | ||
203 | |||
204 | |||
205 | public static void Subtract(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) | ||
206 | { | ||
207 | throw new NotImplementedException(); | ||
208 | } | ||
209 | |||
210 | |||
211 | public static Quaternion Multiply(Quaternion quaternion1, Quaternion quaternion2) | ||
212 | { | ||
213 | throw new NotImplementedException(); | ||
214 | } | ||
215 | |||
216 | |||
217 | public static Quaternion Multiply(Quaternion quaternion1, float scaleFactor) | ||
218 | { | ||
219 | throw new NotImplementedException(); | ||
220 | } | ||
221 | |||
222 | |||
223 | public static void Multiply(ref Quaternion quaternion1, float scaleFactor, out Quaternion result) | ||
224 | { | ||
225 | throw new NotImplementedException(); | ||
226 | } | ||
227 | |||
228 | |||
229 | public static void Multiply(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) | ||
230 | { | ||
231 | throw new NotImplementedException(); | ||
232 | } | ||
233 | |||
234 | |||
235 | public static Quaternion Negate(Quaternion quaternion) | ||
236 | { | ||
237 | throw new NotImplementedException(); | ||
238 | } | ||
239 | |||
240 | |||
241 | public static void Negate(ref Quaternion quaternion, out Quaternion result) | ||
242 | { | ||
243 | throw new NotImplementedException(); | ||
244 | } | ||
245 | |||
246 | |||
247 | public void Normalize() | ||
248 | { | ||
249 | //--- | ||
250 | this = Normalize(this); | ||
251 | //--- | ||
252 | //throw new NotImplementedException(); | ||
253 | } | ||
254 | |||
255 | |||
256 | public static Quaternion Normalize(Quaternion quaternion) | ||
257 | { | ||
258 | //--- | ||
259 | return quaternion / quaternion.Length(); | ||
260 | //--- | ||
261 | //throw new NotImplementedException(); | ||
262 | } | ||
263 | |||
264 | |||
265 | public static void Normalize(ref Quaternion quaternion, out Quaternion result) | ||
266 | { | ||
267 | throw new NotImplementedException(); | ||
268 | } | ||
269 | |||
270 | |||
271 | public static Quaternion operator +(Quaternion quaternion1, Quaternion quaternion2) | ||
272 | { | ||
273 | throw new NotImplementedException(); | ||
274 | } | ||
275 | |||
276 | |||
277 | public static Quaternion operator /(Quaternion quaternion1, Quaternion quaternion2) | ||
278 | { | ||
279 | throw new NotImplementedException(); | ||
280 | } | ||
281 | public static Quaternion operator /(Quaternion quaternion, float factor) | ||
282 | { | ||
283 | quaternion.W /= factor; | ||
284 | quaternion.X /= factor; | ||
285 | quaternion.Y /= factor; | ||
286 | quaternion.Z /= factor; | ||
287 | return quaternion; | ||
288 | } | ||
289 | |||
290 | public static bool operator ==(Quaternion quaternion1, Quaternion quaternion2) | ||
291 | { | ||
292 | throw new NotImplementedException(); | ||
293 | } | ||
294 | |||
295 | |||
296 | public static bool operator !=(Quaternion quaternion1, Quaternion quaternion2) | ||
297 | { | ||
298 | throw new NotImplementedException(); | ||
299 | } | ||
300 | |||
301 | |||
302 | public static Quaternion operator *(Quaternion quaternion1, Quaternion quaternion2) | ||
303 | { | ||
304 | //--- | ||
305 | //Grassmann product | ||
306 | Quaternion quaternionProduct = new Quaternion(); | ||
307 | |||
308 | quaternionProduct.W = quaternion1.W * quaternion2.W - quaternion1.X * quaternion2.X - quaternion1.Y * quaternion2.Y - quaternion1.Z * quaternion2.Z; | ||
309 | quaternionProduct.X = quaternion1.W * quaternion2.X + quaternion1.X * quaternion2.W + quaternion1.Y * quaternion2.Z - quaternion1.Z * quaternion2.Y; | ||
310 | quaternionProduct.Y = quaternion1.W * quaternion2.Y - quaternion1.X * quaternion2.Z + quaternion1.Y * quaternion2.W + quaternion1.Z * quaternion2.X; | ||
311 | quaternionProduct.Z = quaternion1.W * quaternion2.Z + quaternion1.X * quaternion2.Y - quaternion1.Y * quaternion2.X + quaternion1.Z * quaternion2.W; | ||
312 | return quaternionProduct; | ||
313 | //--- | ||
314 | //throw new NotImplementedException(); | ||
315 | } | ||
316 | |||
317 | |||
318 | public static Quaternion operator *(Quaternion quaternion1, float scaleFactor) | ||
319 | { | ||
320 | throw new NotImplementedException(); | ||
321 | } | ||
322 | |||
323 | |||
324 | public static Quaternion operator -(Quaternion quaternion1, Quaternion quaternion2) | ||
325 | { | ||
326 | throw new NotImplementedException(); | ||
327 | } | ||
328 | |||
329 | |||
330 | public static Quaternion operator -(Quaternion quaternion) | ||
331 | { | ||
332 | throw new NotImplementedException(); | ||
333 | } | ||
334 | |||
335 | |||
336 | public override string ToString() | ||
337 | { | ||
338 | throw new NotImplementedException(); | ||
339 | } | ||
340 | |||
341 | private static void Conjugate(ref Quaternion quaternion, out Quaternion result) | ||
342 | { | ||
343 | throw new NotImplementedException(); | ||
344 | } | ||
345 | } | ||
346 | } | ||
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs new file mode 100644 index 0000000..fd4bcd2 --- /dev/null +++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs | |||
@@ -0,0 +1,620 @@ | |||
1 | #region License | ||
2 | /* | ||
3 | MIT License | ||
4 | Copyright © 2006 The Mono.Xna Team | ||
5 | |||
6 | All rights reserved. | ||
7 | |||
8 | Authors: | ||
9 | * Alan McGovern | ||
10 | |||
11 | Permission is hereby granted, free of charge, to any person obtaining a copy | ||
12 | of this software and associated documentation files (the "Software"), to deal | ||
13 | in the Software without restriction, including without limitation the rights | ||
14 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
15 | copies of the Software, and to permit persons to whom the Software is | ||
16 | furnished to do so, subject to the following conditions: | ||
17 | |||
18 | The above copyright notice and this permission notice shall be included in all | ||
19 | copies or substantial portions of the Software. | ||
20 | |||
21 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
22 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
23 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
24 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
25 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
26 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE | ||
27 | SOFTWARE. | ||
28 | */ | ||
29 | #endregion License | ||
30 | |||
31 | using System; | ||
32 | using System.ComponentModel; | ||
33 | using System.Text; | ||
34 | using System.Runtime.InteropServices; | ||
35 | |||
36 | namespace MonoXnaCompactMaths | ||
37 | { | ||
38 | [Serializable] | ||
39 | [StructLayout(LayoutKind.Sequential)] | ||
40 | //[TypeConverter(typeof(Vector3Converter))] | ||
41 | public struct Vector3 : IEquatable<Vector3> | ||
42 | { | ||
43 | #region Private Fields | ||
44 | |||
45 | private static Vector3 zero = new Vector3(0f, 0f, 0f); | ||
46 | private static Vector3 one = new Vector3(1f, 1f, 1f); | ||
47 | private static Vector3 unitX = new Vector3(1f, 0f, 0f); | ||
48 | private static Vector3 unitY = new Vector3(0f, 1f, 0f); | ||
49 | private static Vector3 unitZ = new Vector3(0f, 0f, 1f); | ||
50 | private static Vector3 up = new Vector3(0f, 1f, 0f); | ||
51 | private static Vector3 down = new Vector3(0f, -1f, 0f); | ||
52 | private static Vector3 right = new Vector3(1f, 0f, 0f); | ||
53 | private static Vector3 left = new Vector3(-1f, 0f, 0f); | ||
54 | private static Vector3 forward = new Vector3(0f, 0f, -1f); | ||
55 | private static Vector3 backward = new Vector3(0f, 0f, 1f); | ||
56 | |||
57 | #endregion Private Fields | ||
58 | |||
59 | |||
60 | #region Public Fields | ||
61 | |||
62 | public float X; | ||
63 | public float Y; | ||
64 | public float Z; | ||
65 | |||
66 | #endregion Public Fields | ||
67 | |||
68 | |||
69 | #region Properties | ||
70 | |||
71 | public static Vector3 Zero | ||
72 | { | ||
73 | get { return zero; } | ||
74 | } | ||
75 | |||
76 | public static Vector3 One | ||
77 | { | ||
78 | get { return one; } | ||
79 | } | ||
80 | |||
81 | public static Vector3 UnitX | ||
82 | { | ||
83 | get { return unitX; } | ||
84 | } | ||
85 | |||
86 | public static Vector3 UnitY | ||
87 | { | ||
88 | get { return unitY; } | ||
89 | } | ||
90 | |||
91 | public static Vector3 UnitZ | ||
92 | { | ||
93 | get { return unitZ; } | ||
94 | } | ||
95 | |||
96 | public static Vector3 Up | ||
97 | { | ||
98 | get { return up; } | ||
99 | } | ||
100 | |||
101 | public static Vector3 Down | ||
102 | { | ||
103 | get { return down; } | ||
104 | } | ||
105 | |||
106 | public static Vector3 Right | ||
107 | { | ||
108 | get { return right; } | ||
109 | } | ||
110 | |||
111 | public static Vector3 Left | ||
112 | { | ||
113 | get { return left; } | ||
114 | } | ||
115 | |||
116 | public static Vector3 Forward | ||
117 | { | ||
118 | get { return forward; } | ||
119 | } | ||
120 | |||
121 | public static Vector3 Backward | ||
122 | { | ||
123 | get { return backward; } | ||
124 | } | ||
125 | |||
126 | #endregion Properties | ||
127 | |||
128 | |||
129 | #region Constructors | ||
130 | |||
131 | public Vector3(float x, float y, float z) | ||
132 | { | ||
133 | this.X = x; | ||
134 | this.Y = y; | ||
135 | this.Z = z; | ||
136 | } | ||
137 | |||
138 | |||
139 | public Vector3(float value) | ||
140 | { | ||
141 | this.X = value; | ||
142 | this.Y = value; | ||
143 | this.Z = value; | ||
144 | } | ||
145 | |||
146 | |||
147 | /*public Vector3(Vector2 value, float z) | ||
148 | { | ||
149 | this.X = value.X; | ||
150 | this.Y = value.Y; | ||
151 | this.Z = z; | ||
152 | }*/ | ||
153 | |||
154 | |||
155 | #endregion Constructors | ||
156 | |||
157 | |||
158 | #region Public Methods | ||
159 | |||
160 | public static Vector3 Add(Vector3 value1, Vector3 value2) | ||
161 | { | ||
162 | value1.X += value2.X; | ||
163 | value1.Y += value2.Y; | ||
164 | value1.Z += value2.Z; | ||
165 | return value1; | ||
166 | } | ||
167 | |||
168 | public static void Add(ref Vector3 value1, ref Vector3 value2, out Vector3 result) | ||
169 | { | ||
170 | result.X = value1.X + value2.X; | ||
171 | result.Y = value1.Y + value2.Y; | ||
172 | result.Z = value1.Z + value2.Z; | ||
173 | } | ||
174 | |||
175 | /*public static Vector3 Barycentric(Vector3 value1, Vector3 value2, Vector3 value3, float amount1, float amount2) | ||
176 | { | ||
177 | return new Vector3( | ||
178 | MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), | ||
179 | MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), | ||
180 | MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2)); | ||
181 | }*/ | ||
182 | |||
183 | /*public static void Barycentric(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, float amount1, float amount2, out Vector3 result) | ||
184 | { | ||
185 | result = new Vector3( | ||
186 | MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), | ||
187 | MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), | ||
188 | MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2)); | ||
189 | }*/ | ||
190 | |||
191 | /*public static Vector3 CatmullRom(Vector3 value1, Vector3 value2, Vector3 value3, Vector3 value4, float amount) | ||
192 | { | ||
193 | return new Vector3( | ||
194 | MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), | ||
195 | MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount), | ||
196 | MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount)); | ||
197 | }*/ | ||
198 | |||
199 | /*public static void CatmullRom(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, ref Vector3 value4, float amount, out Vector3 result) | ||
200 | { | ||
201 | result = new Vector3( | ||
202 | MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), | ||
203 | MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount), | ||
204 | MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount)); | ||
205 | }*/ | ||
206 | |||
207 | /*public static Vector3 Clamp(Vector3 value1, Vector3 min, Vector3 max) | ||
208 | { | ||
209 | return new Vector3( | ||
210 | MathHelper.Clamp(value1.X, min.X, max.X), | ||
211 | MathHelper.Clamp(value1.Y, min.Y, max.Y), | ||
212 | MathHelper.Clamp(value1.Z, min.Z, max.Z)); | ||
213 | }*/ | ||
214 | |||
215 | /*public static void Clamp(ref Vector3 value1, ref Vector3 min, ref Vector3 max, out Vector3 result) | ||
216 | { | ||
217 | result = new Vector3( | ||
218 | MathHelper.Clamp(value1.X, min.X, max.X), | ||
219 | MathHelper.Clamp(value1.Y, min.Y, max.Y), | ||
220 | MathHelper.Clamp(value1.Z, min.Z, max.Z)); | ||
221 | }*/ | ||
222 | |||
223 | public static Vector3 Cross(Vector3 vector1, Vector3 vector2) | ||
224 | { | ||
225 | Cross(ref vector1, ref vector2, out vector1); | ||
226 | return vector1; | ||
227 | } | ||
228 | |||
229 | public static void Cross(ref Vector3 vector1, ref Vector3 vector2, out Vector3 result) | ||
230 | { | ||
231 | result = new Vector3(vector1.Y * vector2.Z - vector2.Y * vector1.Z, | ||
232 | -(vector1.X * vector2.Z - vector2.X * vector1.Z), | ||
233 | vector1.X * vector2.Y - vector2.X * vector1.Y); | ||
234 | } | ||
235 | |||
236 | public static float Distance(Vector3 vector1, Vector3 vector2) | ||
237 | { | ||
238 | float result; | ||
239 | DistanceSquared(ref vector1, ref vector2, out result); | ||
240 | return (float)Math.Sqrt(result); | ||
241 | } | ||
242 | |||
243 | public static void Distance(ref Vector3 value1, ref Vector3 value2, out float result) | ||
244 | { | ||
245 | DistanceSquared(ref value1, ref value2, out result); | ||
246 | result = (float)Math.Sqrt(result); | ||
247 | } | ||
248 | |||
249 | public static float DistanceSquared(Vector3 value1, Vector3 value2) | ||
250 | { | ||
251 | float result; | ||
252 | DistanceSquared(ref value1, ref value2, out result); | ||
253 | return result; | ||
254 | } | ||
255 | |||
256 | public static void DistanceSquared(ref Vector3 value1, ref Vector3 value2, out float result) | ||
257 | { | ||
258 | result = (value1.X - value2.X) * (value1.X - value2.X) + | ||
259 | (value1.Y - value2.Y) * (value1.Y - value2.Y) + | ||
260 | (value1.Z - value2.Z) * (value1.Z - value2.Z); | ||
261 | } | ||
262 | |||
263 | public static Vector3 Divide(Vector3 value1, Vector3 value2) | ||
264 | { | ||
265 | value1.X /= value2.X; | ||
266 | value1.Y /= value2.Y; | ||
267 | value1.Z /= value2.Z; | ||
268 | return value1; | ||
269 | } | ||
270 | |||
271 | public static Vector3 Divide(Vector3 value1, float value2) | ||
272 | { | ||
273 | float factor = 1 / value2; | ||
274 | value1.X *= factor; | ||
275 | value1.Y *= factor; | ||
276 | value1.Z *= factor; | ||
277 | return value1; | ||
278 | } | ||
279 | |||
280 | public static void Divide(ref Vector3 value1, float divisor, out Vector3 result) | ||
281 | { | ||
282 | float factor = 1 / divisor; | ||
283 | result.X = value1.X * factor; | ||
284 | result.Y = value1.Y * factor; | ||
285 | result.Z = value1.Z * factor; | ||
286 | } | ||
287 | |||
288 | public static void Divide(ref Vector3 value1, ref Vector3 value2, out Vector3 result) | ||
289 | { | ||
290 | result.X = value1.X / value2.X; | ||
291 | result.Y = value1.Y / value2.Y; | ||
292 | result.Z = value1.Z / value2.Z; | ||
293 | } | ||
294 | |||
295 | public static float Dot(Vector3 vector1, Vector3 vector2) | ||
296 | { | ||
297 | return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z; | ||
298 | } | ||
299 | |||
300 | public static void Dot(ref Vector3 vector1, ref Vector3 vector2, out float result) | ||
301 | { | ||
302 | result = vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z; | ||
303 | } | ||
304 | |||
305 | public override bool Equals(object obj) | ||
306 | { | ||
307 | return (obj is Vector3) ? this == (Vector3)obj : false; | ||
308 | } | ||
309 | |||
310 | public bool Equals(Vector3 other) | ||
311 | { | ||
312 | return this == other; | ||
313 | } | ||
314 | |||
315 | public override int GetHashCode() | ||
316 | { | ||
317 | return (int)(this.X + this.Y + this.Z); | ||
318 | } | ||
319 | |||
320 | /*public static Vector3 Hermite(Vector3 value1, Vector3 tangent1, Vector3 value2, Vector3 tangent2, float amount) | ||
321 | { | ||
322 | Vector3 result = new Vector3(); | ||
323 | Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result); | ||
324 | return result; | ||
325 | }*/ | ||
326 | |||
327 | /*public static void Hermite(ref Vector3 value1, ref Vector3 tangent1, ref Vector3 value2, ref Vector3 tangent2, float amount, out Vector3 result) | ||
328 | { | ||
329 | result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount); | ||
330 | result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount); | ||
331 | result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount); | ||
332 | }*/ | ||
333 | |||
334 | public float Length() | ||
335 | { | ||
336 | float result; | ||
337 | DistanceSquared(ref this, ref zero, out result); | ||
338 | return (float)Math.Sqrt(result); | ||
339 | } | ||
340 | |||
341 | public float LengthSquared() | ||
342 | { | ||
343 | float result; | ||
344 | DistanceSquared(ref this, ref zero, out result); | ||
345 | return result; | ||
346 | } | ||
347 | |||
348 | /*public static Vector3 Lerp(Vector3 value1, Vector3 value2, float amount) | ||
349 | { | ||
350 | return new Vector3( | ||
351 | MathHelper.Lerp(value1.X, value2.X, amount), | ||
352 | MathHelper.Lerp(value1.Y, value2.Y, amount), | ||
353 | MathHelper.Lerp(value1.Z, value2.Z, amount)); | ||
354 | }*/ | ||
355 | |||
356 | /*public static void Lerp(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result) | ||
357 | { | ||
358 | result = new Vector3( | ||
359 | MathHelper.Lerp(value1.X, value2.X, amount), | ||
360 | MathHelper.Lerp(value1.Y, value2.Y, amount), | ||
361 | MathHelper.Lerp(value1.Z, value2.Z, amount)); | ||
362 | }*/ | ||
363 | |||
364 | /*public static Vector3 Max(Vector3 value1, Vector3 value2) | ||
365 | { | ||
366 | return new Vector3( | ||
367 | MathHelper.Max(value1.X, value2.X), | ||
368 | MathHelper.Max(value1.Y, value2.Y), | ||
369 | MathHelper.Max(value1.Z, value2.Z)); | ||
370 | }*/ | ||
371 | |||
372 | /*public static void Max(ref Vector3 value1, ref Vector3 value2, out Vector3 result) | ||
373 | { | ||
374 | result = new Vector3( | ||
375 | MathHelper.Max(value1.X, value2.X), | ||
376 | MathHelper.Max(value1.Y, value2.Y), | ||
377 | MathHelper.Max(value1.Z, value2.Z)); | ||
378 | }*/ | ||
379 | |||
380 | /*public static Vector3 Min(Vector3 value1, Vector3 value2) | ||
381 | { | ||
382 | return new Vector3( | ||
383 | MathHelper.Min(value1.X, value2.X), | ||
384 | MathHelper.Min(value1.Y, value2.Y), | ||
385 | MathHelper.Min(value1.Z, value2.Z)); | ||
386 | }*/ | ||
387 | |||
388 | /*public static void Min(ref Vector3 value1, ref Vector3 value2, out Vector3 result) | ||
389 | { | ||
390 | result = new Vector3( | ||
391 | MathHelper.Min(value1.X, value2.X), | ||
392 | MathHelper.Min(value1.Y, value2.Y), | ||
393 | MathHelper.Min(value1.Z, value2.Z)); | ||
394 | }*/ | ||
395 | |||
396 | public static Vector3 Multiply(Vector3 value1, Vector3 value2) | ||
397 | { | ||
398 | value1.X *= value2.X; | ||
399 | value1.Y *= value2.Y; | ||
400 | value1.Z *= value2.Z; | ||
401 | return value1; | ||
402 | } | ||
403 | |||
404 | public static Vector3 Multiply(Vector3 value1, float scaleFactor) | ||
405 | { | ||
406 | value1.X *= scaleFactor; | ||
407 | value1.Y *= scaleFactor; | ||
408 | value1.Z *= scaleFactor; | ||
409 | return value1; | ||
410 | } | ||
411 | |||
412 | public static void Multiply(ref Vector3 value1, float scaleFactor, out Vector3 result) | ||
413 | { | ||
414 | result.X = value1.X * scaleFactor; | ||
415 | result.Y = value1.Y * scaleFactor; | ||
416 | result.Z = value1.Z * scaleFactor; | ||
417 | } | ||
418 | |||
419 | public static void Multiply(ref Vector3 value1, ref Vector3 value2, out Vector3 result) | ||
420 | { | ||
421 | result.X = value1.X * value2.X; | ||
422 | result.Y = value1.Y * value2.Y; | ||
423 | result.Z = value1.Z * value2.Z; | ||
424 | } | ||
425 | |||
426 | public static Vector3 Negate(Vector3 value) | ||
427 | { | ||
428 | value = new Vector3(-value.X, -value.Y, -value.Z); | ||
429 | return value; | ||
430 | } | ||
431 | |||
432 | public static void Negate(ref Vector3 value, out Vector3 result) | ||
433 | { | ||
434 | result = new Vector3(-value.X, -value.Y, -value.Z); | ||
435 | } | ||
436 | |||
437 | public void Normalize() | ||
438 | { | ||
439 | Normalize(ref this, out this); | ||
440 | } | ||
441 | |||
442 | public static Vector3 Normalize(Vector3 vector) | ||
443 | { | ||
444 | Normalize(ref vector, out vector); | ||
445 | return vector; | ||
446 | } | ||
447 | |||
448 | public static void Normalize(ref Vector3 value, out Vector3 result) | ||
449 | { | ||
450 | float factor; | ||
451 | Distance(ref value, ref zero, out factor); | ||
452 | factor = 1f / factor; | ||
453 | result.X = value.X * factor; | ||
454 | result.Y = value.Y * factor; | ||
455 | result.Z = value.Z * factor; | ||
456 | } | ||
457 | |||
458 | public static Vector3 Reflect(Vector3 vector, Vector3 normal) | ||
459 | { | ||
460 | throw new NotImplementedException(); | ||
461 | } | ||
462 | |||
463 | public static void Reflect(ref Vector3 vector, ref Vector3 normal, out Vector3 result) | ||
464 | { | ||
465 | throw new NotImplementedException(); | ||
466 | } | ||
467 | |||
468 | /*public static Vector3 SmoothStep(Vector3 value1, Vector3 value2, float amount) | ||
469 | { | ||
470 | return new Vector3( | ||
471 | MathHelper.SmoothStep(value1.X, value2.X, amount), | ||
472 | MathHelper.SmoothStep(value1.Y, value2.Y, amount), | ||
473 | MathHelper.SmoothStep(value1.Z, value2.Z, amount)); | ||
474 | }*/ | ||
475 | |||
476 | /*public static void SmoothStep(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result) | ||
477 | { | ||
478 | result = new Vector3( | ||
479 | MathHelper.SmoothStep(value1.X, value2.X, amount), | ||
480 | MathHelper.SmoothStep(value1.Y, value2.Y, amount), | ||
481 | MathHelper.SmoothStep(value1.Z, value2.Z, amount)); | ||
482 | }*/ | ||
483 | |||
484 | public static Vector3 Subtract(Vector3 value1, Vector3 value2) | ||
485 | { | ||
486 | value1.X -= value2.X; | ||
487 | value1.Y -= value2.Y; | ||
488 | value1.Z -= value2.Z; | ||
489 | return value1; | ||
490 | } | ||
491 | |||
492 | public static void Subtract(ref Vector3 value1, ref Vector3 value2, out Vector3 result) | ||
493 | { | ||
494 | result.X = value1.X - value2.X; | ||
495 | result.Y = value1.Y - value2.Y; | ||
496 | result.Z = value1.Z - value2.Z; | ||
497 | } | ||
498 | |||
499 | public override string ToString() | ||
500 | { | ||
501 | StringBuilder sb = new StringBuilder(32); | ||
502 | sb.Append("{X:"); | ||
503 | sb.Append(this.X); | ||
504 | sb.Append(" Y:"); | ||
505 | sb.Append(this.Y); | ||
506 | sb.Append(" Z:"); | ||
507 | sb.Append(this.Z); | ||
508 | sb.Append("}"); | ||
509 | return sb.ToString(); | ||
510 | } | ||
511 | |||
512 | public static Vector3 Transform(Vector3 position, Matrix matrix) | ||
513 | { | ||
514 | Transform(ref position, ref matrix, out position); | ||
515 | return position; | ||
516 | } | ||
517 | |||
518 | public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector3 result) | ||
519 | { | ||
520 | result = new Vector3((position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41, | ||
521 | (position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32) + matrix.M42, | ||
522 | (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43); | ||
523 | } | ||
524 | |||
525 | public static Vector3 TransformNormal(Vector3 normal, Matrix matrix) | ||
526 | { | ||
527 | TransformNormal(ref normal, ref matrix, out normal); | ||
528 | return normal; | ||
529 | } | ||
530 | |||
531 | public static void TransformNormal(ref Vector3 normal, ref Matrix matrix, out Vector3 result) | ||
532 | { | ||
533 | result = new Vector3((normal.X * matrix.M11) + (normal.Y * matrix.M21) + (normal.Z * matrix.M31), | ||
534 | (normal.X * matrix.M12) + (normal.Y * matrix.M22) + (normal.Z * matrix.M32), | ||
535 | (normal.X * matrix.M13) + (normal.Y * matrix.M23) + (normal.Z * matrix.M33)); | ||
536 | } | ||
537 | |||
538 | #endregion Public methods | ||
539 | |||
540 | |||
541 | #region Operators | ||
542 | |||
543 | public static bool operator ==(Vector3 value1, Vector3 value2) | ||
544 | { | ||
545 | return value1.X == value2.X | ||
546 | && value1.Y == value2.Y | ||
547 | && value1.Z == value2.Z; | ||
548 | } | ||
549 | |||
550 | public static bool operator !=(Vector3 value1, Vector3 value2) | ||
551 | { | ||
552 | return !(value1 == value2); | ||
553 | } | ||
554 | |||
555 | public static Vector3 operator +(Vector3 value1, Vector3 value2) | ||
556 | { | ||
557 | value1.X += value2.X; | ||
558 | value1.Y += value2.Y; | ||
559 | value1.Z += value2.Z; | ||
560 | return value1; | ||
561 | } | ||
562 | |||
563 | public static Vector3 operator -(Vector3 value) | ||
564 | { | ||
565 | value = new Vector3(-value.X, -value.Y, -value.Z); | ||
566 | return value; | ||
567 | } | ||
568 | |||
569 | public static Vector3 operator -(Vector3 value1, Vector3 value2) | ||
570 | { | ||
571 | value1.X -= value2.X; | ||
572 | value1.Y -= value2.Y; | ||
573 | value1.Z -= value2.Z; | ||
574 | return value1; | ||
575 | } | ||
576 | |||
577 | public static Vector3 operator *(Vector3 value1, Vector3 value2) | ||
578 | { | ||
579 | value1.X *= value2.X; | ||
580 | value1.Y *= value2.Y; | ||
581 | value1.Z *= value2.Z; | ||
582 | return value1; | ||
583 | } | ||
584 | |||
585 | public static Vector3 operator *(Vector3 value, float scaleFactor) | ||
586 | { | ||
587 | value.X *= scaleFactor; | ||
588 | value.Y *= scaleFactor; | ||
589 | value.Z *= scaleFactor; | ||
590 | return value; | ||
591 | } | ||
592 | |||
593 | public static Vector3 operator *(float scaleFactor, Vector3 value) | ||
594 | { | ||
595 | value.X *= scaleFactor; | ||
596 | value.Y *= scaleFactor; | ||
597 | value.Z *= scaleFactor; | ||
598 | return value; | ||
599 | } | ||
600 | |||
601 | public static Vector3 operator /(Vector3 value1, Vector3 value2) | ||
602 | { | ||
603 | value1.X /= value2.X; | ||
604 | value1.Y /= value2.Y; | ||
605 | value1.Z /= value2.Z; | ||
606 | return value1; | ||
607 | } | ||
608 | |||
609 | public static Vector3 operator /(Vector3 value, float divider) | ||
610 | { | ||
611 | float factor = 1 / divider; | ||
612 | value.X *= factor; | ||
613 | value.Y *= factor; | ||
614 | value.Z *= factor; | ||
615 | return value; | ||
616 | } | ||
617 | |||
618 | #endregion | ||
619 | } | ||
620 | } | ||
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs new file mode 100644 index 0000000..c75b7c8 --- /dev/null +++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs | |||
@@ -0,0 +1,630 @@ | |||
1 | #region License | ||
2 | /* | ||
3 | MIT License | ||
4 | Copyright © 2006 The Mono.Xna Team | ||
5 | |||
6 | All rights reserved. | ||
7 | |||
8 | Permission is hereby granted, free of charge, to any person obtaining a copy | ||
9 | of this software and associated documentation files (the "Software"), to deal | ||
10 | in the Software without restriction, including without limitation the rights | ||
11 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
12 | copies of the Software, and to permit persons to whom the Software is | ||
13 | furnished to do so, subject to the following conditions: | ||
14 | |||
15 | The above copyright notice and this permission notice shall be included in all | ||
16 | copies or substantial portions of the Software. | ||
17 | |||
18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
19 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
20 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
21 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
22 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
23 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE | ||
24 | SOFTWARE. | ||
25 | */ | ||
26 | #endregion License | ||
27 | |||
28 | using System; | ||
29 | using System.ComponentModel; | ||
30 | using System.Text; | ||
31 | using System.Runtime.InteropServices; | ||
32 | |||
33 | namespace MonoXnaCompactMaths | ||
34 | { | ||
35 | [Serializable] | ||
36 | [StructLayout(LayoutKind.Sequential)] | ||
37 | //[TypeConverter(typeof(Vector4Converter))] | ||
38 | public struct Vector4 : IEquatable<Vector4> | ||
39 | { | ||
40 | #region Private Fields | ||
41 | |||
42 | private static Vector4 zeroVector = new Vector4(); | ||
43 | private static Vector4 unitVector = new Vector4(1f, 1f, 1f, 1f); | ||
44 | private static Vector4 unitXVector = new Vector4(1f, 0f, 0f, 0f); | ||
45 | private static Vector4 unitYVector = new Vector4(0f, 1f, 0f, 0f); | ||
46 | private static Vector4 unitZVector = new Vector4(0f, 0f, 1f, 0f); | ||
47 | private static Vector4 unitWVector = new Vector4(0f, 0f, 0f, 1f); | ||
48 | |||
49 | #endregion Private Fields | ||
50 | |||
51 | |||
52 | #region Public Fields | ||
53 | |||
54 | public float X; | ||
55 | public float Y; | ||
56 | public float Z; | ||
57 | public float W; | ||
58 | |||
59 | #endregion Public Fields | ||
60 | |||
61 | |||
62 | #region Properties | ||
63 | |||
64 | public static Vector4 Zero | ||
65 | { | ||
66 | get { return zeroVector; } | ||
67 | } | ||
68 | |||
69 | public static Vector4 One | ||
70 | { | ||
71 | get { return unitVector; } | ||
72 | } | ||
73 | |||
74 | public static Vector4 UnitX | ||
75 | { | ||
76 | get { return unitXVector; } | ||
77 | } | ||
78 | |||
79 | public static Vector4 UnitY | ||
80 | { | ||
81 | get { return unitYVector; } | ||
82 | } | ||
83 | |||
84 | public static Vector4 UnitZ | ||
85 | { | ||
86 | get { return unitZVector; } | ||
87 | } | ||
88 | |||
89 | public static Vector4 UnitW | ||
90 | { | ||
91 | get { return unitWVector; } | ||
92 | } | ||
93 | |||
94 | #endregion Properties | ||
95 | |||
96 | |||
97 | #region Constructors | ||
98 | |||
99 | public Vector4(float x, float y, float z, float w) | ||
100 | { | ||
101 | this.X = x; | ||
102 | this.Y = y; | ||
103 | this.Z = z; | ||
104 | this.W = w; | ||
105 | } | ||
106 | |||
107 | /*public Vector4(Vector2 value, float z, float w) | ||
108 | { | ||
109 | this.X = value.X; | ||
110 | this.Y = value.Y; | ||
111 | this.Z = z; | ||
112 | this.W = w; | ||
113 | }*/ | ||
114 | |||
115 | public Vector4(Vector3 value, float w) | ||
116 | { | ||
117 | this.X = value.X; | ||
118 | this.Y = value.Y; | ||
119 | this.Z = value.Z; | ||
120 | this.W = w; | ||
121 | } | ||
122 | |||
123 | public Vector4(float value) | ||
124 | { | ||
125 | this.X = value; | ||
126 | this.Y = value; | ||
127 | this.Z = value; | ||
128 | this.W = value; | ||
129 | } | ||
130 | |||
131 | #endregion | ||
132 | |||
133 | |||
134 | #region Public Methods | ||
135 | |||
136 | public static Vector4 Add(Vector4 value1, Vector4 value2) | ||
137 | { | ||
138 | value1.W += value2.W; | ||
139 | value1.X += value2.X; | ||
140 | value1.Y += value2.Y; | ||
141 | value1.Z += value2.Z; | ||
142 | return value1; | ||
143 | } | ||
144 | |||
145 | public static void Add(ref Vector4 value1, ref Vector4 value2, out Vector4 result) | ||
146 | { | ||
147 | result.W = value1.W + value2.W; | ||
148 | result.X = value1.X + value2.X; | ||
149 | result.Y = value1.Y + value2.Y; | ||
150 | result.Z = value1.Z + value2.Z; | ||
151 | } | ||
152 | |||
153 | /*public static Vector4 Barycentric(Vector4 value1, Vector4 value2, Vector4 value3, float amount1, float amount2) | ||
154 | { | ||
155 | return new Vector4( | ||
156 | MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), | ||
157 | MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), | ||
158 | MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2), | ||
159 | MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2)); | ||
160 | }*/ | ||
161 | |||
162 | /*public static void Barycentric(ref Vector4 value1, ref Vector4 value2, ref Vector4 value3, float amount1, float amount2, out Vector4 result) | ||
163 | { | ||
164 | result = new Vector4( | ||
165 | MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), | ||
166 | MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), | ||
167 | MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2), | ||
168 | MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2)); | ||
169 | }*/ | ||
170 | |||
171 | /*public static Vector4 CatmullRom(Vector4 value1, Vector4 value2, Vector4 value3, Vector4 value4, float amount) | ||
172 | { | ||
173 | return new Vector4( | ||
174 | MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), | ||
175 | MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount), | ||
176 | MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount), | ||
177 | MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount)); | ||
178 | }*/ | ||
179 | |||
180 | /*public static void CatmullRom(ref Vector4 value1, ref Vector4 value2, ref Vector4 value3, ref Vector4 value4, float amount, out Vector4 result) | ||
181 | { | ||
182 | result = new Vector4( | ||
183 | MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), | ||
184 | MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount), | ||
185 | MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount), | ||
186 | MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount)); | ||
187 | }*/ | ||
188 | |||
189 | /*public static Vector4 Clamp(Vector4 value1, Vector4 min, Vector4 max) | ||
190 | { | ||
191 | return new Vector4( | ||
192 | MathHelper.Clamp(value1.X, min.X, max.X), | ||
193 | MathHelper.Clamp(value1.Y, min.Y, max.Y), | ||
194 | MathHelper.Clamp(value1.Z, min.Z, max.Z), | ||
195 | MathHelper.Clamp(value1.W, min.W, max.W)); | ||
196 | }*/ | ||
197 | |||
198 | /*public static void Clamp(ref Vector4 value1, ref Vector4 min, ref Vector4 max, out Vector4 result) | ||
199 | { | ||
200 | result = new Vector4( | ||
201 | MathHelper.Clamp(value1.X, min.X, max.X), | ||
202 | MathHelper.Clamp(value1.Y, min.Y, max.Y), | ||
203 | MathHelper.Clamp(value1.Z, min.Z, max.Z), | ||
204 | MathHelper.Clamp(value1.W, min.W, max.W)); | ||
205 | }*/ | ||
206 | |||
207 | public static float Distance(Vector4 value1, Vector4 value2) | ||
208 | { | ||
209 | return (float)Math.Sqrt(DistanceSquared(value1, value2)); | ||
210 | } | ||
211 | |||
212 | public static void Distance(ref Vector4 value1, ref Vector4 value2, out float result) | ||
213 | { | ||
214 | result = (float)Math.Sqrt(DistanceSquared(value1, value2)); | ||
215 | } | ||
216 | |||
217 | public static float DistanceSquared(Vector4 value1, Vector4 value2) | ||
218 | { | ||
219 | float result; | ||
220 | DistanceSquared(ref value1, ref value2, out result); | ||
221 | return result; | ||
222 | } | ||
223 | |||
224 | public static void DistanceSquared(ref Vector4 value1, ref Vector4 value2, out float result) | ||
225 | { | ||
226 | result = (value1.W - value2.W) * (value1.W - value2.W) + | ||
227 | (value1.X - value2.X) * (value1.X - value2.X) + | ||
228 | (value1.Y - value2.Y) * (value1.Y - value2.Y) + | ||
229 | (value1.Z - value2.Z) * (value1.Z - value2.Z); | ||
230 | } | ||
231 | |||
232 | public static Vector4 Divide(Vector4 value1, Vector4 value2) | ||
233 | { | ||
234 | value1.W /= value2.W; | ||
235 | value1.X /= value2.X; | ||
236 | value1.Y /= value2.Y; | ||
237 | value1.Z /= value2.Z; | ||
238 | return value1; | ||
239 | } | ||
240 | |||
241 | public static Vector4 Divide(Vector4 value1, float divider) | ||
242 | { | ||
243 | float factor = 1f / divider; | ||
244 | value1.W *= factor; | ||
245 | value1.X *= factor; | ||
246 | value1.Y *= factor; | ||
247 | value1.Z *= factor; | ||
248 | return value1; | ||
249 | } | ||
250 | |||
251 | public static void Divide(ref Vector4 value1, float divider, out Vector4 result) | ||
252 | { | ||
253 | float factor = 1f / divider; | ||
254 | result.W = value1.W * factor; | ||
255 | result.X = value1.X * factor; | ||
256 | result.Y = value1.Y * factor; | ||
257 | result.Z = value1.Z * factor; | ||
258 | } | ||
259 | |||
260 | public static void Divide(ref Vector4 value1, ref Vector4 value2, out Vector4 result) | ||
261 | { | ||
262 | result.W = value1.W / value2.W; | ||
263 | result.X = value1.X / value2.X; | ||
264 | result.Y = value1.Y / value2.Y; | ||
265 | result.Z = value1.Z / value2.Z; | ||
266 | } | ||
267 | |||
268 | public static float Dot(Vector4 vector1, Vector4 vector2) | ||
269 | { | ||
270 | return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W; | ||
271 | } | ||
272 | |||
273 | public static void Dot(ref Vector4 vector1, ref Vector4 vector2, out float result) | ||
274 | { | ||
275 | result = vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W; | ||
276 | } | ||
277 | |||
278 | public override bool Equals(object obj) | ||
279 | { | ||
280 | return (obj is Vector4) ? this == (Vector4)obj : false; | ||
281 | } | ||
282 | |||
283 | public bool Equals(Vector4 other) | ||
284 | { | ||
285 | return this.W == other.W | ||
286 | && this.X == other.X | ||
287 | && this.Y == other.Y | ||
288 | && this.Z == other.Z; | ||
289 | } | ||
290 | |||
291 | public override int GetHashCode() | ||
292 | { | ||
293 | return (int)(this.W + this.X + this.Y + this.Y); | ||
294 | } | ||
295 | |||
296 | /*public static Vector4 Hermite(Vector4 value1, Vector4 tangent1, Vector4 value2, Vector4 tangent2, float amount) | ||
297 | { | ||
298 | Vector4 result = new Vector4(); | ||
299 | Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result); | ||
300 | return result; | ||
301 | }*/ | ||
302 | |||
303 | /*public static void Hermite(ref Vector4 value1, ref Vector4 tangent1, ref Vector4 value2, ref Vector4 tangent2, float amount, out Vector4 result) | ||
304 | { | ||
305 | result.W = MathHelper.Hermite(value1.W, tangent1.W, value2.W, tangent2.W, amount); | ||
306 | result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount); | ||
307 | result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount); | ||
308 | result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount); | ||
309 | }*/ | ||
310 | |||
311 | public float Length() | ||
312 | { | ||
313 | float result; | ||
314 | DistanceSquared(ref this, ref zeroVector, out result); | ||
315 | return (float)Math.Sqrt(result); | ||
316 | } | ||
317 | |||
318 | public float LengthSquared() | ||
319 | { | ||
320 | float result; | ||
321 | DistanceSquared(ref this, ref zeroVector, out result); | ||
322 | return result; | ||
323 | } | ||
324 | |||
325 | /*public static Vector4 Lerp(Vector4 value1, Vector4 value2, float amount) | ||
326 | { | ||
327 | return new Vector4( | ||
328 | MathHelper.Lerp(value1.X, value2.X, amount), | ||
329 | MathHelper.Lerp(value1.Y, value2.Y, amount), | ||
330 | MathHelper.Lerp(value1.Z, value2.Z, amount), | ||
331 | MathHelper.Lerp(value1.W, value2.W, amount)); | ||
332 | }*/ | ||
333 | |||
334 | /*public static void Lerp(ref Vector4 value1, ref Vector4 value2, float amount, out Vector4 result) | ||
335 | { | ||
336 | result = new Vector4( | ||
337 | MathHelper.Lerp(value1.X, value2.X, amount), | ||
338 | MathHelper.Lerp(value1.Y, value2.Y, amount), | ||
339 | MathHelper.Lerp(value1.Z, value2.Z, amount), | ||
340 | MathHelper.Lerp(value1.W, value2.W, amount)); | ||
341 | }*/ | ||
342 | |||
343 | /*public static Vector4 Max(Vector4 value1, Vector4 value2) | ||
344 | { | ||
345 | return new Vector4( | ||
346 | MathHelper.Max(value1.X, value2.X), | ||
347 | MathHelper.Max(value1.Y, value2.Y), | ||
348 | MathHelper.Max(value1.Z, value2.Z), | ||
349 | MathHelper.Max(value1.W, value2.W)); | ||
350 | }*/ | ||
351 | |||
352 | /*public static void Max(ref Vector4 value1, ref Vector4 value2, out Vector4 result) | ||
353 | { | ||
354 | result = new Vector4( | ||
355 | MathHelper.Max(value1.X, value2.X), | ||
356 | MathHelper.Max(value1.Y, value2.Y), | ||
357 | MathHelper.Max(value1.Z, value2.Z), | ||
358 | MathHelper.Max(value1.W, value2.W)); | ||
359 | }*/ | ||
360 | |||
361 | /*public static Vector4 Min(Vector4 value1, Vector4 value2) | ||
362 | { | ||
363 | return new Vector4( | ||
364 | MathHelper.Min(value1.X, value2.X), | ||
365 | MathHelper.Min(value1.Y, value2.Y), | ||
366 | MathHelper.Min(value1.Z, value2.Z), | ||
367 | MathHelper.Min(value1.W, value2.W)); | ||
368 | }*/ | ||
369 | |||
370 | /*public static void Min(ref Vector4 value1, ref Vector4 value2, out Vector4 result) | ||
371 | { | ||
372 | result = new Vector4( | ||
373 | MathHelper.Min(value1.X, value2.X), | ||
374 | MathHelper.Min(value1.Y, value2.Y), | ||
375 | MathHelper.Min(value1.Z, value2.Z), | ||
376 | MathHelper.Min(value1.W, value2.W)); | ||
377 | }*/ | ||
378 | |||
379 | public static Vector4 Multiply(Vector4 value1, Vector4 value2) | ||
380 | { | ||
381 | value1.W *= value2.W; | ||
382 | value1.X *= value2.X; | ||
383 | value1.Y *= value2.Y; | ||
384 | value1.Z *= value2.Z; | ||
385 | return value1; | ||
386 | } | ||
387 | |||
388 | public static Vector4 Multiply(Vector4 value1, float scaleFactor) | ||
389 | { | ||
390 | value1.W *= scaleFactor; | ||
391 | value1.X *= scaleFactor; | ||
392 | value1.Y *= scaleFactor; | ||
393 | value1.Z *= scaleFactor; | ||
394 | return value1; | ||
395 | } | ||
396 | |||
397 | public static void Multiply(ref Vector4 value1, float scaleFactor, out Vector4 result) | ||
398 | { | ||
399 | result.W = value1.W * scaleFactor; | ||
400 | result.X = value1.X * scaleFactor; | ||
401 | result.Y = value1.Y * scaleFactor; | ||
402 | result.Z = value1.Z * scaleFactor; | ||
403 | } | ||
404 | |||
405 | public static void Multiply(ref Vector4 value1, ref Vector4 value2, out Vector4 result) | ||
406 | { | ||
407 | result.W = value1.W * value2.W; | ||
408 | result.X = value1.X * value2.X; | ||
409 | result.Y = value1.Y * value2.Y; | ||
410 | result.Z = value1.Z * value2.Z; | ||
411 | } | ||
412 | |||
413 | public static Vector4 Negate(Vector4 value) | ||
414 | { | ||
415 | value = new Vector4(-value.X, -value.Y, -value.Z, -value.W); | ||
416 | return value; | ||
417 | } | ||
418 | |||
419 | public static void Negate(ref Vector4 value, out Vector4 result) | ||
420 | { | ||
421 | result = new Vector4(-value.X, -value.Y, -value.Z,-value.W); | ||
422 | } | ||
423 | |||
424 | public void Normalize() | ||
425 | { | ||
426 | Normalize(ref this, out this); | ||
427 | } | ||
428 | |||
429 | public static Vector4 Normalize(Vector4 vector) | ||
430 | { | ||
431 | Normalize(ref vector, out vector); | ||
432 | return vector; | ||
433 | } | ||
434 | |||
435 | public static void Normalize(ref Vector4 vector, out Vector4 result) | ||
436 | { | ||
437 | float factor; | ||
438 | DistanceSquared(ref vector, ref zeroVector, out factor); | ||
439 | factor = 1f / (float)Math.Sqrt(factor); | ||
440 | |||
441 | result.W = vector.W * factor; | ||
442 | result.X = vector.X * factor; | ||
443 | result.Y = vector.Y * factor; | ||
444 | result.Z = vector.Z * factor; | ||
445 | } | ||
446 | |||
447 | /*public static Vector4 SmoothStep(Vector4 value1, Vector4 value2, float amount) | ||
448 | { | ||
449 | return new Vector4( | ||
450 | MathHelper.SmoothStep(value1.X, value2.X, amount), | ||
451 | MathHelper.SmoothStep(value1.Y, value2.Y, amount), | ||
452 | MathHelper.SmoothStep(value1.Z, value2.Z, amount), | ||
453 | MathHelper.SmoothStep(value1.W, value2.W, amount)); | ||
454 | }*/ | ||
455 | |||
456 | /*public static void SmoothStep(ref Vector4 value1, ref Vector4 value2, float amount, out Vector4 result) | ||
457 | { | ||
458 | result = new Vector4( | ||
459 | MathHelper.SmoothStep(value1.X, value2.X, amount), | ||
460 | MathHelper.SmoothStep(value1.Y, value2.Y, amount), | ||
461 | MathHelper.SmoothStep(value1.Z, value2.Z, amount), | ||
462 | MathHelper.SmoothStep(value1.W, value2.W, amount)); | ||
463 | }*/ | ||
464 | |||
465 | public static Vector4 Subtract(Vector4 value1, Vector4 value2) | ||
466 | { | ||
467 | value1.W -= value2.W; | ||
468 | value1.X -= value2.X; | ||
469 | value1.Y -= value2.Y; | ||
470 | value1.Z -= value2.Z; | ||
471 | return value1; | ||
472 | } | ||
473 | |||
474 | public static void Subtract(ref Vector4 value1, ref Vector4 value2, out Vector4 result) | ||
475 | { | ||
476 | result.W = value1.W - value2.W; | ||
477 | result.X = value1.X - value2.X; | ||
478 | result.Y = value1.Y - value2.Y; | ||
479 | result.Z = value1.Z - value2.Z; | ||
480 | } | ||
481 | |||
482 | /*public static Vector4 Transform(Vector2 position, Matrix matrix) | ||
483 | { | ||
484 | Vector4 result; | ||
485 | Transform(ref position, ref matrix, out result); | ||
486 | return result; | ||
487 | }*/ | ||
488 | |||
489 | public static Vector4 Transform(Vector3 position, Matrix matrix) | ||
490 | { | ||
491 | Vector4 result; | ||
492 | Transform(ref position, ref matrix, out result); | ||
493 | return result; | ||
494 | } | ||
495 | |||
496 | public static Vector4 Transform(Vector4 vector, Matrix matrix) | ||
497 | { | ||
498 | Transform(ref vector, ref matrix, out vector); | ||
499 | return vector; | ||
500 | } | ||
501 | |||
502 | /*public static void Transform(ref Vector2 position, ref Matrix matrix, out Vector4 result) | ||
503 | { | ||
504 | result = new Vector4((position.X * matrix.M11) + (position.Y * matrix.M21) + matrix.M41, | ||
505 | (position.X * matrix.M12) + (position.Y * matrix.M22) + matrix.M42, | ||
506 | (position.X * matrix.M13) + (position.Y * matrix.M23) + matrix.M43, | ||
507 | (position.X * matrix.M14) + (position.Y * matrix.M24) + matrix.M44); | ||
508 | }*/ | ||
509 | |||
510 | public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector4 result) | ||
511 | { | ||
512 | result = new Vector4((position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41, | ||
513 | (position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32) + matrix.M42, | ||
514 | (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43, | ||
515 | (position.X * matrix.M14) + (position.Y * matrix.M24) + (position.Z * matrix.M34) + matrix.M44); | ||
516 | } | ||
517 | |||
518 | public static void Transform(ref Vector4 vector, ref Matrix matrix, out Vector4 result) | ||
519 | { | ||
520 | result = new Vector4((vector.X * matrix.M11) + (vector.Y * matrix.M21) + (vector.Z * matrix.M31) + (vector.W * matrix.M41), | ||
521 | (vector.X * matrix.M12) + (vector.Y * matrix.M22) + (vector.Z * matrix.M32) + (vector.W * matrix.M42), | ||
522 | (vector.X * matrix.M13) + (vector.Y * matrix.M23) + (vector.Z * matrix.M33) + (vector.W * matrix.M43), | ||
523 | (vector.X * matrix.M14) + (vector.Y * matrix.M24) + (vector.Z * matrix.M34) + (vector.W * matrix.M44)); | ||
524 | } | ||
525 | |||
526 | public override string ToString() | ||
527 | { | ||
528 | StringBuilder sb = new StringBuilder(32); | ||
529 | sb.Append("{X:"); | ||
530 | sb.Append(this.X); | ||
531 | sb.Append(" Y:"); | ||
532 | sb.Append(this.Y); | ||
533 | sb.Append(" Z:"); | ||
534 | sb.Append(this.Z); | ||
535 | sb.Append(" W:"); | ||
536 | sb.Append(this.W); | ||
537 | sb.Append("}"); | ||
538 | return sb.ToString(); | ||
539 | } | ||
540 | |||
541 | #endregion Public Methods | ||
542 | |||
543 | |||
544 | #region Operators | ||
545 | |||
546 | public static Vector4 operator -(Vector4 value) | ||
547 | { | ||
548 | return new Vector4(-value.X, -value.Y, -value.Z, -value.W); | ||
549 | } | ||
550 | |||
551 | public static bool operator ==(Vector4 value1, Vector4 value2) | ||
552 | { | ||
553 | return value1.W == value2.W | ||
554 | && value1.X == value2.X | ||
555 | && value1.Y == value2.Y | ||
556 | && value1.Z == value2.Z; | ||
557 | } | ||
558 | |||
559 | public static bool operator !=(Vector4 value1, Vector4 value2) | ||
560 | { | ||
561 | return !(value1 == value2); | ||
562 | } | ||
563 | |||
564 | public static Vector4 operator +(Vector4 value1, Vector4 value2) | ||
565 | { | ||
566 | value1.W += value2.W; | ||
567 | value1.X += value2.X; | ||
568 | value1.Y += value2.Y; | ||
569 | value1.Z += value2.Z; | ||
570 | return value1; | ||
571 | } | ||
572 | |||
573 | public static Vector4 operator -(Vector4 value1, Vector4 value2) | ||
574 | { | ||
575 | value1.W -= value2.W; | ||
576 | value1.X -= value2.X; | ||
577 | value1.Y -= value2.Y; | ||
578 | value1.Z -= value2.Z; | ||
579 | return value1; | ||
580 | } | ||
581 | |||
582 | public static Vector4 operator *(Vector4 value1, Vector4 value2) | ||
583 | { | ||
584 | value1.W *= value2.W; | ||
585 | value1.X *= value2.X; | ||
586 | value1.Y *= value2.Y; | ||
587 | value1.Z *= value2.Z; | ||
588 | return value1; | ||
589 | } | ||
590 | |||
591 | public static Vector4 operator *(Vector4 value1, float scaleFactor) | ||
592 | { | ||
593 | value1.W *= scaleFactor; | ||
594 | value1.X *= scaleFactor; | ||
595 | value1.Y *= scaleFactor; | ||
596 | value1.Z *= scaleFactor; | ||
597 | return value1; | ||
598 | } | ||
599 | |||
600 | public static Vector4 operator *(float scaleFactor, Vector4 value1) | ||
601 | { | ||
602 | value1.W *= scaleFactor; | ||
603 | value1.X *= scaleFactor; | ||
604 | value1.Y *= scaleFactor; | ||
605 | value1.Z *= scaleFactor; | ||
606 | return value1; | ||
607 | } | ||
608 | |||
609 | public static Vector4 operator /(Vector4 value1, Vector4 value2) | ||
610 | { | ||
611 | value1.W /= value2.W; | ||
612 | value1.X /= value2.X; | ||
613 | value1.Y /= value2.Y; | ||
614 | value1.Z /= value2.Z; | ||
615 | return value1; | ||
616 | } | ||
617 | |||
618 | public static Vector4 operator /(Vector4 value1, float divider) | ||
619 | { | ||
620 | float factor = 1f / divider; | ||
621 | value1.W *= factor; | ||
622 | value1.X *= factor; | ||
623 | value1.Y *= factor; | ||
624 | value1.Z *= factor; | ||
625 | return value1; | ||
626 | } | ||
627 | |||
628 | #endregion Operators | ||
629 | } | ||
630 | } | ||