aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs1578
1 files changed, 789 insertions, 789 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs
index acceb98..3702c2a 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs
@@ -1,790 +1,790 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software 14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// DiscreteDynamicsWorld provides discrete rigid body simulation 30 /// DiscreteDynamicsWorld provides discrete rigid body simulation
31 /// those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController 31 /// those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
32 /// </summary> 32 /// </summary>
33 public class DiscreteDynamicsWorld : DynamicsWorld 33 public class DiscreteDynamicsWorld : DynamicsWorld
34 { 34 {
35 private static bool _reportMe = true; 35 private static bool _reportMe = true;
36 36
37 private IConstraintSolver _constraintSolver; 37 private IConstraintSolver _constraintSolver;
38 private SimulationIslandManager _islandManager; 38 private SimulationIslandManager _islandManager;
39 private List<TypedConstraint> _constraints = new List<TypedConstraint>(); 39 private List<TypedConstraint> _constraints = new List<TypedConstraint>();
40 private IDebugDraw _debugDrawer; 40 private IDebugDraw _debugDrawer;
41 private ContactSolverInfo _solverInfo = new ContactSolverInfo(); 41 private ContactSolverInfo _solverInfo = new ContactSolverInfo();
42 private Vector3 _gravity; 42 private Vector3 _gravity;
43 //for variable timesteps 43 //for variable timesteps
44 private float _localTime; 44 private float _localTime;
45 //for variable timesteps 45 //for variable timesteps
46 private bool _ownsIslandManager; 46 private bool _ownsIslandManager;
47 private bool _ownsConstraintSolver; 47 private bool _ownsConstraintSolver;
48 private List<RaycastVehicle> _vehicles = new List<RaycastVehicle>(); 48 private List<RaycastVehicle> _vehicles = new List<RaycastVehicle>();
49 private int _profileTimings; 49 private int _profileTimings;
50 50
51 public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) 51 public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache)
52 : this(dispatcher, pairCache, null) { } 52 : this(dispatcher, pairCache, null) { }
53 53
54 //this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those 54 //this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those
55 public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver) 55 public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver)
56 : base(dispatcher, pairCache) 56 : base(dispatcher, pairCache)
57 { 57 {
58 _constraintSolver = constraintSolver != null ? constraintSolver : new SequentialImpulseConstraintSolver(); 58 _constraintSolver = constraintSolver != null ? constraintSolver : new SequentialImpulseConstraintSolver();
59 _debugDrawer = null; 59 _debugDrawer = null;
60 _gravity = new Vector3(0, -10, 0); 60 _gravity = new Vector3(0, -10, 0);
61 _localTime = 1f / 60f; 61 _localTime = 1f / 60f;
62 _profileTimings = 0; 62 _profileTimings = 0;
63 _islandManager = new SimulationIslandManager(); 63 _islandManager = new SimulationIslandManager();
64 _ownsIslandManager = true; 64 _ownsIslandManager = true;
65 _ownsConstraintSolver = constraintSolver == null; 65 _ownsConstraintSolver = constraintSolver == null;
66 } 66 }
67 67
68 public ContactSolverInfo SolverInfo { get { return _solverInfo; } } 68 public ContactSolverInfo SolverInfo { get { return _solverInfo; } }
69 public SimulationIslandManager SimulationIslandManager { get { return _islandManager; } } 69 public SimulationIslandManager SimulationIslandManager { get { return _islandManager; } }
70 public CollisionWorld CollisionWorld { get { return this; } } 70 public CollisionWorld CollisionWorld { get { return this; } }
71 71
72 public override IDebugDraw DebugDrawer 72 public override IDebugDraw DebugDrawer
73 { 73 {
74 get 74 get
75 { 75 {
76 return _debugDrawer; 76 return _debugDrawer;
77 } 77 }
78 set 78 set
79 { 79 {
80 _debugDrawer = value; 80 _debugDrawer = value;
81 } 81 }
82 } 82 }
83 83
84 public override Vector3 Gravity 84 public override Vector3 Gravity
85 { 85 {
86 set 86 set
87 { 87 {
88 _gravity = value; 88 _gravity = value;
89 for (int i = 0; i < CollisionObjects.Count; i++) 89 for (int i = 0; i < CollisionObjects.Count; i++)
90 { 90 {
91 CollisionObject colObj = CollisionObjects[i]; 91 CollisionObject colObj = CollisionObjects[i];
92 RigidBody body = RigidBody.Upcast(colObj); 92 RigidBody body = RigidBody.Upcast(colObj);
93 if (body != null) 93 if (body != null)
94 { 94 {
95 body.Gravity = value; 95 body.Gravity = value;
96 } 96 }
97 } 97 }
98 } 98 }
99 } 99 }
100 100
101 public override IConstraintSolver ConstraintSolver 101 public override IConstraintSolver ConstraintSolver
102 { 102 {
103 set 103 set
104 { 104 {
105 _ownsConstraintSolver = false; 105 _ownsConstraintSolver = false;
106 _constraintSolver = value; 106 _constraintSolver = value;
107 } 107 }
108 } 108 }
109 109
110 public override int ConstraintsCount 110 public override int ConstraintsCount
111 { 111 {
112 get 112 get
113 { 113 {
114 return _constraints.Count; 114 return _constraints.Count;
115 } 115 }
116 } 116 }
117 117
118 //if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's 118 //if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
119 public override void StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep) 119 public override void StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep)
120 { 120 {
121 int numSimulationSubSteps = 0; 121 int numSimulationSubSteps = 0;
122 122
123 if (maxSubSteps != 0) 123 if (maxSubSteps != 0)
124 { 124 {
125 //fixed timestep with interpolation 125 //fixed timestep with interpolation
126 _localTime += timeStep; 126 _localTime += timeStep;
127 if (_localTime >= fixedTimeStep) 127 if (_localTime >= fixedTimeStep)
128 { 128 {
129 numSimulationSubSteps = (int)(_localTime / fixedTimeStep); 129 numSimulationSubSteps = (int)(_localTime / fixedTimeStep);
130 _localTime -= numSimulationSubSteps * fixedTimeStep; 130 _localTime -= numSimulationSubSteps * fixedTimeStep;
131 } 131 }
132 } 132 }
133 else 133 else
134 { 134 {
135 //variable timestep 135 //variable timestep
136 fixedTimeStep = timeStep; 136 fixedTimeStep = timeStep;
137 _localTime = timeStep; 137 _localTime = timeStep;
138 if (Math.Abs(timeStep) < float.Epsilon) 138 if (Math.Abs(timeStep) < float.Epsilon)
139 { 139 {
140 numSimulationSubSteps = 0; 140 numSimulationSubSteps = 0;
141 maxSubSteps = 0; 141 maxSubSteps = 0;
142 } 142 }
143 else 143 else
144 { 144 {
145 numSimulationSubSteps = 1; 145 numSimulationSubSteps = 1;
146 maxSubSteps = 1; 146 maxSubSteps = 1;
147 } 147 }
148 } 148 }
149 149
150 //process some debugging flags 150 //process some debugging flags
151 if (DebugDrawer != null) 151 if (DebugDrawer != null)
152 { 152 {
153 RigidBody.DisableDeactivation = (DebugDrawer.DebugMode & DebugDrawModes.NoDeactivation) != 0; 153 RigidBody.DisableDeactivation = (DebugDrawer.DebugMode & DebugDrawModes.NoDeactivation) != 0;
154 } 154 }
155 if (numSimulationSubSteps != 0) 155 if (numSimulationSubSteps != 0)
156 { 156 {
157 157
158 SaveKinematicState(fixedTimeStep); 158 SaveKinematicState(fixedTimeStep);
159 159
160 //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt 160 //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
161 int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; 161 int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
162 162
163 for (int i = 0; i < clampedSimulationSteps; i++) 163 for (int i = 0; i < clampedSimulationSteps; i++)
164 { 164 {
165 InternalSingleStepSimulation(fixedTimeStep); 165 InternalSingleStepSimulation(fixedTimeStep);
166 SynchronizeMotionStates(); 166 SynchronizeMotionStates();
167 } 167 }
168 168
169 } 169 }
170 170
171 SynchronizeMotionStates(); 171 SynchronizeMotionStates();
172 172
173 //return numSimulationSubSteps; 173 //return numSimulationSubSteps;
174 } 174 }
175 175
176 public void StepSimulation(float timeStep, int maxSubSteps) 176 public void StepSimulation(float timeStep, int maxSubSteps)
177 { 177 {
178 StepSimulation(timeStep, maxSubSteps, 1f / 60f); 178 StepSimulation(timeStep, maxSubSteps, 1f / 60f);
179 } 179 }
180 180
181 public override void UpdateAabbs() 181 public override void UpdateAabbs()
182 { 182 {
183 Vector3 colorvec = new Vector3(1, 0, 0); 183 Vector3 colorvec = new Vector3(1, 0, 0);
184 for (int i = 0; i < CollisionObjects.Count; i++) 184 for (int i = 0; i < CollisionObjects.Count; i++)
185 { 185 {
186 CollisionObject colObj = CollisionObjects[i]; 186 CollisionObject colObj = CollisionObjects[i];
187 RigidBody body = RigidBody.Upcast(colObj); 187 RigidBody body = RigidBody.Upcast(colObj);
188 188
189 if (body != null) 189 if (body != null)
190 { 190 {
191 // if (body->IsActive() && (!body->IsStatic())) 191 // if (body->IsActive() && (!body->IsStatic()))
192 { 192 {
193 Vector3 minAabb, maxAabb; 193 Vector3 minAabb, maxAabb;
194 colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb); 194 colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb);
195 OverlappingPairCache bp = BroadphasePairCache; 195 OverlappingPairCache bp = BroadphasePairCache;
196 196
197 //moving objects should be moderately sized, probably something wrong if not 197 //moving objects should be moderately sized, probably something wrong if not
198 if (colObj.IsStaticObject || ((maxAabb - minAabb).LengthSquared() < 1e12f)) 198 if (colObj.IsStaticObject || ((maxAabb - minAabb).LengthSquared() < 1e12f))
199 { 199 {
200 bp.SetAabb(body.Broadphase, minAabb, maxAabb); 200 bp.SetAabb(body.Broadphase, minAabb, maxAabb);
201 } 201 }
202 else 202 else
203 { 203 {
204 //something went wrong, investigate 204 //something went wrong, investigate
205 //this assert is unwanted in 3D modelers (danger of loosing work) 205 //this assert is unwanted in 3D modelers (danger of loosing work)
206 BulletDebug.Assert(false); 206 BulletDebug.Assert(false);
207 body.ActivationState = ActivationState.DisableSimulation; 207 body.ActivationState = ActivationState.DisableSimulation;
208 208
209 if (_reportMe) 209 if (_reportMe)
210 { 210 {
211 _reportMe = false; 211 _reportMe = false;
212 Console.WriteLine("Overflow in AABB, object removed from simulation \n"); 212 Console.WriteLine("Overflow in AABB, object removed from simulation \n");
213 Console.WriteLine("If you can reproduce this, please email bugs@continuousphysics.com\n"); 213 Console.WriteLine("If you can reproduce this, please email bugs@continuousphysics.com\n");
214 Console.WriteLine("Please include above information, your Platform, version of OS.\n"); 214 Console.WriteLine("Please include above information, your Platform, version of OS.\n");
215 Console.WriteLine("Thanks.\n"); 215 Console.WriteLine("Thanks.\n");
216 } 216 }
217 } 217 }
218 if (_debugDrawer != null && (_debugDrawer.DebugMode & DebugDrawModes.DrawAabb) != 0) 218 if (_debugDrawer != null && (_debugDrawer.DebugMode & DebugDrawModes.DrawAabb) != 0)
219 DrawAabb(_debugDrawer, minAabb, maxAabb, colorvec); 219 DrawAabb(_debugDrawer, minAabb, maxAabb, colorvec);
220 } 220 }
221 } 221 }
222 } 222 }
223 } 223 }
224 224
225 public override void AddConstraint(TypedConstraint constraint) 225 public override void AddConstraint(TypedConstraint constraint)
226 { 226 {
227 _constraints.Add(constraint); 227 _constraints.Add(constraint);
228 } 228 }
229 229
230 public override void RemoveConstraint(TypedConstraint constraint) 230 public override void RemoveConstraint(TypedConstraint constraint)
231 { 231 {
232 _constraints.Remove(constraint); 232 _constraints.Remove(constraint);
233 } 233 }
234 234
235 public void AddVehicle(RaycastVehicle vehicle) 235 public void AddVehicle(RaycastVehicle vehicle)
236 { 236 {
237 _vehicles.Add(vehicle); 237 _vehicles.Add(vehicle);
238 } 238 }
239 239
240 public void RemoveVehicle(RaycastVehicle vehicle) 240 public void RemoveVehicle(RaycastVehicle vehicle)
241 { 241 {
242 _vehicles.Remove(vehicle); 242 _vehicles.Remove(vehicle);
243 } 243 }
244 244
245 public override void AddRigidBody(RigidBody body) 245 public override void AddRigidBody(RigidBody body)
246 { 246 {
247 if (!body.IsStaticOrKinematicObject) 247 if (!body.IsStaticOrKinematicObject)
248 { 248 {
249 body.Gravity = _gravity; 249 body.Gravity = _gravity;
250 } 250 }
251 251
252 if (body.CollisionShape != null) 252 if (body.CollisionShape != null)
253 { 253 {
254 bool isDynamic = !(body.IsStaticObject || body.IsKinematicObject); 254 bool isDynamic = !(body.IsStaticObject || body.IsKinematicObject);
255 BroadphaseProxy.CollisionFilterGroups collisionFilterGroup = isDynamic ? BroadphaseProxy.CollisionFilterGroups.Default : BroadphaseProxy.CollisionFilterGroups.Static; 255 BroadphaseProxy.CollisionFilterGroups collisionFilterGroup = isDynamic ? BroadphaseProxy.CollisionFilterGroups.Default : BroadphaseProxy.CollisionFilterGroups.Static;
256 BroadphaseProxy.CollisionFilterGroups collisionFilterMask = isDynamic ? BroadphaseProxy.CollisionFilterGroups.All : (BroadphaseProxy.CollisionFilterGroups.All ^ BroadphaseProxy.CollisionFilterGroups.Static); 256 BroadphaseProxy.CollisionFilterGroups collisionFilterMask = isDynamic ? BroadphaseProxy.CollisionFilterGroups.All : (BroadphaseProxy.CollisionFilterGroups.All ^ BroadphaseProxy.CollisionFilterGroups.Static);
257 257
258 AddCollisionObject(body, collisionFilterGroup, collisionFilterMask); 258 AddCollisionObject(body, collisionFilterGroup, collisionFilterMask);
259 } 259 }
260 } 260 }
261 261
262 public override void RemoveRigidBody(RigidBody body) 262 public override void RemoveRigidBody(RigidBody body)
263 { 263 {
264 RemoveCollisionObject(body); 264 RemoveCollisionObject(body);
265 } 265 }
266 266
267 public void DebugDrawObject(Matrix worldTransform, CollisionShape shape, Vector3 color) 267 public void DebugDrawObject(Matrix worldTransform, CollisionShape shape, Vector3 color)
268 { 268 {
269 if (shape.ShapeType == BroadphaseNativeTypes.Compound) 269 if (shape.ShapeType == BroadphaseNativeTypes.Compound)
270 { 270 {
271 CompoundShape compoundShape = shape as CompoundShape; 271 CompoundShape compoundShape = shape as CompoundShape;
272 for (int i = compoundShape.ChildShapeCount - 1; i >= 0; i--) 272 for (int i = compoundShape.ChildShapeCount - 1; i >= 0; i--)
273 { 273 {
274 Matrix childTrans = compoundShape.GetChildTransform(i); 274 Matrix childTrans = compoundShape.GetChildTransform(i);
275 CollisionShape colShape = compoundShape.GetChildShape(i); 275 CollisionShape colShape = compoundShape.GetChildShape(i);
276 DebugDrawObject(worldTransform * childTrans, colShape, color); 276 DebugDrawObject(worldTransform * childTrans, colShape, color);
277 } 277 }
278 278
279 } 279 }
280 else 280 else
281 { 281 {
282 switch (shape.ShapeType) 282 switch (shape.ShapeType)
283 { 283 {
284 284
285 case BroadphaseNativeTypes.Sphere: 285 case BroadphaseNativeTypes.Sphere:
286 { 286 {
287 SphereShape sphereShape = shape as SphereShape; 287 SphereShape sphereShape = shape as SphereShape;
288 float radius = sphereShape.Margin;//radius doesn't include the margin, so draw with margin 288 float radius = sphereShape.Margin;//radius doesn't include the margin, so draw with margin
289 Vector3 start = worldTransform.Translation; 289 Vector3 start = worldTransform.Translation;
290 DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(radius, 0, 0), worldTransform), color); 290 DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(radius, 0, 0), worldTransform), color);
291 DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, radius, 0), worldTransform), color); 291 DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, radius, 0), worldTransform), color);
292 DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, 0, radius), worldTransform), color); 292 DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, 0, radius), worldTransform), color);
293 //drawSphere 293 //drawSphere
294 break; 294 break;
295 } 295 }
296 case BroadphaseNativeTypes.MultiSphere: 296 case BroadphaseNativeTypes.MultiSphere:
297 case BroadphaseNativeTypes.Cone: 297 case BroadphaseNativeTypes.Cone:
298 { 298 {
299 ConeShape coneShape = shape as ConeShape; 299 ConeShape coneShape = shape as ConeShape;
300 float radius = coneShape.Radius;//+coneShape->getMargin(); 300 float radius = coneShape.Radius;//+coneShape->getMargin();
301 float height = coneShape.Height;//+coneShape->getMargin(); 301 float height = coneShape.Height;//+coneShape->getMargin();
302 Vector3 start = worldTransform.Translation; 302 Vector3 start = worldTransform.Translation;
303 DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(radius, 0f, -0.5f * height), worldTransform), color); 303 DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(radius, 0f, -0.5f * height), worldTransform), color);
304 DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(-radius, 0f, -0.5f * height), worldTransform), color); 304 DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(-radius, 0f, -0.5f * height), worldTransform), color);
305 DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, radius, -0.5f * height), worldTransform), color); 305 DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, radius, -0.5f * height), worldTransform), color);
306 DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, -radius, -0.5f * height), worldTransform), color); 306 DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, -radius, -0.5f * height), worldTransform), color);
307 break; 307 break;
308 } 308 }
309 case BroadphaseNativeTypes.Cylinder: 309 case BroadphaseNativeTypes.Cylinder:
310 { 310 {
311 CylinderShape cylinder = shape as CylinderShape; 311 CylinderShape cylinder = shape as CylinderShape;
312 int upAxis = cylinder.UpAxis; 312 int upAxis = cylinder.UpAxis;
313 float radius = cylinder.Radius; 313 float radius = cylinder.Radius;
314 float halfHeight = MathHelper.GetElement(cylinder.HalfExtents, upAxis); 314 float halfHeight = MathHelper.GetElement(cylinder.HalfExtents, upAxis);
315 Vector3 start = worldTransform.Translation; 315 Vector3 start = worldTransform.Translation;
316 Vector3 offsetHeight = new Vector3(); 316 Vector3 offsetHeight = new Vector3();
317 MathHelper.SetElement(ref offsetHeight, upAxis, halfHeight); 317 MathHelper.SetElement(ref offsetHeight, upAxis, halfHeight);
318 Vector3 offsetRadius = new Vector3(); 318 Vector3 offsetRadius = new Vector3();
319 MathHelper.SetElement(ref offsetRadius, (upAxis + 1) % 3, radius); 319 MathHelper.SetElement(ref offsetRadius, (upAxis + 1) % 3, radius);
320 DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight + offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight + offsetRadius, worldTransform), color); 320 DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight + offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight + offsetRadius, worldTransform), color);
321 DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight - offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color); 321 DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight - offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color);
322 break; 322 break;
323 } 323 }
324 default: 324 default:
325 { 325 {
326 if (shape.ShapeType == BroadphaseNativeTypes.TriangleMesh) 326 if (shape.ShapeType == BroadphaseNativeTypes.TriangleMesh)
327 { 327 {
328 TriangleMeshShape concaveMesh = shape as TriangleMeshShape; 328 TriangleMeshShape concaveMesh = shape as TriangleMeshShape;
329 //btVector3 aabbMax(1e30f,1e30f,1e30f); 329 //btVector3 aabbMax(1e30f,1e30f,1e30f);
330 //btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f); 330 //btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f);
331 331
332 //todo pass camera, for some culling 332 //todo pass camera, for some culling
333 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); 333 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
334 Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); 334 Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
335 335
336 DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color); 336 DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color);
337 concaveMesh.ProcessAllTriangles(drawCallback, aabbMin, aabbMax); 337 concaveMesh.ProcessAllTriangles(drawCallback, aabbMin, aabbMax);
338 } 338 }
339 339
340 if (shape.ShapeType == BroadphaseNativeTypes.ConvexTriangleMesh) 340 if (shape.ShapeType == BroadphaseNativeTypes.ConvexTriangleMesh)
341 { 341 {
342 ConvexTriangleMeshShape convexMesh = shape as ConvexTriangleMeshShape; 342 ConvexTriangleMeshShape convexMesh = shape as ConvexTriangleMeshShape;
343 //todo: pass camera for some culling 343 //todo: pass camera for some culling
344 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); 344 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
345 Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); 345 Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
346 //DebugDrawcallback drawCallback; 346 //DebugDrawcallback drawCallback;
347 DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color); 347 DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color);
348 convexMesh.getStridingMesh().InternalProcessAllTriangles(drawCallback, aabbMin, aabbMax); 348 convexMesh.getStridingMesh().InternalProcessAllTriangles(drawCallback, aabbMin, aabbMax);
349 } 349 }
350 350
351 // for polyhedral shapes 351 // for polyhedral shapes
352 if (shape.IsPolyhedral) 352 if (shape.IsPolyhedral)
353 { 353 {
354 PolyhedralConvexShape polyshape = shape as PolyhedralConvexShape; 354 PolyhedralConvexShape polyshape = shape as PolyhedralConvexShape;
355 355
356 for (int i = 0; i < polyshape.EdgeCount; i++) 356 for (int i = 0; i < polyshape.EdgeCount; i++)
357 { 357 {
358 Vector3 a, b; 358 Vector3 a, b;
359 polyshape.GetEdge(i, out a, out b); 359 polyshape.GetEdge(i, out a, out b);
360 a = Vector3.TransformNormal(a, worldTransform); 360 a = Vector3.TransformNormal(a, worldTransform);
361 b = Vector3.TransformNormal(b, worldTransform); 361 b = Vector3.TransformNormal(b, worldTransform);
362 DebugDrawer.DrawLine(a, b, color); 362 DebugDrawer.DrawLine(a, b, color);
363 } 363 }
364 } 364 }
365 break; 365 break;
366 } 366 }
367 } 367 }
368 } 368 }
369 } 369 }
370 370
371 public override TypedConstraint GetConstraint(int index) 371 public override TypedConstraint GetConstraint(int index)
372 { 372 {
373 return _constraints[index]; 373 return _constraints[index];
374 } 374 }
375 375
376 public static void DrawAabb(IDebugDraw debugDrawer, Vector3 from, Vector3 to, Vector3 color) 376 public static void DrawAabb(IDebugDraw debugDrawer, Vector3 from, Vector3 to, Vector3 color)
377 { 377 {
378 Vector3 halfExtents = (to - from) * 0.5f; 378 Vector3 halfExtents = (to - from) * 0.5f;
379 Vector3 center = (to + from) * 0.5f; 379 Vector3 center = (to + from) * 0.5f;
380 380
381 Vector3 edgecoord = new Vector3(1f, 1f, 1f), pa, pb; 381 Vector3 edgecoord = new Vector3(1f, 1f, 1f), pa, pb;
382 for (int i = 0; i < 4; i++) 382 for (int i = 0; i < 4; i++)
383 { 383 {
384 for (int j = 0; j < 3; j++) 384 for (int j = 0; j < 3; j++)
385 { 385 {
386 pa = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y, 386 pa = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,
387 edgecoord.Z * halfExtents.Z); 387 edgecoord.Z * halfExtents.Z);
388 pa += center; 388 pa += center;
389 389
390 int othercoord = j % 3; 390 int othercoord = j % 3;
391 MathHelper.SetElement(ref edgecoord, othercoord, MathHelper.GetElement(edgecoord, othercoord) * -1f); 391 MathHelper.SetElement(ref edgecoord, othercoord, MathHelper.GetElement(edgecoord, othercoord) * -1f);
392 pb = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y, 392 pb = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,
393 edgecoord.Z * halfExtents.Z); 393 edgecoord.Z * halfExtents.Z);
394 pb += center; 394 pb += center;
395 395
396 debugDrawer.DrawLine(pa, pb, color); 396 debugDrawer.DrawLine(pa, pb, color);
397 } 397 }
398 edgecoord = new Vector3(-1f, -1f, -1f); 398 edgecoord = new Vector3(-1f, -1f, -1f);
399 if (i < 3) 399 if (i < 3)
400 MathHelper.SetElement(ref edgecoord, i, MathHelper.GetElement(edgecoord, i) * -1f); 400 MathHelper.SetElement(ref edgecoord, i, MathHelper.GetElement(edgecoord, i) * -1f);
401 } 401 }
402 } 402 }
403 403
404 protected void PredictUnconstraintMotion(float timeStep) 404 protected void PredictUnconstraintMotion(float timeStep)
405 { 405 {
406 for (int i = 0; i < CollisionObjects.Count; i++) 406 for (int i = 0; i < CollisionObjects.Count; i++)
407 { 407 {
408 CollisionObject colObj = CollisionObjects[i]; 408 CollisionObject colObj = CollisionObjects[i];
409 RigidBody body = RigidBody.Upcast(colObj); 409 RigidBody body = RigidBody.Upcast(colObj);
410 if (body != null) 410 if (body != null)
411 { 411 {
412 if (!body.IsStaticOrKinematicObject) 412 if (!body.IsStaticOrKinematicObject)
413 { 413 {
414 if (body.IsActive) 414 if (body.IsActive)
415 { 415 {
416 body.ApplyForces(timeStep); 416 body.ApplyForces(timeStep);
417 body.IntegrateVelocities(timeStep); 417 body.IntegrateVelocities(timeStep);
418 Matrix temp = body.InterpolationWorldTransform; 418 Matrix temp = body.InterpolationWorldTransform;
419 body.PredictIntegratedTransform(timeStep, ref temp); 419 body.PredictIntegratedTransform(timeStep, ref temp);
420 body.InterpolationWorldTransform = temp; 420 body.InterpolationWorldTransform = temp;
421 } 421 }
422 } 422 }
423 } 423 }
424 } 424 }
425 } 425 }
426 426
427 protected void IntegrateTransforms(float timeStep) 427 protected void IntegrateTransforms(float timeStep)
428 { 428 {
429 Matrix predictedTrans = new Matrix(); 429 Matrix predictedTrans = new Matrix();
430 for (int i = 0; i < CollisionObjects.Count; i++) 430 for (int i = 0; i < CollisionObjects.Count; i++)
431 { 431 {
432 CollisionObject colObj = CollisionObjects[i]; 432 CollisionObject colObj = CollisionObjects[i];
433 RigidBody body = RigidBody.Upcast(colObj); 433 RigidBody body = RigidBody.Upcast(colObj);
434 if (body != null) 434 if (body != null)
435 { 435 {
436 if (body.IsActive && (!body.IsStaticOrKinematicObject)) 436 if (body.IsActive && (!body.IsStaticOrKinematicObject))
437 { 437 {
438 body.PredictIntegratedTransform(timeStep, ref predictedTrans); 438 body.PredictIntegratedTransform(timeStep, ref predictedTrans);
439 body.ProceedToTransform(predictedTrans); 439 body.ProceedToTransform(predictedTrans);
440 } 440 }
441 } 441 }
442 } 442 }
443 } 443 }
444 444
445 protected void CalculateSimulationIslands() 445 protected void CalculateSimulationIslands()
446 { 446 {
447 SimulationIslandManager.UpdateActivationState(this, Dispatcher); 447 SimulationIslandManager.UpdateActivationState(this, Dispatcher);
448 448
449 for (int i = 0; i < _constraints.Count; i++) 449 for (int i = 0; i < _constraints.Count; i++)
450 { 450 {
451 TypedConstraint constraint = _constraints[i]; 451 TypedConstraint constraint = _constraints[i];
452 452
453 RigidBody colObj0 = constraint.RigidBodyA; 453 RigidBody colObj0 = constraint.RigidBodyA;
454 RigidBody colObj1 = constraint.RigidBodyB; 454 RigidBody colObj1 = constraint.RigidBodyB;
455 455
456 if (((colObj0 != null) && (colObj0.MergesSimulationIslands)) && 456 if (((colObj0 != null) && (colObj0.MergesSimulationIslands)) &&
457 ((colObj1 != null) && (colObj1.MergesSimulationIslands))) 457 ((colObj1 != null) && (colObj1.MergesSimulationIslands)))
458 { 458 {
459 if (colObj0.IsActive || colObj1.IsActive) 459 if (colObj0.IsActive || colObj1.IsActive)
460 { 460 {
461 461
462 SimulationIslandManager.UnionFind.Unite((colObj0).IslandTag, 462 SimulationIslandManager.UnionFind.Unite((colObj0).IslandTag,
463 (colObj1).IslandTag); 463 (colObj1).IslandTag);
464 } 464 }
465 } 465 }
466 } 466 }
467 467
468 //Store the island id in each body 468 //Store the island id in each body
469 SimulationIslandManager.StoreIslandActivationState(this); 469 SimulationIslandManager.StoreIslandActivationState(this);
470 } 470 }
471 471
472 //protected void SolveNonContactConstraints(ContactSolverInfo solverInfo) 472 //protected void SolveNonContactConstraints(ContactSolverInfo solverInfo)
473 //{ 473 //{
474 // //constraint preparation: building jacobians 474 // //constraint preparation: building jacobians
475 // for (int i = 0; i < _constraints.Count; i++) 475 // for (int i = 0; i < _constraints.Count; i++)
476 // { 476 // {
477 // TypedConstraint constraint = _constraints[i]; 477 // TypedConstraint constraint = _constraints[i];
478 // constraint.BuildJacobian(); 478 // constraint.BuildJacobian();
479 // } 479 // }
480 480
481 // //solve the regular non-contact constraints (point 2 point, hinge, generic d6) 481 // //solve the regular non-contact constraints (point 2 point, hinge, generic d6)
482 // for (int g = 0; g < solverInfo.IterationsCount; g++) 482 // for (int g = 0; g < solverInfo.IterationsCount; g++)
483 // { 483 // {
484 // for (int i = 0; i < _constraints.Count; i++) 484 // for (int i = 0; i < _constraints.Count; i++)
485 // { 485 // {
486 // TypedConstraint constraint = _constraints[i]; 486 // TypedConstraint constraint = _constraints[i];
487 // constraint.SolveConstraint(solverInfo.TimeStep); 487 // constraint.SolveConstraint(solverInfo.TimeStep);
488 // } 488 // }
489 // } 489 // }
490 //} 490 //}
491 491
492 //protected void SolveContactConstraints(ContactSolverInfo solverInfo) 492 //protected void SolveContactConstraints(ContactSolverInfo solverInfo)
493 //{ 493 //{
494 // InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, _debugDrawer); 494 // InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, _debugDrawer);
495 495
496 // // solve all the contact points and contact friction 496 // // solve all the contact points and contact friction
497 // _islandManager.BuildAndProcessIslands(Dispatcher, CollisionObjects, solverCallback); 497 // _islandManager.BuildAndProcessIslands(Dispatcher, CollisionObjects, solverCallback);
498 //} 498 //}
499 499
500 protected void SolveConstraints(ContactSolverInfo solverInfo) 500 protected void SolveConstraints(ContactSolverInfo solverInfo)
501 { 501 {
502 //sorted version of all btTypedConstraint, based on islandId 502 //sorted version of all btTypedConstraint, based on islandId
503 List<TypedConstraint> sortedConstraints = new List<TypedConstraint>(ConstraintsCount); 503 List<TypedConstraint> sortedConstraints = new List<TypedConstraint>(ConstraintsCount);
504 504
505 for (int i = 0; i < ConstraintsCount; i++) 505 for (int i = 0; i < ConstraintsCount; i++)
506 { 506 {
507 sortedConstraints.Add(_constraints[i]); 507 sortedConstraints.Add(_constraints[i]);
508 } 508 }
509 509
510 sortedConstraints.Sort(new Comparison<TypedConstraint>(TypedConstraint.SortConstraintOnIslandPredicate)); 510 sortedConstraints.Sort(new Comparison<TypedConstraint>(TypedConstraint.SortConstraintOnIslandPredicate));
511 List<TypedConstraint> constraintsPtr = ConstraintsCount != 0 ? sortedConstraints : new List<TypedConstraint>(); 511 List<TypedConstraint> constraintsPtr = ConstraintsCount != 0 ? sortedConstraints : new List<TypedConstraint>();
512 512
513 InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, constraintsPtr, _debugDrawer); 513 InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, constraintsPtr, _debugDrawer);
514 514
515 // solve all the constraints for this island 515 // solve all the constraints for this island
516 _islandManager.BuildAndProcessIslands(CollisionWorld.Dispatcher, CollisionWorld.CollisionObjects, solverCallback); 516 _islandManager.BuildAndProcessIslands(CollisionWorld.Dispatcher, CollisionWorld.CollisionObjects, solverCallback);
517 } 517 }
518 518
519 protected void UpdateActivationState(float timeStep) 519 protected void UpdateActivationState(float timeStep)
520 { 520 {
521 for (int i = 0; i < CollisionObjects.Count; i++) 521 for (int i = 0; i < CollisionObjects.Count; i++)
522 { 522 {
523 CollisionObject colObj = CollisionObjects[i]; 523 CollisionObject colObj = CollisionObjects[i];
524 RigidBody body = RigidBody.Upcast(colObj); 524 RigidBody body = RigidBody.Upcast(colObj);
525 if (body != null) 525 if (body != null)
526 { 526 {
527 body.UpdateDeactivation(timeStep); 527 body.UpdateDeactivation(timeStep);
528 528
529 if (body.WantsSleeping()) 529 if (body.WantsSleeping())
530 { 530 {
531 if (body.IsStaticOrKinematicObject) 531 if (body.IsStaticOrKinematicObject)
532 { 532 {
533 body.ActivationState = ActivationState.IslandSleeping; 533 body.ActivationState = ActivationState.IslandSleeping;
534 } 534 }
535 else 535 else
536 { 536 {
537 if (body.ActivationState == ActivationState.Active) 537 if (body.ActivationState == ActivationState.Active)
538 body.ActivationState = ActivationState.WantsDeactivation; 538 body.ActivationState = ActivationState.WantsDeactivation;
539 } 539 }
540 } 540 }
541 else 541 else
542 { 542 {
543 if (body.ActivationState != ActivationState.DisableDeactivation) 543 if (body.ActivationState != ActivationState.DisableDeactivation)
544 body.ActivationState = ActivationState.Active; 544 body.ActivationState = ActivationState.Active;
545 } 545 }
546 } 546 }
547 } 547 }
548 } 548 }
549 549
550 protected void UpdateVehicles(float timeStep) 550 protected void UpdateVehicles(float timeStep)
551 { 551 {
552 for (int i = 0; i < _vehicles.Count; i++) 552 for (int i = 0; i < _vehicles.Count; i++)
553 { 553 {
554 RaycastVehicle vehicle = _vehicles[i]; 554 RaycastVehicle vehicle = _vehicles[i];
555 vehicle.updateVehicle(timeStep); 555 vehicle.updateVehicle(timeStep);
556 } 556 }
557 } 557 }
558 558
559 protected void StartProfiling(float timeStep) { } 559 protected void StartProfiling(float timeStep) { }
560 560
561 protected virtual void InternalSingleStepSimulation(float timeStep) 561 protected virtual void InternalSingleStepSimulation(float timeStep)
562 { 562 {
563 StartProfiling(timeStep); 563 StartProfiling(timeStep);
564 564
565 //update aabbs information 565 //update aabbs information
566 UpdateAabbs(); 566 UpdateAabbs();
567 567
568 //apply gravity, predict motion 568 //apply gravity, predict motion
569 PredictUnconstraintMotion(timeStep); 569 PredictUnconstraintMotion(timeStep);
570 570
571 DispatcherInfo dispatchInfo = DispatchInfo; 571 DispatcherInfo dispatchInfo = DispatchInfo;
572 dispatchInfo.TimeStep = timeStep; 572 dispatchInfo.TimeStep = timeStep;
573 dispatchInfo.StepCount = 0; 573 dispatchInfo.StepCount = 0;
574 dispatchInfo.DebugDraw = DebugDrawer; 574 dispatchInfo.DebugDraw = DebugDrawer;
575 575
576 //perform collision detection 576 //perform collision detection
577 PerformDiscreteCollisionDetection(); 577 PerformDiscreteCollisionDetection();
578 578
579 CalculateSimulationIslands(); 579 CalculateSimulationIslands();
580 580
581 SolverInfo.TimeStep = timeStep; 581 SolverInfo.TimeStep = timeStep;
582 582
583 //solve contact and other joint constraints 583 //solve contact and other joint constraints
584 SolveConstraints(SolverInfo); 584 SolveConstraints(SolverInfo);
585 585
586 //CallbackTriggers(); 586 //CallbackTriggers();
587 587
588 //integrate transforms 588 //integrate transforms
589 IntegrateTransforms(timeStep); 589 IntegrateTransforms(timeStep);
590 590
591 //update vehicle simulation 591 //update vehicle simulation
592 UpdateVehicles(timeStep); 592 UpdateVehicles(timeStep);
593 593
594 UpdateActivationState(timeStep); 594 UpdateActivationState(timeStep);
595 } 595 }
596 596
597 protected void SynchronizeMotionStates() 597 protected void SynchronizeMotionStates()
598 { 598 {
599 //debug vehicle wheels 599 //debug vehicle wheels
600 { 600 {
601 //todo: iterate over awake simulation islands! 601 //todo: iterate over awake simulation islands!
602 for (int i = 0; i < CollisionObjects.Count; i++) 602 for (int i = 0; i < CollisionObjects.Count; i++)
603 { 603 {
604 CollisionObject colObj = CollisionObjects[i]; 604 CollisionObject colObj = CollisionObjects[i];
605 if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0) 605 if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0)
606 { 606 {
607 Vector3 color = new Vector3(255f, 255f, 255f); 607 Vector3 color = new Vector3(255f, 255f, 255f);
608 switch (colObj.ActivationState) 608 switch (colObj.ActivationState)
609 { 609 {
610 case ActivationState.Active: 610 case ActivationState.Active:
611 color = new Vector3(255f, 255f, 255f); break; 611 color = new Vector3(255f, 255f, 255f); break;
612 case ActivationState.IslandSleeping: 612 case ActivationState.IslandSleeping:
613 color = new Vector3(0f, 255f, 0f); break; 613 color = new Vector3(0f, 255f, 0f); break;
614 case ActivationState.WantsDeactivation: 614 case ActivationState.WantsDeactivation:
615 color = new Vector3(0f, 255f, 255f); break; 615 color = new Vector3(0f, 255f, 255f); break;
616 case ActivationState.DisableDeactivation: 616 case ActivationState.DisableDeactivation:
617 color = new Vector3(255f, 0f, 0f); break; 617 color = new Vector3(255f, 0f, 0f); break;
618 case ActivationState.DisableSimulation: 618 case ActivationState.DisableSimulation:
619 color = new Vector3(255f, 255f, 0f); break; 619 color = new Vector3(255f, 255f, 0f); break;
620 default: 620 default:
621 color = new Vector3(255f, 0f, 0f); break; 621 color = new Vector3(255f, 0f, 0f); break;
622 } 622 }
623 623
624 DebugDrawObject(colObj.WorldTransform, colObj.CollisionShape, color); 624 DebugDrawObject(colObj.WorldTransform, colObj.CollisionShape, color);
625 } 625 }
626 RigidBody body = RigidBody.Upcast(colObj); 626 RigidBody body = RigidBody.Upcast(colObj);
627 if (body != null && body.MotionState != null && !body.IsStaticOrKinematicObject) 627 if (body != null && body.MotionState != null && !body.IsStaticOrKinematicObject)
628 { 628 {
629 //if (body.ActivationState != ActivationState.IslandSleeping) 629 //if (body.ActivationState != ActivationState.IslandSleeping)
630 { 630 {
631 Matrix interpolatedTransform = new Matrix(); 631 Matrix interpolatedTransform = new Matrix();
632 TransformUtil.IntegrateTransform(body.InterpolationWorldTransform, 632 TransformUtil.IntegrateTransform(body.InterpolationWorldTransform,
633 body.InterpolationLinearVelocity, body.InterpolationAngularVelocity, _localTime, ref interpolatedTransform); 633 body.InterpolationLinearVelocity, body.InterpolationAngularVelocity, _localTime, ref interpolatedTransform);
634 body.MotionState.SetWorldTransform(interpolatedTransform); 634 body.MotionState.SetWorldTransform(interpolatedTransform);
635 } 635 }
636 } 636 }
637 } 637 }
638 } 638 }
639 639
640 if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0) 640 if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0)
641 { 641 {
642 for (int i = 0; i < _vehicles.Count; i++) 642 for (int i = 0; i < _vehicles.Count; i++)
643 { 643 {
644 for (int v = 0; v < _vehicles[i].getNumWheels(); v++) 644 for (int v = 0; v < _vehicles[i].getNumWheels(); v++)
645 { 645 {
646 Vector3 wheelColor = new Vector3(0, 255, 255); 646 Vector3 wheelColor = new Vector3(0, 255, 255);
647 if (_vehicles[i].getWheelInfo(v).RaycastInfo.IsInContact) 647 if (_vehicles[i].getWheelInfo(v).RaycastInfo.IsInContact)
648 { 648 {
649 wheelColor = new Vector3(0, 0, 255); 649 wheelColor = new Vector3(0, 0, 255);
650 } 650 }
651 else 651 else
652 { 652 {
653 wheelColor = new Vector3(255, 0, 255); 653 wheelColor = new Vector3(255, 0, 255);
654 } 654 }
655 655
656 //synchronize the wheels with the (interpolated) chassis worldtransform 656 //synchronize the wheels with the (interpolated) chassis worldtransform
657 _vehicles[i].updateWheelTransform(v, true); 657 _vehicles[i].updateWheelTransform(v, true);
658 658
659 Vector3 wheelPosWS = _vehicles[i].getWheelInfo(v).WorldTransform.Translation; 659 Vector3 wheelPosWS = _vehicles[i].getWheelInfo(v).WorldTransform.Translation;
660 660
661 Vector3 axle = new Vector3( 661 Vector3 axle = new Vector3(
662 MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 0, _vehicles[i].getRightAxis()), 662 MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 0, _vehicles[i].getRightAxis()),
663 MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 1, _vehicles[i].getRightAxis()), 663 MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 1, _vehicles[i].getRightAxis()),
664 MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 2, _vehicles[i].getRightAxis())); 664 MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 2, _vehicles[i].getRightAxis()));
665 665
666 666
667 //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS 667 //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
668 //debug wheels (cylinders) 668 //debug wheels (cylinders)
669 _debugDrawer.DrawLine(wheelPosWS, wheelPosWS + axle, wheelColor); 669 _debugDrawer.DrawLine(wheelPosWS, wheelPosWS + axle, wheelColor);
670 _debugDrawer.DrawLine(wheelPosWS, _vehicles[i].getWheelInfo(v).RaycastInfo.ContactPointWS, wheelColor); 670 _debugDrawer.DrawLine(wheelPosWS, _vehicles[i].getWheelInfo(v).RaycastInfo.ContactPointWS, wheelColor);
671 } 671 }
672 } 672 }
673 } 673 }
674 } 674 }
675 675
676 protected void SaveKinematicState(float timeStep) 676 protected void SaveKinematicState(float timeStep)
677 { 677 {
678 for (int i = 0; i < CollisionObjects.Count; i++) 678 for (int i = 0; i < CollisionObjects.Count; i++)
679 { 679 {
680 CollisionObject colObj = CollisionObjects[i]; 680 CollisionObject colObj = CollisionObjects[i];
681 RigidBody body = RigidBody.Upcast(colObj); 681 RigidBody body = RigidBody.Upcast(colObj);
682 if (body != null) 682 if (body != null)
683 { 683 {
684 if (body.ActivationState != ActivationState.IslandSleeping) 684 if (body.ActivationState != ActivationState.IslandSleeping)
685 { 685 {
686 if (body.IsKinematicObject) 686 if (body.IsKinematicObject)
687 { 687 {
688 //to calculate velocities next frame 688 //to calculate velocities next frame
689 body.SaveKinematicState(timeStep); 689 body.SaveKinematicState(timeStep);
690 } 690 }
691 } 691 }
692 } 692 }
693 } 693 }
694 } 694 }
695 695
696 internal class InplaceSolverIslandCallback : SimulationIslandManager.IIslandCallback 696 internal class InplaceSolverIslandCallback : SimulationIslandManager.IIslandCallback
697 { 697 {
698 private ContactSolverInfo _solverInfo; 698 private ContactSolverInfo _solverInfo;
699 private IConstraintSolver _solver; 699 private IConstraintSolver _solver;
700 private IDebugDraw _debugDrawer; 700 private IDebugDraw _debugDrawer;
701 private List<TypedConstraint> _sortedConstraints; 701 private List<TypedConstraint> _sortedConstraints;
702 702
703 public InplaceSolverIslandCallback( 703 public InplaceSolverIslandCallback(
704 ContactSolverInfo solverInfo, 704 ContactSolverInfo solverInfo,
705 IConstraintSolver solver, 705 IConstraintSolver solver,
706 List<TypedConstraint> sortedConstraints, 706 List<TypedConstraint> sortedConstraints,
707 IDebugDraw debugDrawer) 707 IDebugDraw debugDrawer)
708 { 708 {
709 _solverInfo = solverInfo; 709 _solverInfo = solverInfo;
710 _solver = solver; 710 _solver = solver;
711 _sortedConstraints = sortedConstraints; 711 _sortedConstraints = sortedConstraints;
712 _debugDrawer = debugDrawer; 712 _debugDrawer = debugDrawer;
713 } 713 }
714 714
715 public ContactSolverInfo SolverInfo { get { return _solverInfo; } set { _solverInfo = value; } } 715 public ContactSolverInfo SolverInfo { get { return _solverInfo; } set { _solverInfo = value; } }
716 public IConstraintSolver Solver { get { return _solver; } set { _solver = value; } } 716 public IConstraintSolver Solver { get { return _solver; } set { _solver = value; } }
717 public List<TypedConstraint> Constraints { get { return _sortedConstraints; } set { _sortedConstraints = value; } } 717 public List<TypedConstraint> Constraints { get { return _sortedConstraints; } set { _sortedConstraints = value; } }
718 public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } } 718 public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } }
719 719
720 public void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID) 720 public void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID)
721 { 721 {
722 //also add all non-contact constraints/joints for this island 722 //also add all non-contact constraints/joints for this island
723 List<TypedConstraint> startConstraint = new List<TypedConstraint>(); 723 List<TypedConstraint> startConstraint = new List<TypedConstraint>();
724 int numCurConstraints = 0; 724 int numCurConstraints = 0;
725 int startIndex = 0; 725 int startIndex = 0;
726 int i; 726 int i;
727 727
728 //find the first constraint for this island 728 //find the first constraint for this island
729 for (i = 0; i < _sortedConstraints.Count; i++) 729 for (i = 0; i < _sortedConstraints.Count; i++)
730 { 730 {
731 if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID) 731 if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID)
732 { 732 {
733 //startConstraint = &m_sortedConstraints[i]; 733 //startConstraint = &m_sortedConstraints[i];
734 startIndex = i; 734 startIndex = i;
735 break; 735 break;
736 } 736 }
737 } 737 }
738 //count the number of constraints in this island 738 //count the number of constraints in this island
739 for (; i < _sortedConstraints.Count; i++) 739 for (; i < _sortedConstraints.Count; i++)
740 { 740 {
741 if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID) 741 if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID)
742 { 742 {
743 numCurConstraints++; 743 numCurConstraints++;
744 } 744 }
745 } 745 }
746 746
747 for (i = startIndex; i < startIndex + numCurConstraints; i++) 747 for (i = startIndex; i < startIndex + numCurConstraints; i++)
748 { 748 {
749 startConstraint.Add(_sortedConstraints[i]); 749 startConstraint.Add(_sortedConstraints[i]);
750 } 750 }
751 751
752 _solver.SolveGroup(bodies, manifolds, numManifolds, startConstraint, _solverInfo, _debugDrawer); 752 _solver.SolveGroup(bodies, manifolds, numManifolds, startConstraint, _solverInfo, _debugDrawer);
753 } 753 }
754 } 754 }
755 } 755 }
756 756
757 internal class DebugDrawCallback : ITriangleIndexCallback, ITriangleCallback 757 internal class DebugDrawCallback : ITriangleIndexCallback, ITriangleCallback
758 { 758 {
759 private IDebugDraw _debugDrawer; 759 private IDebugDraw _debugDrawer;
760 private Vector3 _color; 760 private Vector3 _color;
761 private Matrix _worldTrans; 761 private Matrix _worldTrans;
762 762
763 public DebugDrawCallback(IDebugDraw debugDrawer, Matrix worldTrans, Vector3 color) 763 public DebugDrawCallback(IDebugDraw debugDrawer, Matrix worldTrans, Vector3 color)
764 { 764 {
765 _debugDrawer = debugDrawer; 765 _debugDrawer = debugDrawer;
766 _worldTrans = worldTrans; 766 _worldTrans = worldTrans;
767 _color = color; 767 _color = color;
768 } 768 }
769 769
770 public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) 770 public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex)
771 { 771 {
772 ProcessTriangle(triangle, partId, triangleIndex); 772 ProcessTriangle(triangle, partId, triangleIndex);
773 } 773 }
774 774
775 #region ITriangleCallback Members 775 #region ITriangleCallback Members
776 776
777 public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) 777 public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)
778 { 778 {
779 Vector3 wv0, wv1, wv2; 779 Vector3 wv0, wv1, wv2;
780 wv0 = Vector3.TransformNormal(triangle[0], _worldTrans); 780 wv0 = Vector3.TransformNormal(triangle[0], _worldTrans);
781 wv1 = Vector3.TransformNormal(triangle[1], _worldTrans); 781 wv1 = Vector3.TransformNormal(triangle[1], _worldTrans);
782 wv2 = Vector3.TransformNormal(triangle[2], _worldTrans); 782 wv2 = Vector3.TransformNormal(triangle[2], _worldTrans);
783 _debugDrawer.DrawLine(wv0, wv1, _color); 783 _debugDrawer.DrawLine(wv0, wv1, _color);
784 _debugDrawer.DrawLine(wv1, wv2, _color); 784 _debugDrawer.DrawLine(wv1, wv2, _color);
785 _debugDrawer.DrawLine(wv2, wv0, _color); 785 _debugDrawer.DrawLine(wv2, wv0, _color);
786 } 786 }
787 787
788 #endregion 788 #endregion
789 } 789 }
790} \ No newline at end of file 790} \ No newline at end of file
generated by cgit v1.1 at 2024-08-26 06:55:07 +0000