aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSPlugin
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/BulletSPlugin')
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs885
-rw-r--r--OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs19
-rw-r--r--OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs96
-rw-r--r--OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs193
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs7
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs113
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs10
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSMotors.cs47
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSParam.cs47
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs10
-rw-r--r--OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs94
-rw-r--r--OpenSim/Region/Physics/BulletSPlugin/BSScene.cs9
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt35
13 files changed, 1015 insertions, 550 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
index b6ff52b..49b1730 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
@@ -129,7 +129,12 @@ private sealed class BulletConstraintXNA : BulletConstraint
129 get { return "XNAConstraint"; } 129 get { return "XNAConstraint"; }
130 } 130 }
131} 131}
132 internal int m_maxCollisions;
133 internal CollisionDesc[] m_collisionArray;
132 134
135 internal int m_maxUpdatesPerFrame;
136 internal EntityProperties[] m_updateArray;
137
133 private static int m_collisionsThisFrame; 138 private static int m_collisionsThisFrame;
134 private BSScene PhysicsScene { get; set; } 139 private BSScene PhysicsScene { get; set; }
135 140
@@ -148,92 +153,98 @@ private sealed class BulletConstraintXNA : BulletConstraint
148 /// <param name="p_2"></param> 153 /// <param name="p_2"></param>
149 public override bool RemoveObjectFromWorld(BulletWorld pWorld, BulletBody pBody) 154 public override bool RemoveObjectFromWorld(BulletWorld pWorld, BulletBody pBody)
150 { 155 {
151 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 156 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
152 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 157 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
153 world.RemoveRigidBody(body); 158 world.RemoveRigidBody(body);
154 return true; 159 return true;
155 } 160 }
156 161
157 public override bool AddConstraintToWorld(BulletWorld world, BulletConstraint constrain, bool disableCollisionsBetweenLinkedObjects) 162 public override bool AddConstraintToWorld(BulletWorld pWorld, BulletConstraint pConstraint, bool pDisableCollisionsBetweenLinkedObjects)
158 { 163 {
159 /* TODO */ 164 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
160 return false; 165 TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
166 world.AddConstraint(constraint, pDisableCollisionsBetweenLinkedObjects);
167
168 return true;
169
161 } 170 }
162 171
163 public override bool RemoveConstraintFromWorld(BulletWorld world, BulletConstraint constrain) 172 public override bool RemoveConstraintFromWorld(BulletWorld pWorld, BulletConstraint pConstraint)
164 { 173 {
165 /* TODO */ 174 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
166 return false; 175 TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
176 world.RemoveConstraint(constraint);
177 return true;
167 } 178 }
168 179
169 public override void SetRestitution(BulletBody pBody, float pRestitution) 180 public override void SetRestitution(BulletBody pCollisionObject, float pRestitution)
170 { 181 {
171 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 182 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
172 body.SetRestitution(pRestitution); 183 collisionObject.SetRestitution(pRestitution);
173 } 184 }
174 185
175 public override int GetShapeType(BulletShape pShape) 186 public override int GetShapeType(BulletShape pShape)
176 { 187 {
177 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 188 CollisionShape shape = (pShape as BulletShapeXNA).shape;
178 return (int)shape.GetShapeType(); 189 return (int)shape.GetShapeType();
179 } 190 }
180 public override void SetMargin(BulletShape pShape, float pMargin) 191 public override void SetMargin(BulletShape pShape, float pMargin)
181 { 192 {
182 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 193 CollisionShape shape = (pShape as BulletShapeXNA).shape;
183 shape.SetMargin(pMargin); 194 shape.SetMargin(pMargin);
184 } 195 }
185 196
186 public override float GetMargin(BulletShape pShape) 197 public override float GetMargin(BulletShape pShape)
187 { 198 {
188 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 199 CollisionShape shape = (pShape as BulletShapeXNA).shape;
189 return shape.GetMargin(); 200 return shape.GetMargin();
190 } 201 }
191 202
192 public override void SetLocalScaling(BulletShape pShape, Vector3 pScale) 203 public override void SetLocalScaling(BulletShape pShape, Vector3 pScale)
193 { 204 {
194 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 205 CollisionShape shape = (pShape as BulletShapeXNA).shape;
195 IndexedVector3 vec = new IndexedVector3(pScale.X, pScale.Y, pScale.Z); 206 IndexedVector3 vec = new IndexedVector3(pScale.X, pScale.Y, pScale.Z);
196 shape.SetLocalScaling(ref vec); 207 shape.SetLocalScaling(ref vec);
197 208
198 } 209 }
199 210
200 public override void SetContactProcessingThreshold(BulletBody pBody, float contactprocessingthreshold) 211 public override void SetContactProcessingThreshold(BulletBody pCollisionObject, float contactprocessingthreshold)
201 { 212 {
202 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 213 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
203 body.SetContactProcessingThreshold(contactprocessingthreshold); 214 collisionObject.SetContactProcessingThreshold(contactprocessingthreshold);
204 } 215 }
205 216
206 public override void SetCcdMotionThreshold(BulletBody pBody, float pccdMotionThreashold) 217 public override void SetCcdMotionThreshold(BulletBody pCollisionObject, float pccdMotionThreashold)
207 { 218 {
208 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 219 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
209 body.SetCcdMotionThreshold(pccdMotionThreashold); 220 collisionObject.SetCcdMotionThreshold(pccdMotionThreashold);
210 } 221 }
211 222
212 public override void SetCcdSweptSphereRadius(BulletBody pBody, float pCcdSweptSphereRadius) 223 public override void SetCcdSweptSphereRadius(BulletBody pCollisionObject, float pCcdSweptSphereRadius)
213 { 224 {
214 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 225 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
215 body.SetCcdSweptSphereRadius(pCcdSweptSphereRadius); 226 collisionObject.SetCcdSweptSphereRadius(pCcdSweptSphereRadius);
216 } 227 }
217 228
218 public override void SetAngularFactorV(BulletBody pBody, Vector3 pAngularFactor) 229 public override void SetAngularFactorV(BulletBody pBody, Vector3 pAngularFactor)
219 { 230 {
220 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 231 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
221 body.SetAngularFactor(new IndexedVector3(pAngularFactor.X, pAngularFactor.Y, pAngularFactor.Z)); 232 body.SetAngularFactor(new IndexedVector3(pAngularFactor.X, pAngularFactor.Y, pAngularFactor.Z));
222 } 233 }
223 234
224 public override CollisionFlags AddToCollisionFlags(BulletBody pBody, CollisionFlags pcollisionFlags) 235 public override CollisionFlags AddToCollisionFlags(BulletBody pCollisionObject, CollisionFlags pcollisionFlags)
225 { 236 {
226 CollisionObject body = ((BulletBodyXNA)pBody).body; 237 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
227 CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)body.GetCollisionFlags(); 238 CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)collisionObject.GetCollisionFlags();
228 existingcollisionFlags |= pcollisionFlags; 239 existingcollisionFlags |= pcollisionFlags;
229 body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags); 240 collisionObject.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
230 return (CollisionFlags) (uint) existingcollisionFlags; 241 return (CollisionFlags) (uint) existingcollisionFlags;
231 } 242 }
232 243
233 public override bool AddObjectToWorld(BulletWorld pWorld, BulletBody pBody) 244 public override bool AddObjectToWorld(BulletWorld pWorld, BulletBody pBody)
234 { 245 {
235 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 246 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
236 CollisionObject cbody = ((BulletBodyXNA)pBody).body; 247 CollisionObject cbody = (pBody as BulletBodyXNA).body;
237 RigidBody rbody = cbody as RigidBody; 248 RigidBody rbody = cbody as RigidBody;
238 249
239 // Bullet resets several variables when an object is added to the world. In particular, 250 // Bullet resets several variables when an object is added to the world. In particular,
@@ -259,99 +270,110 @@ private sealed class BulletConstraintXNA : BulletConstraint
259 return true; 270 return true;
260 } 271 }
261 272
262 public override void ForceActivationState(BulletBody pBody, ActivationState pActivationState) 273 public override void ForceActivationState(BulletBody pCollisionObject, ActivationState pActivationState)
263 { 274 {
264 CollisionObject body = ((BulletBodyXNA)pBody).body; 275 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
265 body.ForceActivationState((BulletXNA.BulletCollision.ActivationState)(uint)pActivationState); 276 collisionObject.ForceActivationState((BulletXNA.BulletCollision.ActivationState)(uint)pActivationState);
266 } 277 }
267 278
268 public override void UpdateSingleAabb(BulletWorld pWorld, BulletBody pBody) 279 public override void UpdateSingleAabb(BulletWorld pWorld, BulletBody pCollisionObject)
269 { 280 {
270 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 281 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
271 CollisionObject body = ((BulletBodyXNA)pBody).body; 282 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
272 world.UpdateSingleAabb(body); 283 world.UpdateSingleAabb(collisionObject);
273 } 284 }
274 285
275 public override void UpdateAabbs(BulletWorld world) { /* TODO */ } 286 public override void UpdateAabbs(BulletWorld pWorld) {
276 public override bool GetForceUpdateAllAabbs(BulletWorld world) { /* TODO */ return false; } 287 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
277 public override void SetForceUpdateAllAabbs(BulletWorld world, bool force) { /* TODO */ } 288 world.UpdateAabbs();
289 }
290 public override bool GetForceUpdateAllAabbs(BulletWorld pWorld) {
291 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
292 return world.GetForceUpdateAllAabbs();
293
294 }
295 public override void SetForceUpdateAllAabbs(BulletWorld pWorld, bool pForce)
296 {
297 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
298 world.SetForceUpdateAllAabbs(pForce);
299 }
278 300
279 public override bool SetCollisionGroupMask(BulletBody pBody, uint pGroup, uint pMask) 301 public override bool SetCollisionGroupMask(BulletBody pCollisionObject, uint pGroup, uint pMask)
280 { 302 {
281 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 303 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
282 body.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup; 304 collisionObject.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
283 body.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup; 305 collisionObject.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
284 if ((uint) body.GetBroadphaseHandle().m_collisionFilterGroup == 0) 306 if ((uint) collisionObject.GetBroadphaseHandle().m_collisionFilterGroup == 0)
285 return false; 307 return false;
286 return true; 308 return true;
287 } 309 }
288 310
289 public override void ClearAllForces(BulletBody pBody) 311 public override void ClearAllForces(BulletBody pCollisionObject)
290 { 312 {
291 CollisionObject body = ((BulletBodyXNA)pBody).body; 313 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
292 IndexedVector3 zeroVector = new IndexedVector3(0, 0, 0); 314 IndexedVector3 zeroVector = new IndexedVector3(0, 0, 0);
293 body.SetInterpolationLinearVelocity(ref zeroVector); 315 collisionObject.SetInterpolationLinearVelocity(ref zeroVector);
294 body.SetInterpolationAngularVelocity(ref zeroVector); 316 collisionObject.SetInterpolationAngularVelocity(ref zeroVector);
295 IndexedMatrix bodytransform = body.GetWorldTransform(); 317 IndexedMatrix bodytransform = collisionObject.GetWorldTransform();
296 318
297 body.SetInterpolationWorldTransform(ref bodytransform); 319 collisionObject.SetInterpolationWorldTransform(ref bodytransform);
298 320
299 if (body is RigidBody) 321 if (collisionObject is RigidBody)
300 { 322 {
301 RigidBody rigidbody = body as RigidBody; 323 RigidBody rigidbody = collisionObject as RigidBody;
302 rigidbody.SetLinearVelocity(zeroVector); 324 rigidbody.SetLinearVelocity(zeroVector);
303 rigidbody.SetAngularVelocity(zeroVector); 325 rigidbody.SetAngularVelocity(zeroVector);
304 rigidbody.ClearForces(); 326 rigidbody.ClearForces();
305 } 327 }
306 } 328 }
307 329
308 public override void SetInterpolationAngularVelocity(BulletBody pBody, Vector3 pVector3) 330 public override void SetInterpolationAngularVelocity(BulletBody pCollisionObject, Vector3 pVector3)
309 { 331 {
310 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 332 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
311 IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z); 333 IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z);
312 body.SetInterpolationAngularVelocity(ref vec); 334 collisionObject.SetInterpolationAngularVelocity(ref vec);
313 } 335 }
314 336
315 public override void SetAngularVelocity(BulletBody pBody, Vector3 pVector3) 337 public override void SetAngularVelocity(BulletBody pBody, Vector3 pVector3)
316 { 338 {
317 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 339 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
318 IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z); 340 IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z);
319 body.SetAngularVelocity(ref vec); 341 body.SetAngularVelocity(ref vec);
320 } 342 }
321 public override Vector3 GetTotalForce(BulletBody pBody) 343 public override Vector3 GetTotalForce(BulletBody pBody)
322 { 344 {
323 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 345 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
324 IndexedVector3 iv3 = body.GetTotalForce(); 346 IndexedVector3 iv3 = body.GetTotalForce();
325 return new Vector3(iv3.X, iv3.Y, iv3.Z); 347 return new Vector3(iv3.X, iv3.Y, iv3.Z);
326 } 348 }
327 public override Vector3 GetTotalTorque(BulletBody pBody) 349 public override Vector3 GetTotalTorque(BulletBody pBody)
328 { 350 {
329 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 351 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
330 IndexedVector3 iv3 = body.GetTotalTorque(); 352 IndexedVector3 iv3 = body.GetTotalTorque();
331 return new Vector3(iv3.X, iv3.Y, iv3.Z); 353 return new Vector3(iv3.X, iv3.Y, iv3.Z);
332 } 354 }
333 public override Vector3 GetInvInertiaDiagLocal(BulletBody pBody) 355 public override Vector3 GetInvInertiaDiagLocal(BulletBody pBody)
334 { 356 {
335 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 357 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
336 IndexedVector3 iv3 = body.GetInvInertiaDiagLocal(); 358 IndexedVector3 iv3 = body.GetInvInertiaDiagLocal();
337 return new Vector3(iv3.X, iv3.Y, iv3.Z); 359 return new Vector3(iv3.X, iv3.Y, iv3.Z);
338 } 360 }
339 public override void SetInvInertiaDiagLocal(BulletBody pBody, Vector3 inert) 361 public override void SetInvInertiaDiagLocal(BulletBody pBody, Vector3 inert)
340 { 362 {
341 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 363 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
342 IndexedVector3 iv3 = new IndexedVector3(inert.X, inert.Y, inert.Z); 364 IndexedVector3 iv3 = new IndexedVector3(inert.X, inert.Y, inert.Z);
343 body.SetInvInertiaDiagLocal(ref iv3); 365 body.SetInvInertiaDiagLocal(ref iv3);
344 } 366 }
345 public override void ApplyForce(BulletBody pBody, Vector3 force, Vector3 pos) 367 public override void ApplyForce(BulletBody pBody, Vector3 force, Vector3 pos)
346 { 368 {
347 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 369 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
348 IndexedVector3 forceiv3 = new IndexedVector3(force.X, force.Y, force.Z); 370 IndexedVector3 forceiv3 = new IndexedVector3(force.X, force.Y, force.Z);
349 IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z); 371 IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
350 body.ApplyForce(ref forceiv3, ref posiv3); 372 body.ApplyForce(ref forceiv3, ref posiv3);
351 } 373 }
352 public override void ApplyImpulse(BulletBody pBody, Vector3 imp, Vector3 pos) 374 public override void ApplyImpulse(BulletBody pBody, Vector3 imp, Vector3 pos)
353 { 375 {
354 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 376 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
355 IndexedVector3 impiv3 = new IndexedVector3(imp.X, imp.Y, imp.Z); 377 IndexedVector3 impiv3 = new IndexedVector3(imp.X, imp.Y, imp.Z);
356 IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z); 378 IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
357 body.ApplyImpulse(ref impiv3, ref posiv3); 379 body.ApplyImpulse(ref impiv3, ref posiv3);
@@ -359,32 +381,32 @@ private sealed class BulletConstraintXNA : BulletConstraint
359 381
360 public override void ClearForces(BulletBody pBody) 382 public override void ClearForces(BulletBody pBody)
361 { 383 {
362 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 384 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
363 body.ClearForces(); 385 body.ClearForces();
364 } 386 }
365 387
366 public override void SetTranslation(BulletBody pBody, Vector3 _position, Quaternion _orientation) 388 public override void SetTranslation(BulletBody pCollisionObject, Vector3 _position, Quaternion _orientation)
367 { 389 {
368 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 390 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
369 IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z); 391 IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
370 IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z, 392 IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
371 _orientation.W); 393 _orientation.W);
372 IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion); 394 IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
373 mat._origin = vposition; 395 mat._origin = vposition;
374 body.SetWorldTransform(mat); 396 collisionObject.SetWorldTransform(mat);
375 397
376 } 398 }
377 399
378 public override Vector3 GetPosition(BulletBody pBody) 400 public override Vector3 GetPosition(BulletBody pCollisionObject)
379 { 401 {
380 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 402 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
381 IndexedVector3 pos = body.GetInterpolationWorldTransform()._origin; 403 IndexedVector3 pos = collisionObject.GetInterpolationWorldTransform()._origin;
382 return new Vector3(pos.X, pos.Y, pos.Z); 404 return new Vector3(pos.X, pos.Y, pos.Z);
383 } 405 }
384 406
385 public override Vector3 CalculateLocalInertia(BulletShape pShape, float pphysMass) 407 public override Vector3 CalculateLocalInertia(BulletShape pShape, float pphysMass)
386 { 408 {
387 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 409 CollisionShape shape = (pShape as BulletShapeXNA).shape;
388 IndexedVector3 inertia = IndexedVector3.Zero; 410 IndexedVector3 inertia = IndexedVector3.Zero;
389 shape.CalculateLocalInertia(pphysMass, out inertia); 411 shape.CalculateLocalInertia(pphysMass, out inertia);
390 return new Vector3(inertia.X, inertia.Y, inertia.Z); 412 return new Vector3(inertia.X, inertia.Y, inertia.Z);
@@ -392,7 +414,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
392 414
393 public override void SetMassProps(BulletBody pBody, float pphysMass, Vector3 plocalInertia) 415 public override void SetMassProps(BulletBody pBody, float pphysMass, Vector3 plocalInertia)
394 { 416 {
395 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 417 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
396 IndexedVector3 inertia = new IndexedVector3(plocalInertia.X, plocalInertia.Y, plocalInertia.Z); 418 IndexedVector3 inertia = new IndexedVector3(plocalInertia.X, plocalInertia.Y, plocalInertia.Z);
397 body.SetMassProps(pphysMass, inertia); 419 body.SetMassProps(pphysMass, inertia);
398 } 420 }
@@ -400,73 +422,90 @@ private sealed class BulletConstraintXNA : BulletConstraint
400 422
401 public override void SetObjectForce(BulletBody pBody, Vector3 _force) 423 public override void SetObjectForce(BulletBody pBody, Vector3 _force)
402 { 424 {
403 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 425 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
404 IndexedVector3 force = new IndexedVector3(_force.X, _force.Y, _force.Z); 426 IndexedVector3 force = new IndexedVector3(_force.X, _force.Y, _force.Z);
405 body.SetTotalForce(ref force); 427 body.SetTotalForce(ref force);
406 } 428 }
407 429
408 public override void SetFriction(BulletBody pBody, float _currentFriction) 430 public override void SetFriction(BulletBody pCollisionObject, float _currentFriction)
409 { 431 {
410 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 432 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
411 body.SetFriction(_currentFriction); 433 collisionObject.SetFriction(_currentFriction);
412 } 434 }
413 435
414 public override void SetLinearVelocity(BulletBody pBody, Vector3 _velocity) 436 public override void SetLinearVelocity(BulletBody pBody, Vector3 _velocity)
415 { 437 {
416 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 438 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
417 IndexedVector3 velocity = new IndexedVector3(_velocity.X, _velocity.Y, _velocity.Z); 439 IndexedVector3 velocity = new IndexedVector3(_velocity.X, _velocity.Y, _velocity.Z);
418 body.SetLinearVelocity(velocity); 440 body.SetLinearVelocity(velocity);
419 } 441 }
420 442
421 public override void Activate(BulletBody pBody, bool pforceactivation) 443 public override void Activate(BulletBody pCollisionObject, bool pforceactivation)
422 { 444 {
423 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 445 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
424 body.Activate(pforceactivation); 446 collisionObject.Activate(pforceactivation);
425 447
426 } 448 }
427 449
428 public override Quaternion GetOrientation(BulletBody pBody) 450 public override Quaternion GetOrientation(BulletBody pCollisionObject)
429 { 451 {
430 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 452 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
431 IndexedQuaternion mat = body.GetInterpolationWorldTransform().GetRotation(); 453 IndexedQuaternion mat = collisionObject.GetInterpolationWorldTransform().GetRotation();
432 return new Quaternion(mat.X, mat.Y, mat.Z, mat.W); 454 return new Quaternion(mat.X, mat.Y, mat.Z, mat.W);
433 } 455 }
434 456
435 public override CollisionFlags RemoveFromCollisionFlags(BulletBody pBody, CollisionFlags pcollisionFlags) 457 public override CollisionFlags RemoveFromCollisionFlags(BulletBody pCollisionObject, CollisionFlags pcollisionFlags)
436 { 458 {
437 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 459 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
438 CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)body.GetCollisionFlags(); 460 CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)collisionObject.GetCollisionFlags();
439 existingcollisionFlags &= ~pcollisionFlags; 461 existingcollisionFlags &= ~pcollisionFlags;
440 body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags); 462 collisionObject.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
441 return (CollisionFlags)(uint)existingcollisionFlags; 463 return (CollisionFlags)(uint)existingcollisionFlags;
442 } 464 }
443 465
444 public override float GetCcdMotionThreshold(BulletBody obj) { /* TODO */ return 0f; } 466 public override float GetCcdMotionThreshold(BulletBody pCollisionObject)
467 {
468 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
469 return collisionObject.GetCcdSquareMotionThreshold();
470 }
445 471
446 public override float GetCcdSweptSphereRadius(BulletBody obj) { /* TODO */ return 0f; } 472 public override float GetCcdSweptSphereRadius(BulletBody pCollisionObject)
473 {
474 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
475 return collisionObject.GetCcdSweptSphereRadius();
476
477 }
447 478
448 public override IntPtr GetUserPointer(BulletBody obj) { /* TODO */ return IntPtr.Zero; } 479 public override IntPtr GetUserPointer(BulletBody pCollisionObject)
480 {
481 CollisionObject shape = (pCollisionObject as BulletBodyXNA).body;
482 return (IntPtr)shape.GetUserPointer();
483 }
449 484
450 public override void SetUserPointer(BulletBody obj, IntPtr val) { /* TODO */ } 485 public override void SetUserPointer(BulletBody pCollisionObject, IntPtr val)
486 {
487 CollisionObject shape = (pCollisionObject as BulletBodyXNA).body;
488 shape.SetUserPointer(val);
489 }
451 490
452 public override void SetGravity(BulletBody pBody, Vector3 pGravity) 491 public override void SetGravity(BulletBody pBody, Vector3 pGravity)
453 { 492 {
454 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 493 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
455 IndexedVector3 gravity = new IndexedVector3(pGravity.X, pGravity.Y, pGravity.Z); 494 IndexedVector3 gravity = new IndexedVector3(pGravity.X, pGravity.Y, pGravity.Z);
456 body.SetGravity(gravity); 495 body.SetGravity(gravity);
457 } 496 }
458 497
459 public override bool DestroyConstraint(BulletWorld pWorld, BulletConstraint pConstraint) 498 public override bool DestroyConstraint(BulletWorld pWorld, BulletConstraint pConstraint)
460 { 499 {
461 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 500 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
462 TypedConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain; 501 TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
463 world.RemoveConstraint(constraint); 502 world.RemoveConstraint(constraint);
464 return true; 503 return true;
465 } 504 }
466 505
467 public override bool SetLinearLimits(BulletConstraint pConstraint, Vector3 low, Vector3 high) 506 public override bool SetLinearLimits(BulletConstraint pConstraint, Vector3 low, Vector3 high)
468 { 507 {
469 Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint; 508 Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
470 IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z); 509 IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z);
471 IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z); 510 IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z);
472 constraint.SetLinearLowerLimit(lowlimit); 511 constraint.SetLinearLowerLimit(lowlimit);
@@ -476,7 +515,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
476 515
477 public override bool SetAngularLimits(BulletConstraint pConstraint, Vector3 low, Vector3 high) 516 public override bool SetAngularLimits(BulletConstraint pConstraint, Vector3 low, Vector3 high)
478 { 517 {
479 Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint; 518 Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
480 IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z); 519 IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z);
481 IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z); 520 IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z);
482 constraint.SetAngularLowerLimit(lowlimit); 521 constraint.SetAngularLowerLimit(lowlimit);
@@ -486,20 +525,20 @@ private sealed class BulletConstraintXNA : BulletConstraint
486 525
487 public override void SetConstraintNumSolverIterations(BulletConstraint pConstraint, float cnt) 526 public override void SetConstraintNumSolverIterations(BulletConstraint pConstraint, float cnt)
488 { 527 {
489 Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint; 528 Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
490 constraint.SetOverrideNumSolverIterations((int)cnt); 529 constraint.SetOverrideNumSolverIterations((int)cnt);
491 } 530 }
492 531
493 public override bool CalculateTransforms(BulletConstraint pConstraint) 532 public override bool CalculateTransforms(BulletConstraint pConstraint)
494 { 533 {
495 Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint; 534 Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
496 constraint.CalculateTransforms(); 535 constraint.CalculateTransforms();
497 return true; 536 return true;
498 } 537 }
499 538
500 public override void SetConstraintEnable(BulletConstraint pConstraint, float p_2) 539 public override void SetConstraintEnable(BulletConstraint pConstraint, float p_2)
501 { 540 {
502 Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint; 541 Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
503 constraint.SetEnabled((p_2 == 0) ? false : true); 542 constraint.SetEnabled((p_2 == 0) ? false : true);
504 } 543 }
505 544
@@ -508,9 +547,9 @@ private sealed class BulletConstraintXNA : BulletConstraint
508 public override BulletConstraint Create6DofConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies) 547 public override BulletConstraint Create6DofConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
509 548
510 { 549 {
511 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 550 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
512 RigidBody body1 = ((BulletBodyXNA)pBody1).rigidBody; 551 RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
513 RigidBody body2 = ((BulletBodyXNA)pBody2).rigidBody; 552 RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
514 IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z); 553 IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
515 IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W); 554 IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
516 IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot); 555 IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
@@ -542,9 +581,9 @@ private sealed class BulletConstraintXNA : BulletConstraint
542 /// <returns></returns> 581 /// <returns></returns>
543 public override BulletConstraint Create6DofConstraintToPoint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, Vector3 pjoinPoint, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies) 582 public override BulletConstraint Create6DofConstraintToPoint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, Vector3 pjoinPoint, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
544 { 583 {
545 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 584 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
546 RigidBody body1 = ((BulletBodyXNA)pBody1).rigidBody; 585 RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
547 RigidBody body2 = ((BulletBodyXNA)pBody2).rigidBody; 586 RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
548 IndexedMatrix frame1 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0)); 587 IndexedMatrix frame1 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0));
549 IndexedMatrix frame2 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0)); 588 IndexedMatrix frame2 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0));
550 589
@@ -563,7 +602,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
563 //SetFrames(m_constraint.ptr, frameA, frameArot, frameB, frameBrot); 602 //SetFrames(m_constraint.ptr, frameA, frameArot, frameB, frameBrot);
564 public override bool SetFrames(BulletConstraint pConstraint, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot) 603 public override bool SetFrames(BulletConstraint pConstraint, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot)
565 { 604 {
566 Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint; 605 Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
567 IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z); 606 IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
568 IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W); 607 IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
569 IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot); 608 IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
@@ -579,109 +618,110 @@ private sealed class BulletConstraintXNA : BulletConstraint
579 618
580 public override Vector3 GetLinearVelocity(BulletBody pBody) 619 public override Vector3 GetLinearVelocity(BulletBody pBody)
581 { 620 {
582 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 621 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
583 IndexedVector3 iv3 = body.GetLinearVelocity(); 622 IndexedVector3 iv3 = body.GetLinearVelocity();
584 return new Vector3(iv3.X, iv3.Y, iv3.Z); 623 return new Vector3(iv3.X, iv3.Y, iv3.Z);
585 } 624 }
586 public override Vector3 GetAngularVelocity(BulletBody pBody) 625 public override Vector3 GetAngularVelocity(BulletBody pBody)
587 { 626 {
588 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 627 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
589 IndexedVector3 iv3 = body.GetAngularVelocity(); 628 IndexedVector3 iv3 = body.GetAngularVelocity();
590 return new Vector3(iv3.X, iv3.Y, iv3.Z); 629 return new Vector3(iv3.X, iv3.Y, iv3.Z);
591 } 630 }
592 public override Vector3 GetVelocityInLocalPoint(BulletBody pBody, Vector3 pos) 631 public override Vector3 GetVelocityInLocalPoint(BulletBody pBody, Vector3 pos)
593 { 632 {
594 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 633 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
595 IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z); 634 IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
596 IndexedVector3 iv3 = body.GetVelocityInLocalPoint(ref posiv3); 635 IndexedVector3 iv3 = body.GetVelocityInLocalPoint(ref posiv3);
597 return new Vector3(iv3.X, iv3.Y, iv3.Z); 636 return new Vector3(iv3.X, iv3.Y, iv3.Z);
598 } 637 }
599 public override void Translate(BulletBody pBody, Vector3 trans) 638 public override void Translate(BulletBody pCollisionObject, Vector3 trans)
600 { 639 {
601 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 640 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
641 collisionObject.Translate(new IndexedVector3(trans.X,trans.Y,trans.Z));
602 } 642 }
603 public override void UpdateDeactivation(BulletBody pBody, float timeStep) 643 public override void UpdateDeactivation(BulletBody pBody, float timeStep)
604 { 644 {
605 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 645 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
606 body.UpdateDeactivation(timeStep); 646 body.UpdateDeactivation(timeStep);
607 } 647 }
608 648
609 public override bool WantsSleeping(BulletBody pBody) 649 public override bool WantsSleeping(BulletBody pBody)
610 { 650 {
611 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 651 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
612 return body.WantsSleeping(); 652 return body.WantsSleeping();
613 } 653 }
614 654
615 public override void SetAngularFactor(BulletBody pBody, float factor) 655 public override void SetAngularFactor(BulletBody pBody, float factor)
616 { 656 {
617 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 657 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
618 body.SetAngularFactor(factor); 658 body.SetAngularFactor(factor);
619 } 659 }
620 660
621 public override Vector3 GetAngularFactor(BulletBody pBody) 661 public override Vector3 GetAngularFactor(BulletBody pBody)
622 { 662 {
623 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 663 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
624 IndexedVector3 iv3 = body.GetAngularFactor(); 664 IndexedVector3 iv3 = body.GetAngularFactor();
625 return new Vector3(iv3.X, iv3.Y, iv3.Z); 665 return new Vector3(iv3.X, iv3.Y, iv3.Z);
626 } 666 }
627 667
628 public override bool IsInWorld(BulletWorld pWorld, BulletBody pBody) 668 public override bool IsInWorld(BulletWorld pWorld, BulletBody pCollisionObject)
629 { 669 {
630 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 670 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
631 CollisionObject body = ((BulletBodyXNA)pBody).body; 671 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
632 return world.IsInWorld(body); 672 return world.IsInWorld(collisionObject);
633 } 673 }
634 674
635 public override void AddConstraintRef(BulletBody pBody, BulletConstraint pConstrain) 675 public override void AddConstraintRef(BulletBody pBody, BulletConstraint pConstraint)
636 { 676 {
637 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 677 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
638 TypedConstraint constrain = ((BulletConstraintXNA)pConstrain).constrain; 678 TypedConstraint constrain = (pConstraint as BulletConstraintXNA).constrain;
639 body.AddConstraintRef(constrain); 679 body.AddConstraintRef(constrain);
640 } 680 }
641 681
642 public override void RemoveConstraintRef(BulletBody pBody, BulletConstraint pConstrain) 682 public override void RemoveConstraintRef(BulletBody pBody, BulletConstraint pConstraint)
643 { 683 {
644 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 684 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
645 TypedConstraint constrain = ((BulletConstraintXNA)pConstrain).constrain; 685 TypedConstraint constrain = (pConstraint as BulletConstraintXNA).constrain;
646 body.RemoveConstraintRef(constrain); 686 body.RemoveConstraintRef(constrain);
647 } 687 }
648 688
649 public override BulletConstraint GetConstraintRef(BulletBody pBody, int index) 689 public override BulletConstraint GetConstraintRef(BulletBody pBody, int index)
650 { 690 {
651 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 691 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
652 return new BulletConstraintXNA(body.GetConstraintRef(index)); 692 return new BulletConstraintXNA(body.GetConstraintRef(index));
653 } 693 }
654 694
655 public override int GetNumConstraintRefs(BulletBody pBody) 695 public override int GetNumConstraintRefs(BulletBody pBody)
656 { 696 {
657 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 697 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
658 return body.GetNumConstraintRefs(); 698 return body.GetNumConstraintRefs();
659 } 699 }
660 700
661 public override void SetInterpolationLinearVelocity(BulletBody pBody, Vector3 VehicleVelocity) 701 public override void SetInterpolationLinearVelocity(BulletBody pCollisionObject, Vector3 VehicleVelocity)
662 { 702 {
663 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 703 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
664 IndexedVector3 velocity = new IndexedVector3(VehicleVelocity.X, VehicleVelocity.Y, VehicleVelocity.Z); 704 IndexedVector3 velocity = new IndexedVector3(VehicleVelocity.X, VehicleVelocity.Y, VehicleVelocity.Z);
665 body.SetInterpolationLinearVelocity(ref velocity); 705 collisionObject.SetInterpolationLinearVelocity(ref velocity);
666 } 706 }
667 707
668 public override bool UseFrameOffset(BulletConstraint pConstraint, float onOff) 708 public override bool UseFrameOffset(BulletConstraint pConstraint, float onOff)
669 { 709 {
670 Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint; 710 Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
671 constraint.SetUseFrameOffset((onOff == 0) ? false : true); 711 constraint.SetUseFrameOffset((onOff == 0) ? false : true);
672 return true; 712 return true;
673 } 713 }
674 //SetBreakingImpulseThreshold(m_constraint.ptr, threshold); 714 //SetBreakingImpulseThreshold(m_constraint.ptr, threshold);
675 public override bool SetBreakingImpulseThreshold(BulletConstraint pConstraint, float threshold) 715 public override bool SetBreakingImpulseThreshold(BulletConstraint pConstraint, float threshold)
676 { 716 {
677 Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint; 717 Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
678 constraint.SetBreakingImpulseThreshold(threshold); 718 constraint.SetBreakingImpulseThreshold(threshold);
679 return true; 719 return true;
680 } 720 }
681 //BulletSimAPI.SetAngularDamping(Prim.PhysBody.ptr, angularDamping); 721 //BulletSimAPI.SetAngularDamping(Prim.PhysBody.ptr, angularDamping);
682 public override void SetAngularDamping(BulletBody pBody, float angularDamping) 722 public override void SetAngularDamping(BulletBody pBody, float angularDamping)
683 { 723 {
684 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 724 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
685 float lineardamping = body.GetLinearDamping(); 725 float lineardamping = body.GetLinearDamping();
686 body.SetDamping(lineardamping, angularDamping); 726 body.SetDamping(lineardamping, angularDamping);
687 727
@@ -689,111 +729,183 @@ private sealed class BulletConstraintXNA : BulletConstraint
689 729
690 public override void UpdateInertiaTensor(BulletBody pBody) 730 public override void UpdateInertiaTensor(BulletBody pBody)
691 { 731 {
692 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 732 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
693 body.UpdateInertiaTensor(); 733 body.UpdateInertiaTensor();
694 } 734 }
695 735
696 public override void RecalculateCompoundShapeLocalAabb(BulletShape pCompoundShape) 736 public override void RecalculateCompoundShapeLocalAabb(BulletShape pCompoundShape)
697 { 737 {
698 CompoundShape shape = ((BulletShapeXNA)pCompoundShape).shape as CompoundShape; 738 CompoundShape shape = (pCompoundShape as BulletShapeXNA).shape as CompoundShape;
699 shape.RecalculateLocalAabb(); 739 shape.RecalculateLocalAabb();
700 } 740 }
701 741
702 //BulletSimAPI.GetCollisionFlags(PhysBody.ptr) 742 //BulletSimAPI.GetCollisionFlags(PhysBody.ptr)
703 public override CollisionFlags GetCollisionFlags(BulletBody pBody) 743 public override CollisionFlags GetCollisionFlags(BulletBody pCollisionObject)
704 { 744 {
705 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 745 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
706 uint flags = (uint)body.GetCollisionFlags(); 746 uint flags = (uint)collisionObject.GetCollisionFlags();
707 return (CollisionFlags) flags; 747 return (CollisionFlags) flags;
708 } 748 }
709 749
710 public override void SetDamping(BulletBody pBody, float pLinear, float pAngular) 750 public override void SetDamping(BulletBody pBody, float pLinear, float pAngular)
711 { 751 {
712 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 752 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
713 body.SetDamping(pLinear, pAngular); 753 body.SetDamping(pLinear, pAngular);
714 } 754 }
715 //PhysBody.ptr, PhysicsScene.Params.deactivationTime); 755 //PhysBody.ptr, PhysicsScene.Params.deactivationTime);
716 public override void SetDeactivationTime(BulletBody pBody, float pDeactivationTime) 756 public override void SetDeactivationTime(BulletBody pCollisionObject, float pDeactivationTime)
717 { 757 {
718 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 758 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
719 body.SetDeactivationTime(pDeactivationTime); 759 collisionObject.SetDeactivationTime(pDeactivationTime);
720 } 760 }
721 //SetSleepingThresholds(PhysBody.ptr, PhysicsScene.Params.linearSleepingThreshold, PhysicsScene.Params.angularSleepingThreshold); 761 //SetSleepingThresholds(PhysBody.ptr, PhysicsScene.Params.linearSleepingThreshold, PhysicsScene.Params.angularSleepingThreshold);
722 public override void SetSleepingThresholds(BulletBody pBody, float plinearSleepingThreshold, float pangularSleepingThreshold) 762 public override void SetSleepingThresholds(BulletBody pBody, float plinearSleepingThreshold, float pangularSleepingThreshold)
723 { 763 {
724 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 764 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
725 body.SetSleepingThresholds(plinearSleepingThreshold, pangularSleepingThreshold); 765 body.SetSleepingThresholds(plinearSleepingThreshold, pangularSleepingThreshold);
726 } 766 }
727 767
728 public override CollisionObjectTypes GetBodyType(BulletBody pBody) 768 public override CollisionObjectTypes GetBodyType(BulletBody pCollisionObject)
729 { 769 {
730 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 770 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
731 return (CollisionObjectTypes)(int) body.GetInternalType(); 771 return (CollisionObjectTypes)(int) collisionObject.GetInternalType();
732 } 772 }
733 773
734 public override void ApplyGravity(BulletBody obj) { /* TODO */ } 774 public override void ApplyGravity(BulletBody pBody)
775 {
735 776
736 public override Vector3 GetGravity(BulletBody obj) { /* TODO */ return Vector3.Zero; } 777 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
778 body.ApplyGravity();
779 }
737 780
738 public override void SetLinearDamping(BulletBody obj, float lin_damping) { /* TODO */ } 781 public override Vector3 GetGravity(BulletBody pBody)
782 {
783 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
784 IndexedVector3 gravity = body.GetGravity();
785 return new Vector3(gravity.X, gravity.Y, gravity.Z);
786 }
739 787
740 public override float GetLinearDamping(BulletBody obj) { /* TODO */ return 0f; } 788 public override void SetLinearDamping(BulletBody pBody, float lin_damping)
789 {
790 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
791 float angularDamping = body.GetAngularDamping();
792 body.SetDamping(lin_damping, angularDamping);
793 }
741 794
742 public override float GetAngularDamping(BulletBody obj) { /* TODO */ return 0f; } 795 public override float GetLinearDamping(BulletBody pBody)
796 {
797 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
798 return body.GetLinearDamping();
799 }
800
801 public override float GetAngularDamping(BulletBody pBody)
802 {
803 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
804 return body.GetAngularDamping();
805 }
743 806
744 public override float GetLinearSleepingThreshold(BulletBody obj) { /* TODO */ return 0f; } 807 public override float GetLinearSleepingThreshold(BulletBody pBody)
808 {
809 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
810 return body.GetLinearSleepingThreshold();
811 }
745 812
746 public override void ApplyDamping(BulletBody obj, float timeStep) { /* TODO */ } 813 public override void ApplyDamping(BulletBody pBody, float timeStep)
814 {
815 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
816 body.ApplyDamping(timeStep);
817 }
747 818
748 public override Vector3 GetLinearFactor(BulletBody obj) { /* TODO */ return Vector3.Zero; } 819 public override Vector3 GetLinearFactor(BulletBody pBody)
820 {
821 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
822 IndexedVector3 linearFactor = body.GetLinearFactor();
823 return new Vector3(linearFactor.X, linearFactor.Y, linearFactor.Z);
824 }
749 825
750 public override void SetLinearFactor(BulletBody obj, Vector3 factor) { /* TODO */ } 826 public override void SetLinearFactor(BulletBody pBody, Vector3 factor)
827 {
828 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
829 body.SetLinearFactor(new IndexedVector3(factor.X, factor.Y, factor.Z));
830 }
751 831
752 public override void SetCenterOfMassByPosRot(BulletBody obj, Vector3 pos, Quaternion rot) { /* TODO */ } 832 public override void SetCenterOfMassByPosRot(BulletBody pBody, Vector3 pos, Quaternion rot)
833 {
834 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
835 IndexedQuaternion quat = new IndexedQuaternion(rot.X, rot.Y, rot.Z,rot.W);
836 IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(quat);
837 mat._origin = new IndexedVector3(pos.X, pos.Y, pos.Z);
838 body.SetCenterOfMassTransform( ref mat);
839 /* TODO: double check this */
840 }
753 841
754 //BulletSimAPI.ApplyCentralForce(PhysBody.ptr, fSum); 842 //BulletSimAPI.ApplyCentralForce(PhysBody.ptr, fSum);
755 public override void ApplyCentralForce(BulletBody pBody, Vector3 pfSum) 843 public override void ApplyCentralForce(BulletBody pBody, Vector3 pfSum)
756 { 844 {
757 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 845 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
758 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z); 846 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
759 body.ApplyCentralForce(ref fSum); 847 body.ApplyCentralForce(ref fSum);
760 } 848 }
761 public override void ApplyCentralImpulse(BulletBody pBody, Vector3 pfSum) 849 public override void ApplyCentralImpulse(BulletBody pBody, Vector3 pfSum)
762 { 850 {
763 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 851 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
764 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z); 852 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
765 body.ApplyCentralImpulse(ref fSum); 853 body.ApplyCentralImpulse(ref fSum);
766 } 854 }
767 public override void ApplyTorque(BulletBody pBody, Vector3 pfSum) 855 public override void ApplyTorque(BulletBody pBody, Vector3 pfSum)
768 { 856 {
769 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 857 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
770 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z); 858 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
771 body.ApplyTorque(ref fSum); 859 body.ApplyTorque(ref fSum);
772 } 860 }
773 public override void ApplyTorqueImpulse(BulletBody pBody, Vector3 pfSum) 861 public override void ApplyTorqueImpulse(BulletBody pBody, Vector3 pfSum)
774 { 862 {
775 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 863 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
776 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z); 864 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
777 body.ApplyTorqueImpulse(ref fSum); 865 body.ApplyTorqueImpulse(ref fSum);
778 } 866 }
779 867
780 public override void DestroyObject(BulletWorld p, BulletBody p_2) 868 public override void DestroyObject(BulletWorld pWorld, BulletBody pBody)
781 { 869 {
782 //TODO: 870 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
871 CollisionObject co = (pBody as BulletBodyXNA).rigidBody;
872 RigidBody bo = co as RigidBody;
873 if (bo == null)
874 {
875
876 if (world.IsInWorld(co))
877 {
878 world.RemoveCollisionObject(co);
879 }
880 }
881 else
882 {
883
884 if (world.IsInWorld(bo))
885 {
886 world.RemoveRigidBody(bo);
887 }
888 }
889
783 } 890 }
784 891
785 public override void Shutdown(BulletWorld pWorld) 892 public override void Shutdown(BulletWorld pWorld)
786 { 893 {
787 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 894 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
788 world.Cleanup(); 895 world.Cleanup();
789 } 896 }
790 897
791 public override BulletShape DuplicateCollisionShape(BulletWorld sim, BulletShape srcShape, uint id) 898 public override BulletShape DuplicateCollisionShape(BulletWorld pWorld, BulletShape pShape, uint id)
792 { 899 {
793 return null; 900 CollisionShape shape1 = (pShape as BulletShapeXNA).shape;
901
902 // TODO: Turn this from a reference copy to a Value Copy.
903 BulletShapeXNA shape2 = new BulletShapeXNA(shape1, BSPhysicsShapeType.SHAPE_UNKNOWN);
904
905 return shape2;
794 } 906 }
795 907
796 public override bool DeleteCollisionShape(BulletWorld p, BulletShape p_2) 908 public override bool DeleteCollisionShape(BulletWorld pWorld, BulletShape pShape)
797 { 909 {
798 //TODO: 910 //TODO:
799 return false; 911 return false;
@@ -802,17 +914,40 @@ private sealed class BulletConstraintXNA : BulletConstraint
802 914
803 public override BulletBody CreateBodyFromShape(BulletWorld pWorld, BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation) 915 public override BulletBody CreateBodyFromShape(BulletWorld pWorld, BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
804 { 916 {
805 CollisionWorld world = ((BulletWorldXNA)pWorld).world; 917 CollisionWorld world = (pWorld as BulletWorldXNA).world;
806 IndexedMatrix mat = 918 IndexedMatrix mat =
807 IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y, 919 IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y,
808 pRawOrientation.Z, pRawOrientation.W)); 920 pRawOrientation.Z, pRawOrientation.W));
809 mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z); 921 mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
810 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 922 CollisionShape shape = (pShape as BulletShapeXNA).shape;
811 //UpdateSingleAabb(world, shape); 923 //UpdateSingleAabb(world, shape);
812 // TODO: Feed Update array into null 924 // TODO: Feed Update array into null
813 RigidBody body = new RigidBody(0,new SimMotionState(world,pLocalID,mat,null),shape,IndexedVector3.Zero); 925 SimMotionState motionState = new SimMotionState(world, pLocalID, mat, null);
814 926 RigidBody body = new RigidBody(0,motionState,shape,IndexedVector3.Zero);
927 RigidBodyConstructionInfo constructionInfo = new RigidBodyConstructionInfo(0, new SimMotionState(world, pLocalID, mat, null),shape,IndexedVector3.Zero)
928 {
929 m_mass = 0
930 };
931 /*
932 m_mass = mass;
933 m_motionState =motionState;
934 m_collisionShape = collisionShape;
935 m_localInertia = localInertia;
936 m_linearDamping = 0f;
937 m_angularDamping = 0f;
938 m_friction = 0.5f;
939 m_restitution = 0f;
940 m_linearSleepingThreshold = 0.8f;
941 m_angularSleepingThreshold = 1f;
942 m_additionalDamping = false;
943 m_additionalDampingFactor = 0.005f;
944 m_additionalLinearDampingThresholdSqr = 0.01f;
945 m_additionalAngularDampingThresholdSqr = 0.01f;
946 m_additionalAngularDampingFactor = 0.01f;
947 m_startWorldTransform = IndexedMatrix.Identity;
948 */
815 body.SetUserPointer(pLocalID); 949 body.SetUserPointer(pLocalID);
950
816 return new BulletBodyXNA(pLocalID, body); 951 return new BulletBodyXNA(pLocalID, body);
817 } 952 }
818 953
@@ -825,7 +960,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
825 pRawOrientation.Z, pRawOrientation.W)); 960 pRawOrientation.Z, pRawOrientation.W));
826 mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z); 961 mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
827 962
828 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 963 CollisionShape shape = (pShape as BulletShapeXNA).shape;
829 964
830 // TODO: Feed Update array into null 965 // TODO: Feed Update array into null
831 RigidBody body = new RigidBody(0, new DefaultMotionState( mat, IndexedMatrix.Identity), shape, IndexedVector3.Zero); 966 RigidBody body = new RigidBody(0, new DefaultMotionState( mat, IndexedMatrix.Identity), shape, IndexedVector3.Zero);
@@ -834,21 +969,43 @@ private sealed class BulletConstraintXNA : BulletConstraint
834 return new BulletBodyXNA(pLocalID, body); 969 return new BulletBodyXNA(pLocalID, body);
835 } 970 }
836 //(m_mapInfo.terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT); 971 //(m_mapInfo.terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
837 public override CollisionFlags SetCollisionFlags(BulletBody pBody, CollisionFlags collisionFlags) 972 public override CollisionFlags SetCollisionFlags(BulletBody pCollisionObject, CollisionFlags collisionFlags)
838 { 973 {
839 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 974 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
840 body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags) (uint) collisionFlags); 975 collisionObject.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags) (uint) collisionFlags);
841 return (CollisionFlags)body.GetCollisionFlags(); 976 return (CollisionFlags)collisionObject.GetCollisionFlags();
842 } 977 }
843 978
844 public override Vector3 GetAnisotripicFriction(BulletConstraint pconstrain) { /* TODO */ return Vector3.Zero; } 979 public override Vector3 GetAnisotripicFriction(BulletConstraint pconstrain)
980 {
981
982 /* TODO */
983 return Vector3.Zero;
984 }
845 public override Vector3 SetAnisotripicFriction(BulletConstraint pconstrain, Vector3 frict) { /* TODO */ return Vector3.Zero; } 985 public override Vector3 SetAnisotripicFriction(BulletConstraint pconstrain, Vector3 frict) { /* TODO */ return Vector3.Zero; }
846 public override bool HasAnisotripicFriction(BulletConstraint pconstrain) { /* TODO */ return false; } 986 public override bool HasAnisotripicFriction(BulletConstraint pconstrain) { /* TODO */ return false; }
847 public override float GetContactProcessingThreshold(BulletBody pBody) { /* TODO */ return 0f; } 987 public override float GetContactProcessingThreshold(BulletBody pBody) { /* TODO */ return 0f; }
848 public override bool IsStaticObject(BulletBody pBody) { /* TODO */ return false; } 988 public override bool IsStaticObject(BulletBody pCollisionObject)
849 public override bool IsKinematicObject(BulletBody pBody) { /* TODO */ return false; } 989 {
850 public override bool IsStaticOrKinematicObject(BulletBody pBody) { /* TODO */ return false; } 990 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
851 public override bool HasContactResponse(BulletBody pBody) { /* TODO */ return false; } 991 return collisionObject.IsStaticObject();
992
993 }
994 public override bool IsKinematicObject(BulletBody pCollisionObject)
995 {
996 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
997 return collisionObject.IsKinematicObject();
998 }
999 public override bool IsStaticOrKinematicObject(BulletBody pCollisionObject)
1000 {
1001 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
1002 return collisionObject.IsStaticOrKinematicObject();
1003 }
1004 public override bool HasContactResponse(BulletBody pCollisionObject)
1005 {
1006 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
1007 return collisionObject.HasContactResponse();
1008 }
852 public override int GetActivationState(BulletBody pBody) { /* TODO */ return 0; } 1009 public override int GetActivationState(BulletBody pBody) { /* TODO */ return 0; }
853 public override void SetActivationState(BulletBody pBody, int state) { /* TODO */ } 1010 public override void SetActivationState(BulletBody pBody, int state) { /* TODO */ }
854 public override float GetDeactivationTime(BulletBody pBody) { /* TODO */ return 0f; } 1011 public override float GetDeactivationTime(BulletBody pBody) { /* TODO */ return 0f; }
@@ -859,15 +1016,15 @@ private sealed class BulletConstraintXNA : BulletConstraint
859 public override float GetHitFraction(BulletBody pBody) { /* TODO */ return 0f; } 1016 public override float GetHitFraction(BulletBody pBody) { /* TODO */ return 0f; }
860 1017
861 //(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainHitFraction); 1018 //(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainHitFraction);
862 public override void SetHitFraction(BulletBody pBody, float pHitFraction) 1019 public override void SetHitFraction(BulletBody pCollisionObject, float pHitFraction)
863 { 1020 {
864 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 1021 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
865 body.SetHitFraction(pHitFraction); 1022 collisionObject.SetHitFraction(pHitFraction);
866 } 1023 }
867 //BuildCapsuleShape(physicsScene.World.ptr, 1f, 1f, prim.Scale); 1024 //BuildCapsuleShape(physicsScene.World.ptr, 1f, 1f, prim.Scale);
868 public override BulletShape BuildCapsuleShape(BulletWorld pWorld, float pRadius, float pHeight, Vector3 pScale) 1025 public override BulletShape BuildCapsuleShape(BulletWorld pWorld, float pRadius, float pHeight, Vector3 pScale)
869 { 1026 {
870 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 1027 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
871 IndexedVector3 scale = new IndexedVector3(pScale.X, pScale.Y, pScale.Z); 1028 IndexedVector3 scale = new IndexedVector3(pScale.X, pScale.Y, pScale.Z);
872 CapsuleShapeZ capsuleShapeZ = new CapsuleShapeZ(pRadius, pHeight); 1029 CapsuleShapeZ capsuleShapeZ = new CapsuleShapeZ(pRadius, pHeight);
873 capsuleShapeZ.SetMargin(world.WorldSettings.Params.collisionMargin); 1030 capsuleShapeZ.SetMargin(world.WorldSettings.Params.collisionMargin);
@@ -881,14 +1038,24 @@ private sealed class BulletConstraintXNA : BulletConstraint
881 int maxUpdates, ref EntityProperties[] updateArray 1038 int maxUpdates, ref EntityProperties[] updateArray
882 ) 1039 )
883 { 1040 {
1041
1042 m_updateArray = updateArray;
1043 m_collisionArray = collisionArray;
884 /* TODO */ 1044 /* TODO */
885 return new BulletWorldXNA(1, null, null); 1045 ConfigurationParameters[] configparms = new ConfigurationParameters[1];
1046 configparms[0] = parms;
1047 Vector3 worldExtent = new Vector3(Constants.RegionSize, Constants.RegionSize, Constants.RegionHeight);
1048 m_maxCollisions = maxCollisions;
1049 m_maxUpdatesPerFrame = maxUpdates;
1050
1051
1052 return new BulletWorldXNA(1, PhysicsScene, BSAPIXNA.Initialize2(worldExtent, configparms, maxCollisions, ref collisionArray, maxUpdates, ref updateArray, null));
886 } 1053 }
887 1054
888 private static object Initialize2(Vector3 worldExtent, 1055 private static DiscreteDynamicsWorld Initialize2(Vector3 worldExtent,
889 ConfigurationParameters[] o, 1056 ConfigurationParameters[] o,
890 int mMaxCollisionsPerFrame, ref List<BulletXNA.CollisionDesc> collisionArray, 1057 int mMaxCollisionsPerFrame, ref CollisionDesc[] collisionArray,
891 int mMaxUpdatesPerFrame, ref List<BulletXNA.EntityProperties> updateArray, 1058 int mMaxUpdatesPerFrame, ref EntityProperties[] updateArray,
892 object mDebugLogCallbackHandle) 1059 object mDebugLogCallbackHandle)
893 { 1060 {
894 CollisionWorld.WorldData.ParamData p = new CollisionWorld.WorldData.ParamData(); 1061 CollisionWorld.WorldData.ParamData p = new CollisionWorld.WorldData.ParamData();
@@ -968,8 +1135,13 @@ private sealed class BulletConstraintXNA : BulletConstraint
968 SequentialImpulseConstraintSolver m_solver = new SequentialImpulseConstraintSolver(); 1135 SequentialImpulseConstraintSolver m_solver = new SequentialImpulseConstraintSolver();
969 1136
970 DiscreteDynamicsWorld world = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, cci); 1137 DiscreteDynamicsWorld world = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, cci);
971 world.UpdatedObjects = updateArray; 1138
972 world.UpdatedCollisions = collisionArray; 1139
1140 world.UpdatedObjects = BSAPIXNA.GetBulletXNAEntityStruct(BSAPIXNA.BulletSimEntityStructToByteArray(updateArray, updateArray.Length));
1141 world.UpdatedCollisions = BSAPIXNA.GetBulletXNACollisionStruct(BSAPIXNA.BulletSimCollisionStructToByteArray(collisionArray, collisionArray.Length));
1142 world.LastCollisionDesc = 0;
1143 world.LastEntityProperty = 0;
1144
973 world.WorldSettings.Params = p; 1145 world.WorldSettings.Params = p;
974 world.SetForceUpdateAllAabbs(p.shouldForceUpdateAllAabbs != 0); 1146 world.SetForceUpdateAllAabbs(p.shouldForceUpdateAllAabbs != 0);
975 world.GetSolverInfo().m_solverMode = SolverMode.SOLVER_USE_WARMSTARTING | SolverMode.SOLVER_SIMD; 1147 world.GetSolverInfo().m_solverMode = SolverMode.SOLVER_USE_WARMSTARTING | SolverMode.SOLVER_SIMD;
@@ -1003,7 +1175,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
1003 world.GetSolverInfo().m_restingContactRestitutionThreshold = 2; 1175 world.GetSolverInfo().m_restingContactRestitutionThreshold = 2;
1004 world.SetForceUpdateAllAabbs(true); 1176 world.SetForceUpdateAllAabbs(true);
1005 1177
1006 1178 //BSParam.TerrainImplementation = 0;
1007 world.SetGravity(new IndexedVector3(0,0,p.gravity)); 1179 world.SetGravity(new IndexedVector3(0,0,p.gravity));
1008 1180
1009 return world; 1181 return world;
@@ -1011,7 +1183,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
1011 //m_constraint.ptr, ConstraintParams.BT_CONSTRAINT_STOP_CFM, cfm, ConstraintParamAxis.AXIS_ALL 1183 //m_constraint.ptr, ConstraintParams.BT_CONSTRAINT_STOP_CFM, cfm, ConstraintParamAxis.AXIS_ALL
1012 public override bool SetConstraintParam(BulletConstraint pConstraint, ConstraintParams paramIndex, float paramvalue, ConstraintParamAxis axis) 1184 public override bool SetConstraintParam(BulletConstraint pConstraint, ConstraintParams paramIndex, float paramvalue, ConstraintParamAxis axis)
1013 { 1185 {
1014 Generic6DofConstraint constrain = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint; 1186 Generic6DofConstraint constrain = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
1015 if (axis == ConstraintParamAxis.AXIS_LINEAR_ALL || axis == ConstraintParamAxis.AXIS_ALL) 1187 if (axis == ConstraintParamAxis.AXIS_LINEAR_ALL || axis == ConstraintParamAxis.AXIS_ALL)
1016 { 1188 {
1017 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 0); 1189 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 0);
@@ -1034,7 +1206,8 @@ private sealed class BulletConstraintXNA : BulletConstraint
1034 public override bool PushUpdate(BulletBody pCollisionObject) 1206 public override bool PushUpdate(BulletBody pCollisionObject)
1035 { 1207 {
1036 bool ret = false; 1208 bool ret = false;
1037 RigidBody rb = ((BulletBodyXNA)pCollisionObject).rigidBody; 1209 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
1210 RigidBody rb = collisionObject as RigidBody;
1038 if (rb != null) 1211 if (rb != null)
1039 { 1212 {
1040 SimMotionState sms = rb.GetMotionState() as SimMotionState; 1213 SimMotionState sms = rb.GetMotionState() as SimMotionState;
@@ -1052,57 +1225,57 @@ private sealed class BulletConstraintXNA : BulletConstraint
1052 1225
1053 public override float GetAngularMotionDisc(BulletShape pShape) 1226 public override float GetAngularMotionDisc(BulletShape pShape)
1054 { 1227 {
1055 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 1228 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1056 return shape.GetAngularMotionDisc(); 1229 return shape.GetAngularMotionDisc();
1057 } 1230 }
1058 public override float GetContactBreakingThreshold(BulletShape pShape, float defaultFactor) 1231 public override float GetContactBreakingThreshold(BulletShape pShape, float defaultFactor)
1059 { 1232 {
1060 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 1233 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1061 return shape.GetContactBreakingThreshold(defaultFactor); 1234 return shape.GetContactBreakingThreshold(defaultFactor);
1062 } 1235 }
1063 public override bool IsCompound(BulletShape pShape) 1236 public override bool IsCompound(BulletShape pShape)
1064 { 1237 {
1065 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 1238 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1066 return shape.IsCompound(); 1239 return shape.IsCompound();
1067 } 1240 }
1068 public override bool IsSoftBody(BulletShape pShape) 1241 public override bool IsSoftBody(BulletShape pShape)
1069 { 1242 {
1070 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 1243 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1071 return shape.IsSoftBody(); 1244 return shape.IsSoftBody();
1072 } 1245 }
1073 public override bool IsPolyhedral(BulletShape pShape) 1246 public override bool IsPolyhedral(BulletShape pShape)
1074 { 1247 {
1075 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 1248 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1076 return shape.IsPolyhedral(); 1249 return shape.IsPolyhedral();
1077 } 1250 }
1078 public override bool IsConvex2d(BulletShape pShape) 1251 public override bool IsConvex2d(BulletShape pShape)
1079 { 1252 {
1080 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 1253 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1081 return shape.IsConvex2d(); 1254 return shape.IsConvex2d();
1082 } 1255 }
1083 public override bool IsConvex(BulletShape pShape) 1256 public override bool IsConvex(BulletShape pShape)
1084 { 1257 {
1085 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 1258 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1086 return shape.IsConvex(); 1259 return shape.IsConvex();
1087 } 1260 }
1088 public override bool IsNonMoving(BulletShape pShape) 1261 public override bool IsNonMoving(BulletShape pShape)
1089 { 1262 {
1090 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 1263 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1091 return shape.IsNonMoving(); 1264 return shape.IsNonMoving();
1092 } 1265 }
1093 public override bool IsConcave(BulletShape pShape) 1266 public override bool IsConcave(BulletShape pShape)
1094 { 1267 {
1095 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 1268 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1096 return shape.IsConcave(); 1269 return shape.IsConcave();
1097 } 1270 }
1098 public override bool IsInfinite(BulletShape pShape) 1271 public override bool IsInfinite(BulletShape pShape)
1099 { 1272 {
1100 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 1273 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1101 return shape.IsInfinite(); 1274 return shape.IsInfinite();
1102 } 1275 }
1103 public override bool IsNativeShape(BulletShape pShape) 1276 public override bool IsNativeShape(BulletShape pShape)
1104 { 1277 {
1105 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 1278 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1106 bool ret; 1279 bool ret;
1107 switch (shape.GetShapeType()) 1280 switch (shape.GetShapeType())
1108 { 1281 {
@@ -1119,38 +1292,53 @@ private sealed class BulletConstraintXNA : BulletConstraint
1119 return ret; 1292 return ret;
1120 } 1293 }
1121 1294
1122 public override void SetShapeCollisionMargin(BulletShape shape, float margin) { /* TODO */ } 1295 public override void SetShapeCollisionMargin(BulletShape pShape, float pMargin)
1296 {
1297 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1298 shape.SetMargin(pMargin);
1299 }
1123 1300
1124 //sim.ptr, shape.ptr,prim.LocalID, prim.RawPosition, prim.RawOrientation 1301 //sim.ptr, shape.ptr,prim.LocalID, prim.RawPosition, prim.RawOrientation
1125 public override BulletBody CreateGhostFromShape(BulletWorld pWorld, BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation) 1302 public override BulletBody CreateGhostFromShape(BulletWorld pWorld, BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
1126 { 1303 {
1127 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 1304 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1128 IndexedMatrix bodyTransform = new IndexedMatrix(); 1305 IndexedMatrix bodyTransform = new IndexedMatrix();
1129 bodyTransform._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z); 1306 bodyTransform._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
1130 bodyTransform.SetRotation(new IndexedQuaternion(pRawOrientation.X,pRawOrientation.Y,pRawOrientation.Z,pRawOrientation.W)); 1307 bodyTransform.SetRotation(new IndexedQuaternion(pRawOrientation.X,pRawOrientation.Y,pRawOrientation.Z,pRawOrientation.W));
1131 GhostObject gObj = new PairCachingGhostObject(); 1308 GhostObject gObj = new PairCachingGhostObject();
1132 gObj.SetWorldTransform(bodyTransform); 1309 gObj.SetWorldTransform(bodyTransform);
1133 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 1310 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1134 gObj.SetCollisionShape(shape); 1311 gObj.SetCollisionShape(shape);
1135 gObj.SetUserPointer(pLocalID); 1312 gObj.SetUserPointer(pLocalID);
1136 // TODO: Add to Special CollisionObjects! 1313 // TODO: Add to Special CollisionObjects!
1137 return new BulletBodyXNA(pLocalID, gObj); 1314 return new BulletBodyXNA(pLocalID, gObj);
1138 } 1315 }
1139 1316
1140 public override void SetCollisionShape(BulletWorld pWorld, BulletBody pObj, BulletShape pShape) 1317 public override void SetCollisionShape(BulletWorld pWorld, BulletBody pCollisionObject, BulletShape pShape)
1141 { 1318 {
1142 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 1319 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1143 CollisionObject obj = ((BulletBodyXNA)pObj).body; 1320 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
1144 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 1321 if (pShape == null)
1145 obj.SetCollisionShape(shape); 1322 {
1146 1323 collisionObject.SetCollisionShape(new EmptyShape());
1324 }
1325 else
1326 {
1327 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1328 collisionObject.SetCollisionShape(shape);
1329 }
1330 }
1331 public override BulletShape GetCollisionShape(BulletBody pCollisionObject)
1332 {
1333 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
1334 CollisionShape shape = collisionObject.GetCollisionShape();
1335 return new BulletShapeXNA(shape,BSPhysicsShapeType.SHAPE_UNKNOWN);
1147 } 1336 }
1148 public override BulletShape GetCollisionShape(BulletBody obj) { /* TODO */ return null; }
1149 1337
1150 //(PhysicsScene.World.ptr, nativeShapeData) 1338 //(PhysicsScene.World.ptr, nativeShapeData)
1151 public override BulletShape BuildNativeShape(BulletWorld pWorld, ShapeData pShapeData) 1339 public override BulletShape BuildNativeShape(BulletWorld pWorld, ShapeData pShapeData)
1152 { 1340 {
1153 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 1341 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1154 CollisionShape shape = null; 1342 CollisionShape shape = null;
1155 switch (pShapeData.Type) 1343 switch (pShapeData.Type)
1156 { 1344 {
@@ -1185,15 +1373,15 @@ private sealed class BulletConstraintXNA : BulletConstraint
1185 1373
1186 public override int GetNumberOfCompoundChildren(BulletShape pCompoundShape) 1374 public override int GetNumberOfCompoundChildren(BulletShape pCompoundShape)
1187 { 1375 {
1188 CompoundShape compoundshape = ((BulletShapeXNA)pCompoundShape).shape as CompoundShape; 1376 CompoundShape compoundshape = (pCompoundShape as BulletShapeXNA).shape as CompoundShape;
1189 return compoundshape.GetNumChildShapes(); 1377 return compoundshape.GetNumChildShapes();
1190 } 1378 }
1191 //LinksetRoot.PhysShape.ptr, newShape.ptr, displacementPos, displacementRot 1379 //LinksetRoot.PhysShape.ptr, newShape.ptr, displacementPos, displacementRot
1192 public override void AddChildShapeToCompoundShape(BulletShape pCShape, BulletShape paddShape, Vector3 displacementPos, Quaternion displacementRot) 1380 public override void AddChildShapeToCompoundShape(BulletShape pCShape, BulletShape paddShape, Vector3 displacementPos, Quaternion displacementRot)
1193 { 1381 {
1194 IndexedMatrix relativeTransform = new IndexedMatrix(); 1382 IndexedMatrix relativeTransform = new IndexedMatrix();
1195 CompoundShape compoundshape = ((BulletShapeXNA)pCShape).shape as CompoundShape; 1383 CompoundShape compoundshape = (pCShape as BulletShapeXNA).shape as CompoundShape;
1196 CollisionShape addshape = ((BulletShapeXNA)paddShape).shape; 1384 CollisionShape addshape = (paddShape as BulletShapeXNA).shape;
1197 1385
1198 relativeTransform._origin = new IndexedVector3(displacementPos.X, displacementPos.Y, displacementPos.Z); 1386 relativeTransform._origin = new IndexedVector3(displacementPos.X, displacementPos.Y, displacementPos.Z);
1199 relativeTransform.SetRotation(new IndexedQuaternion(displacementRot.X,displacementRot.Y,displacementRot.Z,displacementRot.W)); 1387 relativeTransform.SetRotation(new IndexedQuaternion(displacementRot.X,displacementRot.Y,displacementRot.Z,displacementRot.W));
@@ -1203,7 +1391,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
1203 1391
1204 public override BulletShape RemoveChildShapeFromCompoundShapeIndex(BulletShape pCShape, int pii) 1392 public override BulletShape RemoveChildShapeFromCompoundShapeIndex(BulletShape pCShape, int pii)
1205 { 1393 {
1206 CompoundShape compoundshape = ((BulletShapeXNA)pCShape).shape as CompoundShape; 1394 CompoundShape compoundshape = (pCShape as BulletShapeXNA).shape as CompoundShape;
1207 CollisionShape ret = null; 1395 CollisionShape ret = null;
1208 ret = compoundshape.GetChildShape(pii); 1396 ret = compoundshape.GetChildShape(pii);
1209 compoundshape.RemoveChildShapeByIndex(pii); 1397 compoundshape.RemoveChildShapeByIndex(pii);
@@ -1222,12 +1410,12 @@ private sealed class BulletConstraintXNA : BulletConstraint
1222 return new BulletShapeXNA(m_planeshape, BSPhysicsShapeType.SHAPE_GROUNDPLANE); 1410 return new BulletShapeXNA(m_planeshape, BSPhysicsShapeType.SHAPE_GROUNDPLANE);
1223 } 1411 }
1224 1412
1225 public override BulletConstraint CreateHingeConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody ppBody2, Vector3 ppivotInA, Vector3 ppivotInB, Vector3 paxisInA, Vector3 paxisInB, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies) 1413 public override BulletConstraint CreateHingeConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, Vector3 ppivotInA, Vector3 ppivotInB, Vector3 paxisInA, Vector3 paxisInB, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
1226 { 1414 {
1227 HingeConstraint constrain = null; 1415 HingeConstraint constrain = null;
1228 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 1416 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1229 RigidBody rb1 = ((BulletBodyXNA)pBody1).rigidBody; 1417 RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
1230 RigidBody rb2 = ((BulletBodyXNA)ppBody2).rigidBody; 1418 RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
1231 if (rb1 != null && rb2 != null) 1419 if (rb1 != null && rb2 != null)
1232 { 1420 {
1233 IndexedVector3 pivotInA = new IndexedVector3(ppivotInA.X, ppivotInA.Y, ppivotInA.Z); 1421 IndexedVector3 pivotInA = new IndexedVector3(ppivotInA.X, ppivotInA.Y, ppivotInA.Z);
@@ -1241,7 +1429,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
1241 1429
1242 public override BulletShape CreateHullShape(BulletWorld pWorld, int pHullCount, float[] pConvHulls) 1430 public override BulletShape CreateHullShape(BulletWorld pWorld, int pHullCount, float[] pConvHulls)
1243 { 1431 {
1244 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 1432 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1245 CompoundShape compoundshape = new CompoundShape(false); 1433 CompoundShape compoundshape = new CompoundShape(false);
1246 1434
1247 compoundshape.SetMargin(world.WorldSettings.Params.collisionMargin); 1435 compoundshape.SetMargin(world.WorldSettings.Params.collisionMargin);
@@ -1271,7 +1459,11 @@ private sealed class BulletConstraintXNA : BulletConstraint
1271 return new BulletShapeXNA(compoundshape, BSPhysicsShapeType.SHAPE_HULL); 1459 return new BulletShapeXNA(compoundshape, BSPhysicsShapeType.SHAPE_HULL);
1272 } 1460 }
1273 1461
1274 public override BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShape meshShape) { /* TODO */ return null; } 1462 public override BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShape meshShape)
1463 {
1464 /* TODO */ return null;
1465
1466 }
1275 1467
1276 public override BulletShape CreateMeshShape(BulletWorld pWorld, int pIndicesCount, int[] indices, int pVerticesCount, float[] verticesAsFloats) 1468 public override BulletShape CreateMeshShape(BulletWorld pWorld, int pIndicesCount, int[] indices, int pVerticesCount, float[] verticesAsFloats)
1277 { 1469 {
@@ -1286,7 +1478,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
1286 ObjectArray<int> indicesarr = new ObjectArray<int>(indices); 1478 ObjectArray<int> indicesarr = new ObjectArray<int>(indices);
1287 ObjectArray<float> vertices = new ObjectArray<float>(verticesAsFloats); 1479 ObjectArray<float> vertices = new ObjectArray<float>(verticesAsFloats);
1288 DumpRaw(indicesarr,vertices,pIndicesCount,pVerticesCount); 1480 DumpRaw(indicesarr,vertices,pIndicesCount,pVerticesCount);
1289 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 1481 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1290 IndexedMesh mesh = new IndexedMesh(); 1482 IndexedMesh mesh = new IndexedMesh();
1291 mesh.m_indexType = PHY_ScalarType.PHY_INTEGER; 1483 mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
1292 mesh.m_numTriangles = pIndicesCount/3; 1484 mesh.m_numTriangles = pIndicesCount/3;
@@ -1402,7 +1594,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
1402 1594
1403 public override bool TranslationalLimitMotor(BulletConstraint pConstraint, float ponOff, float targetVelocity, float maxMotorForce) 1595 public override bool TranslationalLimitMotor(BulletConstraint pConstraint, float ponOff, float targetVelocity, float maxMotorForce)
1404 { 1596 {
1405 TypedConstraint tconstrain = ((BulletConstraintXNA)pConstraint).constrain; 1597 TypedConstraint tconstrain = (pConstraint as BulletConstraintXNA).constrain;
1406 bool onOff = ponOff != 0; 1598 bool onOff = ponOff != 0;
1407 bool ret = false; 1599 bool ret = false;
1408 1600
@@ -1428,47 +1620,45 @@ private sealed class BulletConstraintXNA : BulletConstraint
1428 /* TODO */ 1620 /* TODO */
1429 updatedEntityCount = 0; 1621 updatedEntityCount = 0;
1430 collidersCount = 0; 1622 collidersCount = 0;
1431 return 1; 1623
1624
1625 int ret = PhysicsStep2(world,timeStep,maxSubSteps,fixedTimeStep,out updatedEntityCount,out world.physicsScene.m_updateArray, out collidersCount, out world.physicsScene.m_collisionArray);
1626
1627 return ret;
1432 } 1628 }
1433 1629
1434 private int PhysicsStep2(BulletWorld pWorld, float timeStep, int m_maxSubSteps, float m_fixedTimeStep, 1630 private int PhysicsStep2(BulletWorld pWorld, float timeStep, int m_maxSubSteps, float m_fixedTimeStep,
1435 out int updatedEntityCount, out List<BulletXNA.EntityProperties> updatedEntities, 1631 out int updatedEntityCount, out EntityProperties[] updatedEntities,
1436 out int collidersCount, out List<BulletXNA.CollisionDesc>colliders) 1632 out int collidersCount, out CollisionDesc[] colliders)
1437 { 1633 {
1438 int epic = PhysicsStepint(pWorld, timeStep, m_maxSubSteps, m_fixedTimeStep, out updatedEntityCount, out updatedEntities, 1634 int epic = PhysicsStepint(pWorld, timeStep, m_maxSubSteps, m_fixedTimeStep, out updatedEntityCount, out updatedEntities,
1439 out collidersCount, out colliders); 1635 out collidersCount, out colliders, m_maxCollisions, m_maxUpdatesPerFrame);
1440 return epic; 1636 return epic;
1441 } 1637 }
1442 1638
1443 private static int PhysicsStepint(BulletWorld pWorld,float timeStep, int m_maxSubSteps, float m_fixedTimeStep, out int updatedEntityCount, out List<BulletXNA.EntityProperties> updatedEntities, out int collidersCount, out List<BulletXNA.CollisionDesc> colliders) 1639 private static int PhysicsStepint(BulletWorld pWorld,float timeStep, int m_maxSubSteps, float m_fixedTimeStep, out int updatedEntityCount,
1640 out EntityProperties[] updatedEntities, out int collidersCount, out CollisionDesc[] colliders, int maxCollisions, int maxUpdates)
1444 { 1641 {
1445 int numSimSteps = 0; 1642 int numSimSteps = 0;
1446
1447 1643
1448 //if (updatedEntities is null) 1644 updatedEntityCount = 0;
1449 // updatedEntities = new List<BulletXNA.EntityProperties>(); 1645 collidersCount = 0;
1450
1451 //if (colliders is null)
1452 // colliders = new List<BulletXNA.CollisionDesc>();
1453 1646
1454 1647
1455 if (pWorld is BulletWorldXNA) 1648 if (pWorld is BulletWorldXNA)
1456 { 1649 {
1457 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 1650 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1458 1651
1652 world.LastCollisionDesc = 0;
1653 world.LastEntityProperty = 0;
1654 world.UpdatedObjects = new BulletXNA.EntityProperties[maxUpdates];
1655 world.UpdatedCollisions = new BulletXNA.CollisionDesc[maxCollisions];
1459 numSimSteps = world.StepSimulation(timeStep, m_maxSubSteps, m_fixedTimeStep); 1656 numSimSteps = world.StepSimulation(timeStep, m_maxSubSteps, m_fixedTimeStep);
1460 int updates = 0; 1657 int updates = 0;
1461 1658
1462 updatedEntityCount = world.UpdatedObjects.Count; 1659
1463 updatedEntities = new List<BulletXNA.EntityProperties>(world.UpdatedObjects);
1464 updatedEntityCount = updatedEntities.Count;
1465 world.UpdatedObjects.Clear();
1466 1660
1467 1661
1468 collidersCount = world.UpdatedCollisions.Count;
1469 colliders = new List<BulletXNA.CollisionDesc>(world.UpdatedCollisions);
1470
1471 world.UpdatedCollisions.Clear();
1472 m_collisionsThisFrame = 0; 1662 m_collisionsThisFrame = 0;
1473 int numManifolds = world.GetDispatcher().GetNumManifolds(); 1663 int numManifolds = world.GetDispatcher().GetNumManifolds();
1474 for (int j = 0; j < numManifolds; j++) 1664 for (int j = 0; j < numManifolds; j++)
@@ -1493,16 +1683,31 @@ private sealed class BulletConstraintXNA : BulletConstraint
1493 1683
1494 } 1684 }
1495 1685
1686 updatedEntityCount = world.LastEntityProperty;
1687 updatedEntities = GetBulletSimEntityStruct(BulletXNAEntityStructToByteArray(world.UpdatedObjects, world.LastEntityProperty));
1688
1689
1690
1691
1692 collidersCount = world.LastCollisionDesc;
1693 colliders =
1694 GetBulletSimCollisionStruct(BulletXNACollisionStructToByteArray(world.UpdatedCollisions, world.LastCollisionDesc));//new List<BulletXNA.CollisionDesc>(world.UpdatedCollisions);
1496 1695
1497 } 1696 }
1498 else 1697 else
1499 { 1698 {
1500 //if (updatedEntities is null) 1699 //if (updatedEntities is null)
1501 updatedEntities = new List<BulletXNA.EntityProperties>(); 1700 //updatedEntities = new List<BulletXNA.EntityProperties>();
1502 updatedEntityCount = 0; 1701 //updatedEntityCount = 0;
1503 //if (colliders is null) 1702 //if (colliders is null)
1504 colliders = new List<BulletXNA.CollisionDesc>(); 1703 //colliders = new List<BulletXNA.CollisionDesc>();
1505 collidersCount = 0; 1704 //collidersCount = 0;
1705
1706 updatedEntities = new EntityProperties[0];
1707
1708
1709 colliders = new CollisionDesc[0];
1710
1506 } 1711 }
1507 return numSimSteps; 1712 return numSimSteps;
1508 } 1713 }
@@ -1535,22 +1740,23 @@ private sealed class BulletConstraintXNA : BulletConstraint
1535 point = contact, 1740 point = contact,
1536 normal = contactNormal 1741 normal = contactNormal
1537 }; 1742 };
1538 world.UpdatedCollisions.Add(cDesc); 1743 if (world.LastCollisionDesc < world.UpdatedCollisions.Length)
1744 world.UpdatedCollisions[world.LastCollisionDesc++] = (cDesc);
1539 m_collisionsThisFrame++; 1745 m_collisionsThisFrame++;
1540 1746
1541 1747
1542 } 1748 }
1543 private static EntityProperties GetDebugProperties(BulletWorld pWorld, BulletBody pBody) 1749 private static EntityProperties GetDebugProperties(BulletWorld pWorld, BulletBody pCollisionObject)
1544 { 1750 {
1545 EntityProperties ent = new EntityProperties(); 1751 EntityProperties ent = new EntityProperties();
1546 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 1752 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1547 RigidBody body = ((BulletBodyXNA)pBody).rigidBody; 1753 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
1548 IndexedMatrix transform = body.GetWorldTransform(); 1754 IndexedMatrix transform = collisionObject.GetWorldTransform();
1549 IndexedVector3 LinearVelocity = body.GetInterpolationLinearVelocity(); 1755 IndexedVector3 LinearVelocity = collisionObject.GetInterpolationLinearVelocity();
1550 IndexedVector3 AngularVelocity = body.GetInterpolationAngularVelocity(); 1756 IndexedVector3 AngularVelocity = collisionObject.GetInterpolationAngularVelocity();
1551 IndexedQuaternion rotation = transform.GetRotation(); 1757 IndexedQuaternion rotation = transform.GetRotation();
1552 ent.Acceleration = Vector3.Zero; 1758 ent.Acceleration = Vector3.Zero;
1553 ent.ID = (uint)body.GetUserPointer(); 1759 ent.ID = (uint)collisionObject.GetUserPointer();
1554 ent.Position = new Vector3(transform._origin.X,transform._origin.Y,transform._origin.Z); 1760 ent.Position = new Vector3(transform._origin.X,transform._origin.Y,transform._origin.Z);
1555 ent.Rotation = new Quaternion(rotation.X,rotation.Y,rotation.Z,rotation.W); 1761 ent.Rotation = new Quaternion(rotation.X,rotation.Y,rotation.Z,rotation.W);
1556 ent.Velocity = new Vector3(LinearVelocity.X, LinearVelocity.Y, LinearVelocity.Z); 1762 ent.Velocity = new Vector3(LinearVelocity.X, LinearVelocity.Y, LinearVelocity.Z);
@@ -1562,19 +1768,19 @@ private sealed class BulletConstraintXNA : BulletConstraint
1562 1768
1563 public override Vector3 GetLocalScaling(BulletShape pShape) 1769 public override Vector3 GetLocalScaling(BulletShape pShape)
1564 { 1770 {
1565 CollisionShape shape = ((BulletShapeXNA)pShape).shape; 1771 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1566 IndexedVector3 scale = shape.GetLocalScaling(); 1772 IndexedVector3 scale = shape.GetLocalScaling();
1567 return new Vector3(scale.X,scale.Y,scale.Z); 1773 return new Vector3(scale.X,scale.Y,scale.Z);
1568 } 1774 }
1569 1775
1570 public bool RayCastGround(BulletWorld pWorld, Vector3 _RayOrigin, float pRayHeight, BulletBody NotMe) 1776 public bool RayCastGround(BulletWorld pWorld, Vector3 _RayOrigin, float pRayHeight, BulletBody NotMe)
1571 { 1777 {
1572 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world; 1778 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1573 if (world != null) 1779 if (world != null)
1574 { 1780 {
1575 if (NotMe is BulletBodyXNA && NotMe.HasPhysicalBody) 1781 if (NotMe is BulletBodyXNA && NotMe.HasPhysicalBody)
1576 { 1782 {
1577 CollisionObject AvoidBody = ((BulletBodyXNA)NotMe).body; 1783 CollisionObject AvoidBody = (NotMe as BulletBodyXNA).body;
1578 1784
1579 IndexedVector3 rOrigin = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z); 1785 IndexedVector3 rOrigin = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z);
1580 IndexedVector3 rEnd = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z - pRayHeight); 1786 IndexedVector3 rEnd = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z - pRayHeight);
@@ -1594,5 +1800,160 @@ private sealed class BulletConstraintXNA : BulletConstraint
1594 } 1800 }
1595 return false; 1801 return false;
1596 } 1802 }
1803
1804 public static unsafe BulletXNA.CollisionDesc[] GetBulletXNACollisionStruct(byte[] buffer)
1805 {
1806 int count = buffer.Length/sizeof (BulletXNA.CollisionDesc);
1807 BulletXNA.CollisionDesc[] result = new BulletXNA.CollisionDesc[count];
1808 BulletXNA.CollisionDesc* ptr;
1809 fixed (byte* localBytes = new byte[buffer.Length])
1810 {
1811 for (int i = 0; i < buffer.Length; i++)
1812 {
1813 localBytes[i] = buffer[i];
1814 }
1815 for (int i=0;i<count;i++)
1816 {
1817 ptr = (BulletXNA.CollisionDesc*) (localBytes + sizeof (BulletXNA.CollisionDesc)*i);
1818 result[i] = new BulletXNA.CollisionDesc();
1819 result[i] = *ptr;
1820 }
1821 }
1822 return result;
1823 }
1824
1825 public static unsafe CollisionDesc[] GetBulletSimCollisionStruct(byte[] buffer)
1826 {
1827 int count = buffer.Length / sizeof(CollisionDesc);
1828 CollisionDesc[] result = new CollisionDesc[count];
1829 CollisionDesc* ptr;
1830 fixed (byte* localBytes = new byte[buffer.Length])
1831 {
1832 for (int i = 0; i < buffer.Length; i++)
1833 {
1834 localBytes[i] = buffer[i];
1835 }
1836 for (int i = 0; i < count; i++)
1837 {
1838 ptr = (CollisionDesc*)(localBytes + sizeof(CollisionDesc) * i);
1839 result[i] = new CollisionDesc();
1840 result[i] = *ptr;
1841 }
1842 }
1843 return result;
1844 }
1845 public static unsafe byte[] BulletSimCollisionStructToByteArray(CollisionDesc[] CollisionDescArray, int count)
1846 {
1847 int arrayLength = CollisionDescArray.Length > count ? count : CollisionDescArray.Length;
1848 byte[] byteArray = new byte[sizeof(CollisionDesc) * arrayLength];
1849 fixed (CollisionDesc* floatPointer = CollisionDescArray)
1850 {
1851 fixed (byte* bytePointer = byteArray)
1852 {
1853 CollisionDesc* read = floatPointer;
1854 CollisionDesc* write = (CollisionDesc*)bytePointer;
1855 for (int i = 0; i < arrayLength; i++)
1856 {
1857 *write++ = *read++;
1858 }
1859 }
1860 }
1861 return byteArray;
1862 }
1863 public static unsafe byte[] BulletXNACollisionStructToByteArray(BulletXNA.CollisionDesc[] CollisionDescArray, int count)
1864 {
1865 int arrayLength = CollisionDescArray.Length > count ? count : CollisionDescArray.Length;
1866 byte[] byteArray = new byte[sizeof(BulletXNA.CollisionDesc) * arrayLength];
1867 fixed (BulletXNA.CollisionDesc* floatPointer = CollisionDescArray)
1868 {
1869 fixed (byte* bytePointer = byteArray)
1870 {
1871 BulletXNA.CollisionDesc* read = floatPointer;
1872 BulletXNA.CollisionDesc* write = (BulletXNA.CollisionDesc*)bytePointer;
1873 for (int i = 0; i < arrayLength; i++)
1874 {
1875 *write++ = *read++;
1876 }
1877 }
1878 }
1879 return byteArray;
1880 }
1881 public static unsafe BulletXNA.EntityProperties[] GetBulletXNAEntityStruct(byte[] buffer)
1882 {
1883 int count = buffer.Length / sizeof(BulletXNA.EntityProperties);
1884 BulletXNA.EntityProperties[] result = new BulletXNA.EntityProperties[count];
1885 BulletXNA.EntityProperties* ptr;
1886 fixed (byte* localBytes = new byte[buffer.Length])
1887 {
1888 for (int i = 0; i < buffer.Length; i++)
1889 {
1890 localBytes[i] = buffer[i];
1891 }
1892 for (int i = 0; i < count; i++)
1893 {
1894 ptr = (BulletXNA.EntityProperties*)(localBytes + sizeof(BulletXNA.EntityProperties) * i);
1895 result[i] = new BulletXNA.EntityProperties();
1896 result[i] = *ptr;
1897 }
1898 }
1899 return result;
1900 }
1901
1902 public static unsafe EntityProperties[] GetBulletSimEntityStruct(byte[] buffer)
1903 {
1904 int count = buffer.Length / sizeof(EntityProperties);
1905 EntityProperties[] result = new EntityProperties[count];
1906 EntityProperties* ptr;
1907 fixed (byte* localBytes = new byte[buffer.Length])
1908 {
1909 for (int i = 0; i < buffer.Length; i++)
1910 {
1911 localBytes[i] = buffer[i];
1912 }
1913 for (int i = 0; i < count; i++)
1914 {
1915 ptr = (EntityProperties*)(localBytes + sizeof(EntityProperties) * i);
1916 result[i] = new EntityProperties();
1917 result[i] = *ptr;
1918 }
1919 }
1920 return result;
1921 }
1922 public static unsafe byte[] BulletSimEntityStructToByteArray(EntityProperties[] CollisionDescArray, int count)
1923 {
1924 int arrayLength = CollisionDescArray.Length > count ? count : CollisionDescArray.Length;
1925 byte[] byteArray = new byte[sizeof(EntityProperties) * arrayLength];
1926 fixed (EntityProperties* floatPointer = CollisionDescArray)
1927 {
1928 fixed (byte* bytePointer = byteArray)
1929 {
1930 EntityProperties* read = floatPointer;
1931 EntityProperties* write = (EntityProperties*)bytePointer;
1932 for (int i = 0; i < arrayLength; i++)
1933 {
1934 *write++ = *read++;
1935 }
1936 }
1937 }
1938 return byteArray;
1939 }
1940 public static unsafe byte[] BulletXNAEntityStructToByteArray(BulletXNA.EntityProperties[] CollisionDescArray, int count)
1941 {
1942 int arrayLength = CollisionDescArray.Length > count ? count : CollisionDescArray.Length;
1943 byte[] byteArray = new byte[sizeof(BulletXNA.EntityProperties) * arrayLength];
1944 fixed (BulletXNA.EntityProperties* floatPointer = CollisionDescArray)
1945 {
1946 fixed (byte* bytePointer = byteArray)
1947 {
1948 BulletXNA.EntityProperties* read = floatPointer;
1949 BulletXNA.EntityProperties* write = (BulletXNA.EntityProperties*)bytePointer;
1950 for (int i = 0; i < arrayLength; i++)
1951 {
1952 *write++ = *read++;
1953 }
1954 }
1955 }
1956 return byteArray;
1957 }
1597} 1958}
1598} 1959}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
index bc163eb..2828cab 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
@@ -140,6 +140,25 @@ public struct EntityProperties
140 public Vector3 Velocity; 140 public Vector3 Velocity;
141 public Vector3 Acceleration; 141 public Vector3 Acceleration;
142 public Vector3 RotationalVelocity; 142 public Vector3 RotationalVelocity;
143
144 public override string ToString()
145 {
146 StringBuilder buff = new StringBuilder();
147 buff.Append("<i=");
148 buff.Append(ID.ToString());
149 buff.Append(",p=");
150 buff.Append(Position.ToString());
151 buff.Append(",r=");
152 buff.Append(Rotation.ToString());
153 buff.Append(",v=");
154 buff.Append(Velocity.ToString());
155 buff.Append(",a=");
156 buff.Append(Acceleration.ToString());
157 buff.Append(",rv=");
158 buff.Append(RotationalVelocity.ToString());
159 buff.Append(">");
160 return buff.ToString();
161 }
143} 162}
144 163
145// Format of this structure must match the definition in the C++ code 164// Format of this structure must match the definition in the C++ code
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 478aeab..7603254 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -56,6 +56,7 @@ public sealed class BSCharacter : BSPhysObject
56 private int _physicsActorType; 56 private int _physicsActorType;
57 private bool _isPhysical; 57 private bool _isPhysical;
58 private bool _flying; 58 private bool _flying;
59 private bool _wasWalking; // 'true' if the avatar was walking/moving last frame
59 private bool _setAlwaysRun; 60 private bool _setAlwaysRun;
60 private bool _throttleUpdates; 61 private bool _throttleUpdates;
61 private bool _floatOnWater; 62 private bool _floatOnWater;
@@ -83,6 +84,7 @@ public sealed class BSCharacter : BSPhysObject
83 _position = pos; 84 _position = pos;
84 85
85 _flying = isFlying; 86 _flying = isFlying;
87 _wasWalking = true; // causes first step to initialize standing
86 _orientation = OMV.Quaternion.Identity; 88 _orientation = OMV.Quaternion.Identity;
87 _velocity = OMV.Vector3.Zero; 89 _velocity = OMV.Vector3.Zero;
88 _buoyancy = ComputeBuoyancyFromFlying(isFlying); 90 _buoyancy = ComputeBuoyancyFromFlying(isFlying);
@@ -198,25 +200,68 @@ public sealed class BSCharacter : BSPhysObject
198 // TODO: Decide if the step parameters should be changed depending on the avatar's 200 // TODO: Decide if the step parameters should be changed depending on the avatar's
199 // state (flying, colliding, ...). There is code in ODE to do this. 201 // state (flying, colliding, ...). There is code in ODE to do this.
200 202
201 OMV.Vector3 stepVelocity = _velocityMotor.Step(timeStep); 203 // COMMENTARY: when the user is making the avatar walk, except for falling, the velocity
202 204 // specified for the avatar is the one that should be used. For falling, if the avatar
203 // If falling, we keep the world's downward vector no matter what the other axis specify. 205 // is not flying and is not colliding then it is presumed to be falling and the Z
204 if (!Flying && !IsColliding) 206 // component is not fooled with (thus allowing gravity to do its thing).
207 // When the avatar is standing, though, the user has specified a velocity of zero and
208 // the avatar should be standing. But if the avatar is pushed by something in the world
209 // (raising elevator platform, moving vehicle, ...) the avatar should be allowed to
210 // move. Thus, the velocity cannot be forced to zero. The problem is that small velocity
211 // errors can creap in and the avatar will slowly float off in some direction.
212 // So, the problem is that, when an avatar is standing, we cannot tell creaping error
213 // from real pushing.OMV.Vector3.Zero;
214 // The code below keeps setting the velocity to zero hoping the world will keep pushing.
215
216 _velocityMotor.Step(timeStep);
217
218 // If we're not supposed to be moving, make sure things are zero.
219 if (_velocityMotor.ErrorIsZero() && _velocityMotor.TargetValue == OMV.Vector3.Zero && IsColliding)
205 { 220 {
206 stepVelocity.Z = _velocity.Z; 221 // The avatar shouldn't be moving
207 // DetailLog("{0},BSCharacter.MoveMotor,taint,overrideStepZWithWorldZ,stepVel={1}", LocalID, stepVelocity); 222 _velocityMotor.Zero();
223 ZeroMotion(true /* inTaintTime */);
224
225 // Standing has more friction on the ground
226 if (_currentFriction != BSParam.AvatarStandingFriction)
227 {
228 _currentFriction = BSParam.AvatarStandingFriction;
229 PhysicsScene.PE.SetFriction(PhysBody, _currentFriction);
230 }
231 DetailLog("{0},BSCharacter.MoveMotor,taint,stopping,target={1}", LocalID, _velocityMotor.TargetValue);
232
233 _wasWalking = false;
208 } 234 }
235 else
236 {
237 OMV.Vector3 stepVelocity = _velocityMotor.CurrentValue;
238
239 if (_currentFriction != BSParam.AvatarFriction)
240 {
241 // Probably starting up walking. Set friction to moving friction.
242 _currentFriction = BSParam.AvatarFriction;
243 PhysicsScene.PE.SetFriction(PhysBody, _currentFriction);
244 }
245
246 // If falling, we keep the world's downward vector no matter what the other axis specify.
247 if (!Flying && !IsColliding)
248 {
249 stepVelocity.Z = _velocity.Z;
250 // DetailLog("{0},BSCharacter.MoveMotor,taint,overrideStepZWithWorldZ,stepVel={1}", LocalID, stepVelocity);
251 }
209 252
210 // 'stepVelocity' is now the speed we'd like the avatar to move in. Turn that into an instantanous force. 253 // 'stepVelocity' is now the speed we'd like the avatar to move in. Turn that into an instantanous force.
211 OMV.Vector3 moveForce = (stepVelocity - _velocity) * Mass; 254 OMV.Vector3 moveForce = (stepVelocity - _velocity) * Mass;
212 255
213 // Should we check for move force being small and forcing velocity to zero? 256 // Should we check for move force being small and forcing velocity to zero?
214 257
215 // Add special movement force to allow avatars to walk up stepped surfaces. 258 // Add special movement force to allow avatars to walk up stepped surfaces.
216 moveForce += WalkUpStairs(); 259 moveForce += WalkUpStairs();
217 260
218 // DetailLog("{0},BSCharacter.MoveMotor,move,stepVel={1},vel={2},mass={3},moveForce={4}", LocalID, stepVelocity, _velocity, Mass, moveForce); 261 DetailLog("{0},BSCharacter.MoveMotor,move,stepVel={1},vel={2},mass={3},moveForce={4}", LocalID, stepVelocity, _velocity, Mass, moveForce);
219 PhysicsScene.PE.ApplyCentralImpulse(PhysBody, moveForce); 262 PhysicsScene.PE.ApplyCentralImpulse(PhysBody, moveForce);
263 _wasWalking = true;
264 }
220 }); 265 });
221 } 266 }
222 267
@@ -559,27 +604,6 @@ public sealed class BSCharacter : BSPhysObject
559 PhysicsScene.AssertInTaintTime("BSCharacter.ForceVelocity"); 604 PhysicsScene.AssertInTaintTime("BSCharacter.ForceVelocity");
560 605
561 _velocity = value; 606 _velocity = value;
562 // Depending on whether the avatar is moving or not, change the friction
563 // to keep the avatar from slipping around
564 if (_velocity.Length() == 0)
565 {
566 if (_currentFriction != BSParam.AvatarStandingFriction)
567 {
568 _currentFriction = BSParam.AvatarStandingFriction;
569 if (PhysBody.HasPhysicalBody)
570 PhysicsScene.PE.SetFriction(PhysBody, _currentFriction);
571 }
572 }
573 else
574 {
575 if (_currentFriction != BSParam.AvatarFriction)
576 {
577 _currentFriction = BSParam.AvatarFriction;
578 if (PhysBody.HasPhysicalBody)
579 PhysicsScene.PE.SetFriction(PhysBody, _currentFriction);
580 }
581 }
582
583 PhysicsScene.PE.SetLinearVelocity(PhysBody, _velocity); 607 PhysicsScene.PE.SetLinearVelocity(PhysBody, _velocity);
584 PhysicsScene.PE.Activate(PhysBody, true); 608 PhysicsScene.PE.Activate(PhysBody, true);
585 } 609 }
@@ -854,7 +878,7 @@ public sealed class BSCharacter : BSPhysObject
854 _position = entprop.Position; 878 _position = entprop.Position;
855 _orientation = entprop.Rotation; 879 _orientation = entprop.Rotation;
856 880
857 // Smooth velocity. OpenSimulator is very sensitive to changes in velocity of the avatar 881 // Smooth velocity. OpenSimulator is VERY sensitive to changes in velocity of the avatar
858 // and will send agent updates to the clients if velocity changes by more than 882 // and will send agent updates to the clients if velocity changes by more than
859 // 0.001m/s. Bullet introduces a lot of jitter in the velocity which causes many 883 // 0.001m/s. Bullet introduces a lot of jitter in the velocity which causes many
860 // extra updates. 884 // extra updates.
@@ -875,7 +899,7 @@ public sealed class BSCharacter : BSPhysObject
875 CurrentEntityProperties = entprop; 899 CurrentEntityProperties = entprop;
876 900
877 // Tell the linkset about value changes 901 // Tell the linkset about value changes
878 Linkset.UpdateProperties(this, true); 902 Linkset.UpdateProperties(UpdatedProperties.EntPropUpdates, this);
879 903
880 // Avatars don't report their changes the usual way. Changes are checked for in the heartbeat loop. 904 // Avatars don't report their changes the usual way. Changes are checked for in the heartbeat loop.
881 // base.RequestPhysicsterseUpdate(); 905 // base.RequestPhysicsterseUpdate();
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index f2c7cec..fe7891e 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -231,6 +231,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
231 break; 231 break;
232 case Vehicle.ANGULAR_MOTOR_DIRECTION: 232 case Vehicle.ANGULAR_MOTOR_DIRECTION:
233 m_angularMotorDirection = new Vector3(pValue, pValue, pValue); 233 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
234 m_angularMotor.Zero();
234 m_angularMotor.SetTarget(m_angularMotorDirection); 235 m_angularMotor.SetTarget(m_angularMotorDirection);
235 break; 236 break;
236 case Vehicle.LINEAR_FRICTION_TIMESCALE: 237 case Vehicle.LINEAR_FRICTION_TIMESCALE:
@@ -264,6 +265,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
264 pValue.Y = ClampInRange(-12.56f, pValue.Y, 12.56f); 265 pValue.Y = ClampInRange(-12.56f, pValue.Y, 12.56f);
265 pValue.Z = ClampInRange(-12.56f, pValue.Z, 12.56f); 266 pValue.Z = ClampInRange(-12.56f, pValue.Z, 12.56f);
266 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); 267 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
268 m_angularMotor.Zero();
267 m_angularMotor.SetTarget(m_angularMotorDirection); 269 m_angularMotor.SetTarget(m_angularMotorDirection);
268 break; 270 break;
269 case Vehicle.LINEAR_FRICTION_TIMESCALE: 271 case Vehicle.LINEAR_FRICTION_TIMESCALE:
@@ -688,7 +690,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
688 // Bullet does a bunch of smoothing for changing parameters. 690 // Bullet does a bunch of smoothing for changing parameters.
689 // Since the vehicle is demanding this setting, we override Bullet's smoothing 691 // Since the vehicle is demanding this setting, we override Bullet's smoothing
690 // by telling Bullet the value was the same last time. 692 // by telling Bullet the value was the same last time.
691 PhysicsScene.PE.SetInterpolationLinearVelocity(Prim.PhysBody, m_knownVelocity); 693 // PhysicsScene.PE.SetInterpolationLinearVelocity(Prim.PhysBody, m_knownVelocity);
692 } 694 }
693 695
694 if ((m_knownChanged & m_knownChangedForce) != 0) 696 if ((m_knownChanged & m_knownChangedForce) != 0)
@@ -700,7 +702,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
700 if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0) 702 if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0)
701 { 703 {
702 Prim.ForceRotationalVelocity = m_knownRotationalVelocity; 704 Prim.ForceRotationalVelocity = m_knownRotationalVelocity;
703 PhysicsScene.PE.SetInterpolationAngularVelocity(Prim.PhysBody, m_knownRotationalVelocity); 705 // PhysicsScene.PE.SetInterpolationAngularVelocity(Prim.PhysBody, m_knownRotationalVelocity);
704 } 706 }
705 707
706 if ((m_knownChanged & m_knownChangedRotationalImpulse) != 0) 708 if ((m_knownChanged & m_knownChangedRotationalImpulse) != 0)
@@ -720,10 +722,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
720 722
721 // Since the computation of terrain height can be a little involved, this routine 723 // Since the computation of terrain height can be a little involved, this routine
722 // is used to fetch the height only once for each vehicle simulation step. 724 // is used to fetch the height only once for each vehicle simulation step.
725 Vector3 lastRememberedHeightPos;
723 private float GetTerrainHeight(Vector3 pos) 726 private float GetTerrainHeight(Vector3 pos)
724 { 727 {
725 if ((m_knownHas & m_knownChangedTerrainHeight) == 0) 728 if ((m_knownHas & m_knownChangedTerrainHeight) == 0 || pos != lastRememberedHeightPos)
726 { 729 {
730 lastRememberedHeightPos = pos;
727 m_knownTerrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos); 731 m_knownTerrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
728 m_knownHas |= m_knownChangedTerrainHeight; 732 m_knownHas |= m_knownChangedTerrainHeight;
729 } 733 }
@@ -943,10 +947,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
943 // ================================================================== 947 // ==================================================================
944 // Clamp high or low velocities 948 // Clamp high or low velocities
945 float newVelocityLengthSq = VehicleVelocity.LengthSquared(); 949 float newVelocityLengthSq = VehicleVelocity.LengthSquared();
946 if (newVelocityLengthSq > 1000f) 950 if (newVelocityLengthSq > BSParam.VehicleMaxLinearVelocity)
947 { 951 {
948 VehicleVelocity /= VehicleVelocity.Length(); 952 VehicleVelocity /= VehicleVelocity.Length();
949 VehicleVelocity *= 1000f; 953 VehicleVelocity *= BSParam.VehicleMaxLinearVelocity;
950 } 954 }
951 else if (newVelocityLengthSq < 0.001f) 955 else if (newVelocityLengthSq < 0.001f)
952 VehicleVelocity = Vector3.Zero; 956 VehicleVelocity = Vector3.Zero;
@@ -957,39 +961,25 @@ namespace OpenSim.Region.Physics.BulletSPlugin
957 961
958 public void ComputeLinearVelocity(float pTimestep) 962 public void ComputeLinearVelocity(float pTimestep)
959 { 963 {
960 Vector3 linearMotorStep = m_linearMotor.Step(pTimestep); 964 // Step the motor from the current value. Get the correction needed this step.
965 Vector3 currentVel = VehicleVelocity * Quaternion.Inverse(VehicleOrientation);
966 Vector3 linearMotorCorrectionV = m_linearMotor.Step(pTimestep, currentVel);
961 967
962 // The movement computed in the linear motor is relative to the vehicle 968 // Motor is vehicle coordinates. Rotate it to world coordinates
963 // coordinates. Rotate the movement to world coordinates. 969 Vector3 linearMotorVelocityW = linearMotorCorrectionV * VehicleOrientation;
964 Vector3 linearMotorVelocity = linearMotorStep * VehicleOrientation;
965 970
966 // If we're a ground vehicle, don't loose any Z action (like gravity acceleration). 971 // If we're a ground vehicle, don't add any upward Z movement
967 float mixFactor = 1f; // 1 means use all linear motor Z value, 0 means use all existing Z
968 if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0) 972 if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
969 { 973 {
970 if (!Prim.IsColliding) 974 if (linearMotorVelocityW.Z > 0f)
971 { 975 linearMotorVelocityW.Z = 0f;
972 // If a ground vehicle and not on the ground, I want gravity effect
973 mixFactor = 0.2f;
974 }
975 }
976 else
977 {
978 // I'm not a ground vehicle but don't totally loose the effect of the environment
979 mixFactor = 0.8f;
980 } 976 }
981 linearMotorVelocity.Z = mixFactor * linearMotorVelocity.Z + (1f - mixFactor) * VehicleVelocity.Z;
982
983 // What we want to contribute to the vehicle's existing velocity
984 Vector3 linearMotorForce = linearMotorVelocity - VehicleVelocity;
985
986 // Act against the inertia of the vehicle
987 linearMotorForce *= m_vehicleMass;
988 977
989 VehicleAddForceImpulse(linearMotorForce * pTimestep); 978 // Add this correction to the velocity to make it faster/slower.
979 VehicleVelocity += linearMotorVelocityW;
990 980
991 VDetailLog("{0}, MoveLinear,velocity,vehVel={1},step={2},stepVel={3},mix={4},force={5}", 981 VDetailLog("{0}, MoveLinear,velocity,vehVel={1},correction={2},force={3}",
992 Prim.LocalID, VehicleVelocity, linearMotorStep, linearMotorVelocity, mixFactor, linearMotorForce); 982 Prim.LocalID, VehicleVelocity, linearMotorCorrectionV, linearMotorVelocityW);
993 } 983 }
994 984
995 public void ComputeLinearTerrainHeightCorrection(float pTimestep) 985 public void ComputeLinearTerrainHeightCorrection(float pTimestep)
@@ -1133,8 +1123,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
1133 // a downward raycast to find what is below. 1123 // a downward raycast to find what is below.
1134 public void ComputeLinearMotorUp(float pTimestep) 1124 public void ComputeLinearMotorUp(float pTimestep)
1135 { 1125 {
1136 Vector3 ret = Vector3.Zero;
1137
1138 if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) 1126 if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
1139 { 1127 {
1140 // This code tries to decide if the object is not on the ground and then pushing down 1128 // This code tries to decide if the object is not on the ground and then pushing down
@@ -1202,62 +1190,27 @@ namespace OpenSim.Region.Physics.BulletSPlugin
1202 // set directly on the vehicle. 1190 // set directly on the vehicle.
1203 private void MoveAngular(float pTimestep) 1191 private void MoveAngular(float pTimestep)
1204 { 1192 {
1205 // The user wants this many radians per second angular change? 1193 // VehicleRotationalVelocity = Vector3.Zero;
1206 Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep);
1207 1194
1208 // ================================================================== 1195 ComputeAngularTurning(pTimestep);
1209 // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
1210 // This flag prevents linear deflection parallel to world z-axis. This is useful
1211 // for preventing ground vehicles with large linear deflection, like bumper cars,
1212 // from climbing their linear deflection into the sky.
1213 // That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
1214 // TODO: This is here because this is where ODE put it but documentation says it
1215 // is a linear effect. Where should this check go?
1216 if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
1217 {
1218 angularMotorContribution.X = 0f;
1219 angularMotorContribution.Y = 0f;
1220 VDetailLog("{0}, MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution);
1221 }
1222
1223 Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction();
1224 1196
1225 Vector3 deflectionContribution = ComputeAngularDeflection(); 1197 ComputeAngularVerticalAttraction();
1226 1198
1227 Vector3 bankingContribution = ComputeAngularBanking(); 1199 ComputeAngularDeflection();
1228 1200
1229 // ================================================================== 1201 ComputeAngularBanking();
1230 m_lastVertAttractor = verticalAttractionContribution;
1231
1232 m_lastAngularVelocity = angularMotorContribution
1233 + verticalAttractionContribution
1234 + deflectionContribution
1235 + bankingContribution;
1236
1237 // Add of the above computation are made relative to vehicle coordinates.
1238 // Convert to world coordinates.
1239 m_lastAngularVelocity *= VehicleOrientation;
1240 1202
1241 // ================================================================== 1203 // ==================================================================
1242 // Apply the correction velocity. 1204 if (VehicleRotationalVelocity.ApproxEquals(Vector3.Zero, 0.01f))
1243 // TODO: Should this be applied as an angular force (torque)?
1244 if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
1245 {
1246 VehicleRotationalVelocity = m_lastAngularVelocity;
1247
1248 VDetailLog("{0}, MoveAngular,done,nonZero,angMotorContrib={1},vertAttrContrib={2},bankContrib={3},deflectContrib={4},totalContrib={5}",
1249 Prim.LocalID,
1250 angularMotorContribution, verticalAttractionContribution,
1251 bankingContribution, deflectionContribution,
1252 m_lastAngularVelocity
1253 );
1254 }
1255 else
1256 { 1205 {
1257 // The vehicle is not adding anything angular wise. 1206 // The vehicle is not adding anything angular wise.
1258 VehicleRotationalVelocity = Vector3.Zero; 1207 VehicleRotationalVelocity = Vector3.Zero;
1259 VDetailLog("{0}, MoveAngular,done,zero", Prim.LocalID); 1208 VDetailLog("{0}, MoveAngular,done,zero", Prim.LocalID);
1260 } 1209 }
1210 else
1211 {
1212 VDetailLog("{0}, MoveAngular,done,nonZero,angVel={1}", Prim.LocalID, VehicleRotationalVelocity);
1213 }
1261 1214
1262 // ================================================================== 1215 // ==================================================================
1263 //Offset section 1216 //Offset section
@@ -1291,6 +1244,31 @@ namespace OpenSim.Region.Physics.BulletSPlugin
1291 } 1244 }
1292 1245
1293 } 1246 }
1247
1248 private void ComputeAngularTurning(float pTimestep)
1249 {
1250 // The user wants this many radians per second angular change?
1251 Vector3 currentAngularV = VehicleRotationalVelocity * Quaternion.Inverse(VehicleOrientation);
1252 Vector3 angularMotorContributionV = m_angularMotor.Step(pTimestep, currentAngularV);
1253
1254 // ==================================================================
1255 // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
1256 // This flag prevents linear deflection parallel to world z-axis. This is useful
1257 // for preventing ground vehicles with large linear deflection, like bumper cars,
1258 // from climbing their linear deflection into the sky.
1259 // That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
1260 // TODO: This is here because this is where ODE put it but documentation says it
1261 // is a linear effect. Where should this check go?
1262 if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
1263 {
1264 angularMotorContributionV.X = 0f;
1265 angularMotorContributionV.Y = 0f;
1266 }
1267
1268 VehicleRotationalVelocity += angularMotorContributionV * VehicleOrientation;
1269 VDetailLog("{0}, MoveAngular,angularTurning,angularMotorContrib={1}", Prim.LocalID, angularMotorContributionV);
1270 }
1271
1294 // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial: 1272 // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
1295 // Some vehicles, like boats, should always keep their up-side up. This can be done by 1273 // Some vehicles, like boats, should always keep their up-side up. This can be done by
1296 // enabling the "vertical attractor" behavior that springs the vehicle's local z-axis to 1274 // enabling the "vertical attractor" behavior that springs the vehicle's local z-axis to
@@ -1299,13 +1277,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
1299 // and then set the VEHICLE_VERTICAL_ATTRACTION_EFFICIENCY to control the damping. An 1277 // and then set the VEHICLE_VERTICAL_ATTRACTION_EFFICIENCY to control the damping. An
1300 // efficiency of 0.0 will cause the spring to wobble around its equilibrium, while an 1278 // efficiency of 0.0 will cause the spring to wobble around its equilibrium, while an
1301 // efficiency of 1.0 will cause the spring to reach its equilibrium with exponential decay. 1279 // efficiency of 1.0 will cause the spring to reach its equilibrium with exponential decay.
1302 public Vector3 ComputeAngularVerticalAttraction() 1280 public void ComputeAngularVerticalAttraction()
1303 { 1281 {
1304 Vector3 ret = Vector3.Zero;
1305
1306 // If vertical attaction timescale is reasonable 1282 // If vertical attaction timescale is reasonable
1307 if (enableAngularVerticalAttraction && m_verticalAttractionTimescale < m_verticalAttractionCutoff) 1283 if (enableAngularVerticalAttraction && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
1308 { 1284 {
1285 Vector3 vertContributionV = Vector3.Zero;
1286
1309 // Take a vector pointing up and convert it from world to vehicle relative coords. 1287 // Take a vector pointing up and convert it from world to vehicle relative coords.
1310 Vector3 verticalError = Vector3.UnitZ * VehicleOrientation; 1288 Vector3 verticalError = Vector3.UnitZ * VehicleOrientation;
1311 1289
@@ -1319,37 +1297,36 @@ namespace OpenSim.Region.Physics.BulletSPlugin
1319 1297
1320 // Y error means needed rotation around X axis and visa versa. 1298 // Y error means needed rotation around X axis and visa versa.
1321 // Since the error goes from zero to one, the asin is the corresponding angle. 1299 // Since the error goes from zero to one, the asin is the corresponding angle.
1322 ret.X = (float)Math.Asin(verticalError.Y); 1300 vertContributionV.X = (float)Math.Asin(verticalError.Y);
1323 // (Tilt forward (positive X) needs to tilt back (rotate negative) around Y axis.) 1301 // (Tilt forward (positive X) needs to tilt back (rotate negative) around Y axis.)
1324 ret.Y = -(float)Math.Asin(verticalError.X); 1302 vertContributionV.Y = -(float)Math.Asin(verticalError.X);
1325 1303
1326 // If verticalError.Z is negative, the vehicle is upside down. Add additional push. 1304 // If verticalError.Z is negative, the vehicle is upside down. Add additional push.
1327 if (verticalError.Z < 0f) 1305 if (verticalError.Z < 0f)
1328 { 1306 {
1329 ret.X += PIOverFour; 1307 vertContributionV.X += PIOverFour;
1330 ret.Y += PIOverFour; 1308 // vertContribution.Y -= PIOverFour;
1331 } 1309 }
1332 1310
1333 // 'ret' is now the necessary velocity to correct tilt in one second. 1311 // 'vertContrbution' is now the necessary angular correction to correct tilt in one second.
1334 // Correction happens over a number of seconds. 1312 // Correction happens over a number of seconds.
1335 Vector3 unscaledContrib = ret; 1313 Vector3 unscaledContrib = vertContributionV; // DEBUG DEBUG
1336 ret /= m_verticalAttractionTimescale; 1314 vertContributionV /= m_verticalAttractionTimescale;
1315
1316 VehicleRotationalVelocity += vertContributionV * VehicleOrientation;
1337 1317
1338 VDetailLog("{0}, MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},eff={3},ts={4},vertAttr={5}", 1318 VDetailLog("{0}, MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},eff={3},ts={4},vertAttr={5}",
1339 Prim.LocalID, verticalError, unscaledContrib, m_verticalAttractionEfficiency, m_verticalAttractionTimescale, ret); 1319 Prim.LocalID, verticalError, unscaledContrib, m_verticalAttractionEfficiency, m_verticalAttractionTimescale, vertContributionV);
1340 } 1320 }
1341 return ret;
1342 } 1321 }
1343 1322
1344 // Return the angular correction to correct the direction the vehicle is pointing to be 1323 // Angular correction to correct the direction the vehicle is pointing to be
1345 // the direction is should want to be pointing. 1324 // the direction is should want to be pointing.
1346 // The vehicle is moving in some direction and correct its orientation to it is pointing 1325 // The vehicle is moving in some direction and correct its orientation to it is pointing
1347 // in that direction. 1326 // in that direction.
1348 // TODO: implement reference frame. 1327 // TODO: implement reference frame.
1349 public Vector3 ComputeAngularDeflection() 1328 public void ComputeAngularDeflection()
1350 { 1329 {
1351 Vector3 ret = Vector3.Zero;
1352
1353 // Since angularMotorUp and angularDeflection are computed independently, they will calculate 1330 // Since angularMotorUp and angularDeflection are computed independently, they will calculate
1354 // approximately the same X or Y correction. When added together (when contributions are combined) 1331 // approximately the same X or Y correction. When added together (when contributions are combined)
1355 // this creates an over-correction and then wabbling as the target is overshot. 1332 // this creates an over-correction and then wabbling as the target is overshot.
@@ -1357,6 +1334,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
1357 1334
1358 if (enableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleForwardSpeed > 0.2) 1335 if (enableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleForwardSpeed > 0.2)
1359 { 1336 {
1337 Vector3 deflectContributionV = Vector3.Zero;
1338
1360 // The direction the vehicle is moving 1339 // The direction the vehicle is moving
1361 Vector3 movingDirection = VehicleVelocity; 1340 Vector3 movingDirection = VehicleVelocity;
1362 movingDirection.Normalize(); 1341 movingDirection.Normalize();
@@ -1382,18 +1361,19 @@ namespace OpenSim.Region.Physics.BulletSPlugin
1382 // ret = m_angularDeflectionCorrectionMotor(1f, deflectionError); 1361 // ret = m_angularDeflectionCorrectionMotor(1f, deflectionError);
1383 1362
1384 // Scale the correction by recovery timescale and efficiency 1363 // Scale the correction by recovery timescale and efficiency
1385 ret = (-deflectionError) * m_angularDeflectionEfficiency; 1364 deflectContributionV = (-deflectionError) * m_angularDeflectionEfficiency;
1386 ret /= m_angularDeflectionTimescale; 1365 deflectContributionV /= m_angularDeflectionTimescale;
1366
1367 VehicleRotationalVelocity += deflectContributionV * VehicleOrientation;
1387 1368
1388 VDetailLog("{0}, MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}", 1369 VDetailLog("{0}, MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}",
1389 Prim.LocalID, movingDirection, pointingDirection, deflectionError, ret); 1370 Prim.LocalID, movingDirection, pointingDirection, deflectionError, deflectContributionV);
1390 VDetailLog("{0}, MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3}", 1371 VDetailLog("{0}, MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3}",
1391 Prim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale); 1372 Prim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale);
1392 } 1373 }
1393 return ret;
1394 } 1374 }
1395 1375
1396 // Return an angular change to rotate the vehicle around the Z axis when the vehicle 1376 // Angular change to rotate the vehicle around the Z axis when the vehicle
1397 // is tipped around the X axis. 1377 // is tipped around the X axis.
1398 // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial: 1378 // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
1399 // The vertical attractor feature must be enabled in order for the banking behavior to 1379 // The vertical attractor feature must be enabled in order for the banking behavior to
@@ -1424,12 +1404,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
1424 // world z-axis is determined by the VEHICLE_BANKING_TIMESCALE. So if you want the vehicle to 1404 // world z-axis is determined by the VEHICLE_BANKING_TIMESCALE. So if you want the vehicle to
1425 // bank quickly then give it a banking timescale of about a second or less, otherwise you can 1405 // bank quickly then give it a banking timescale of about a second or less, otherwise you can
1426 // make a sluggish vehicle by giving it a timescale of several seconds. 1406 // make a sluggish vehicle by giving it a timescale of several seconds.
1427 public Vector3 ComputeAngularBanking() 1407 public void ComputeAngularBanking()
1428 { 1408 {
1429 Vector3 ret = Vector3.Zero;
1430
1431 if (enableAngularBanking && m_bankingEfficiency != 0 && m_verticalAttractionTimescale < m_verticalAttractionCutoff) 1409 if (enableAngularBanking && m_bankingEfficiency != 0 && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
1432 { 1410 {
1411 Vector3 bankingContributionV = Vector3.Zero;
1412
1433 // Rotate a UnitZ vector (pointing up) to how the vehicle is oriented. 1413 // Rotate a UnitZ vector (pointing up) to how the vehicle is oriented.
1434 // As the vehicle rolls to the right or left, the Y value will increase from 1414 // As the vehicle rolls to the right or left, the Y value will increase from
1435 // zero (straight up) to 1 or -1 (full tilt right or left) 1415 // zero (straight up) to 1 or -1 (full tilt right or left)
@@ -1446,15 +1426,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin
1446 mixedYawAngle = ClampInRange(-20f, mixedYawAngle, 20f); 1426 mixedYawAngle = ClampInRange(-20f, mixedYawAngle, 20f);
1447 1427
1448 // Build the force vector to change rotation from what it is to what it should be 1428 // Build the force vector to change rotation from what it is to what it should be
1449 ret.Z = -mixedYawAngle; 1429 bankingContributionV.Z = -mixedYawAngle;
1450 1430
1451 // Don't do it all at once. 1431 // Don't do it all at once.
1452 ret /= m_bankingTimescale; 1432 bankingContributionV /= m_bankingTimescale;
1433
1434 VehicleRotationalVelocity += bankingContributionV * VehicleOrientation;
1453 1435
1454 VDetailLog("{0}, MoveAngular,Banking,rollComp={1},speed={2},rollComp={3},yAng={4},mYAng={5},ret={6}", 1436 VDetailLog("{0}, MoveAngular,Banking,rollComp={1},speed={2},rollComp={3},yAng={4},mYAng={5},ret={6}",
1455 Prim.LocalID, rollComponents, VehicleForwardSpeed, rollComponents, yawAngle, mixedYawAngle, ret); 1437 Prim.LocalID, rollComponents, VehicleForwardSpeed, rollComponents, yawAngle, mixedYawAngle, bankingContributionV);
1456 } 1438 }
1457 return ret;
1458 } 1439 }
1459 1440
1460 // This is from previous instantiations of XXXDynamics.cs. 1441 // This is from previous instantiations of XXXDynamics.cs.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index cbd160f..1e3e5d8 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -252,7 +252,7 @@ public abstract class BSLinkset
252 // of the linkset is received. 252 // of the linkset is received.
253 // Passed flag is update came from physics engine (true) or the user (false). 253 // Passed flag is update came from physics engine (true) or the user (false).
254 // Called at taint-time!! 254 // Called at taint-time!!
255 public abstract void UpdateProperties(BSPhysObject physObject, bool physicalUpdate); 255 public abstract void UpdateProperties(UpdatedProperties whichUpdated, BSPhysObject physObject);
256 256
257 // Routine used when rebuilding the body of the root of the linkset 257 // Routine used when rebuilding the body of the root of the linkset
258 // Destroy all the constraints have have been made to root. 258 // Destroy all the constraints have have been made to root.
@@ -261,11 +261,6 @@ public abstract class BSLinkset
261 // Called at taint-time!! 261 // Called at taint-time!!
262 public abstract bool RemoveBodyDependencies(BSPrim child); 262 public abstract bool RemoveBodyDependencies(BSPrim child);
263 263
264 // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
265 // this routine will restore the removed constraints.
266 // Called at taint-time!!
267 public abstract void RestoreBodyDependencies(BSPrim child);
268
269 // ================================================================ 264 // ================================================================
270 protected virtual float ComputeLinksetMass() 265 protected virtual float ComputeLinksetMass()
271 { 266 {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 8c9a774..2c8dd23 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -51,6 +51,21 @@ sealed class BSLinksetCompoundInfo : BSLinksetInfo
51 OffsetFromCenterOfMass = p; 51 OffsetFromCenterOfMass = p;
52 OffsetRot = r; 52 OffsetRot = r;
53 } 53 }
54 // 'centerDisplacement' is the distance from the root the the center-of-mass (Bullet 'zero' of the shape)
55 public BSLinksetCompoundInfo(int indx, BSPhysObject root, BSPhysObject child, OMV.Vector3 centerDisplacement)
56 {
57 // Each child position and rotation is given relative to the center-of-mass.
58 OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(root.RawOrientation);
59 OMV.Vector3 displacementFromRoot = (child.RawPosition - root.RawPosition) * invRootOrientation;
60 OMV.Vector3 displacementFromCOM = displacementFromRoot - centerDisplacement;
61 OMV.Quaternion displacementRot = child.RawOrientation * invRootOrientation;
62
63 // Save relative position for recomputing child's world position after moving linkset.
64 Index = indx;
65 OffsetFromRoot = displacementFromRoot;
66 OffsetFromCenterOfMass = displacementFromCOM;
67 OffsetRot = displacementRot;
68 }
54 public override void Clear() 69 public override void Clear()
55 { 70 {
56 Index = 0; 71 Index = 0;
@@ -182,24 +197,71 @@ public sealed class BSLinksetCompound : BSLinkset
182 197
183 // 'physicalUpdate' is true if these changes came directly from the physics engine. Don't need to rebuild then. 198 // 'physicalUpdate' is true if these changes came directly from the physics engine. Don't need to rebuild then.
184 // Called at taint-time. 199 // Called at taint-time.
185 public override void UpdateProperties(BSPhysObject updated, bool physicalUpdate) 200 public override void UpdateProperties(UpdatedProperties whichUpdated, BSPhysObject updated)
186 { 201 {
187 // The user moving a child around requires the rebuilding of the linkset compound shape 202 // The user moving a child around requires the rebuilding of the linkset compound shape
188 // One problem is this happens when a border is crossed -- the simulator implementation 203 // One problem is this happens when a border is crossed -- the simulator implementation
189 // is to store the position into the group which causes the move of the object 204 // stores the position into the group which causes the move of the object
190 // but it also means all the child positions get updated. 205 // but it also means all the child positions get updated.
191 // What would cause an unnecessary rebuild so we make sure the linkset is in a 206 // What would cause an unnecessary rebuild so we make sure the linkset is in a
192 // region before bothering to do a rebuild. 207 // region before bothering to do a rebuild.
193 if (!IsRoot(updated) 208 if (!IsRoot(updated) && PhysicsScene.TerrainManager.IsWithinKnownTerrain(LinksetRoot.RawPosition))
194 && !physicalUpdate
195 && PhysicsScene.TerrainManager.IsWithinKnownTerrain(LinksetRoot.RawPosition))
196 { 209 {
197 // TODO: replace this with are calculation of the child prim's orientation and pos. 210 // If a child of the linkset is updating only the position or rotation, that can be done
198 // TODO: for the moment, don't rebuild the compound shape. 211 // without rebuilding the linkset.
199 // This is often just the car turning its wheels. When we can just reorient the one 212 // If a handle for the child can be fetch, we update the child here. If a rebuild was
200 // member shape of the compound shape, the overhead of rebuilding won't be a problem. 213 // scheduled by someone else, the rebuild will just replace this setting.
201 // updated.LinksetInfo = null; 214
202 // ScheduleRebuild(updated); 215 bool updatedChild = false;
216 // Anything other than updating position or orientation usually means a physical update
217 // and that is caused by us updating the object.
218 if ((whichUpdated & ~(UpdatedProperties.Position | UpdatedProperties.Orientation)) == 0)
219 {
220 // Gather the child info. It might not be there if the linkset is in transition.
221 BSLinksetCompoundInfo lsi = updated.LinksetInfo as BSLinksetCompoundInfo;
222 if (LinksetRoot.PhysShape.HasPhysicalShape && lsi != null)
223 {
224 if (PhysicsScene.PE.IsCompound(LinksetRoot.PhysShape))
225 {
226 BulletShape linksetChildShape = PhysicsScene.PE.GetChildShapeFromCompoundShapeIndex(LinksetRoot.PhysShape, lsi.Index);
227 if (linksetChildShape.HasPhysicalShape)
228 {
229 // Compute the offset from the center-of-gravity
230 BSLinksetCompoundInfo newLsi = new BSLinksetCompoundInfo(lsi.Index, LinksetRoot, updated, LinksetRoot.PositionDisplacement);
231 PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, lsi.Index,
232 newLsi.OffsetFromCenterOfMass,
233 newLsi.OffsetRot,
234 true /* shouldRecalculateLocalAabb */);
235 DetailLog("{0},BSLinksetCompound.UpdateProperties,changeChildPosRot,whichUpdated={1}newLsi={2}",
236 updated.LocalID, whichUpdated, newLsi);
237 updated.LinksetInfo = newLsi;
238 updatedChild = true;
239 }
240 else // DEBUG DEBUG
241 { // DEBUG DEBUG
242 DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noChildShape,shape={1}",
243 updated.LocalID, linksetChildShape);
244 } // DEBUG DEBUG
245 }
246 else // DEBUG DEBUG
247 { // DEBUG DEBUG
248 DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,notCompound", updated.LocalID);
249 } // DEBUG DEBUG
250 }
251 else // DEBUG DEBUG
252 { // DEBUG DEBUG
253 DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,rootPhysShape={1},lsi={2}",
254 updated.LocalID, LinksetRoot.PhysShape, lsi == null ? "NULL" : lsi.ToString());
255 } // DEBUG DEBUG
256 if (!updatedChild)
257 {
258 // If couldn't do the individual child, the linkset needs a rebuild to incorporate the new child info.
259 DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild.schedulingRebuild,whichUpdated={1}",
260 updated.LocalID, whichUpdated);
261 updated.LinksetInfo = null; // setting to 'null' causes relative position to be recomputed.
262 ScheduleRebuild(updated);
263 }
264 }
203 } 265 }
204 } 266 }
205 267
@@ -228,13 +290,6 @@ public sealed class BSLinksetCompound : BSLinkset
228 return ret; 290 return ret;
229 } 291 }
230 292
231 // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
232 // this routine will restore the removed constraints.
233 // Called at taint-time!!
234 public override void RestoreBodyDependencies(BSPrim child)
235 {
236 }
237
238 // When the linkset is built, the child shape is added to the compound shape relative to the 293 // When the linkset is built, the child shape is added to the compound shape relative to the
239 // root shape. The linkset then moves around but this does not move the actual child 294 // root shape. The linkset then moves around but this does not move the actual child
240 // prim. The child prim's location must be recomputed based on the location of the root shape. 295 // prim. The child prim's location must be recomputed based on the location of the root shape.
@@ -322,7 +377,7 @@ public sealed class BSLinksetCompound : BSLinkset
322 // Constraint linksets are rebuilt every time. 377 // Constraint linksets are rebuilt every time.
323 // Note that this works for rebuilding just the root after a linkset is taken apart. 378 // Note that this works for rebuilding just the root after a linkset is taken apart.
324 // Called at taint time!! 379 // Called at taint time!!
325 private bool disableCOM = true; // disable until we get this debugged 380 private bool disableCOM = false; // disable until we get this debugged
326 private void RecomputeLinksetCompound() 381 private void RecomputeLinksetCompound()
327 { 382 {
328 try 383 try
@@ -345,12 +400,16 @@ public sealed class BSLinksetCompound : BSLinkset
345 } // DEBUG DEBUG 400 } // DEBUG DEBUG
346 else 401 else
347 { 402 {
348 centerOfMass = ComputeLinksetGeometricCenter(); 403 centerOfMass = ComputeLinksetCenterOfMass();
349 centerDisplacement = centerOfMass - LinksetRoot.RawPosition; 404 // 'centerDisplacement' is the value to *add* to all the shape offsets
405 centerDisplacement = LinksetRoot.RawPosition - centerOfMass;
350 406
351 // Since we're displacing the center of the shape, we need to move the body in the world 407 // Since we're displacing the center of the shape, we need to move the body in the world
352 LinksetRoot.PositionDisplacement = centerDisplacement; 408 LinksetRoot.PositionDisplacement = centerDisplacement;
353 409
410 // This causes the root prim position to be set properly based on the new PositionDisplacement
411 LinksetRoot.ForcePosition = LinksetRoot.RawPosition;
412 // Update the local transform for the root child shape so it is offset from the <0,0,0> which is COM
354 PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0, -centerDisplacement, OMV.Quaternion.Identity, false); 413 PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0, -centerDisplacement, OMV.Quaternion.Identity, false);
355 DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}", 414 DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}",
356 LinksetRoot.LocalID, centerOfMass, LinksetRoot.RawPosition, centerDisplacement); 415 LinksetRoot.LocalID, centerOfMass, LinksetRoot.RawPosition, centerDisplacement);
@@ -372,15 +431,7 @@ public sealed class BSLinksetCompound : BSLinkset
372 BSLinksetCompoundInfo lci = cPrim.LinksetInfo as BSLinksetCompoundInfo; 431 BSLinksetCompoundInfo lci = cPrim.LinksetInfo as BSLinksetCompoundInfo;
373 if (lci == null) 432 if (lci == null)
374 { 433 {
375 // Each child position and rotation is given relative to the center-of-mass. 434 lci = new BSLinksetCompoundInfo(memberIndex, LinksetRoot, cPrim, centerDisplacement);
376 OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);
377 OMV.Vector3 displacementFromRoot = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation;
378 OMV.Vector3 displacementFromCOM = displacementFromRoot - centerDisplacement;
379 OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation;
380
381 // Save relative position for recomputing child's world position after moving linkset.
382 lci = new BSLinksetCompoundInfo(memberIndex, displacementFromCOM, displacementRot);
383 lci.OffsetFromRoot = displacementFromRoot;
384 cPrim.LinksetInfo = lci; 435 cPrim.LinksetInfo = lci;
385 DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,creatingRelPos,lci={1}", cPrim.LocalID, lci); 436 DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,creatingRelPos,lci={1}", cPrim.LocalID, lci);
386 } 437 }
@@ -390,7 +441,7 @@ public sealed class BSLinksetCompound : BSLinkset
390 441
391 if (cPrim.PhysShape.isNativeShape) 442 if (cPrim.PhysShape.isNativeShape)
392 { 443 {
393 // A native shape is turning into a hull collision shape because native 444 // A native shape is turned into a hull collision shape because native
394 // shapes are not shared so we have to hullify it so it will be tracked 445 // shapes are not shared so we have to hullify it so it will be tracked
395 // and freed at the correct time. This also solves the scaling problem 446 // and freed at the correct time. This also solves the scaling problem
396 // (native shapes scaled but hull/meshes are assumed to not be). 447 // (native shapes scaled but hull/meshes are assumed to not be).
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
index d0b2a56..3011465 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -83,7 +83,7 @@ public sealed class BSLinksetConstraints : BSLinkset
83 } 83 }
84 84
85 // Called at taint-time!! 85 // Called at taint-time!!
86 public override void UpdateProperties(BSPhysObject updated, bool inTaintTime) 86 public override void UpdateProperties(UpdatedProperties whichUpdated, BSPhysObject pObj)
87 { 87 {
88 // Nothing to do for constraints on property updates 88 // Nothing to do for constraints on property updates
89 } 89 }
@@ -110,14 +110,6 @@ public sealed class BSLinksetConstraints : BSLinkset
110 return ret; 110 return ret;
111 } 111 }
112 112
113 // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
114 // this routine will restore the removed constraints.
115 // Called at taint-time!!
116 public override void RestoreBodyDependencies(BSPrim child)
117 {
118 // The Refresh operation queued by RemoveBodyDependencies() will build any missing constraints.
119 }
120
121 // ================================================================ 113 // ================================================================
122 114
123 // Add a new child to the linkset. 115 // Add a new child to the linkset.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index 6d0db2e..9501e2d 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -138,7 +138,8 @@ public class BSVMotor : BSMotor
138 CurrentValue = TargetValue = Vector3.Zero; 138 CurrentValue = TargetValue = Vector3.Zero;
139 } 139 }
140 140
141 // Compute the next step and return the new current value 141 // Compute the next step and return the new current value.
142 // Returns the correction needed to move 'current' to 'target'.
142 public virtual Vector3 Step(float timeStep) 143 public virtual Vector3 Step(float timeStep)
143 { 144 {
144 if (!Enabled) return TargetValue; 145 if (!Enabled) return TargetValue;
@@ -148,9 +149,10 @@ public class BSVMotor : BSMotor
148 149
149 Vector3 correction = Vector3.Zero; 150 Vector3 correction = Vector3.Zero;
150 Vector3 error = TargetValue - CurrentValue; 151 Vector3 error = TargetValue - CurrentValue;
152 LastError = error;
151 if (!ErrorIsZero(error)) 153 if (!ErrorIsZero(error))
152 { 154 {
153 correction = Step(timeStep, error); 155 correction = StepError(timeStep, error);
154 156
155 CurrentValue += correction; 157 CurrentValue += correction;
156 158
@@ -187,20 +189,31 @@ public class BSVMotor : BSMotor
187 else 189 else
188 { 190 {
189 // Difference between what we have and target is small. Motor is done. 191 // Difference between what we have and target is small. Motor is done.
192 if (TargetValue.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
193 {
194 // The target can step down to nearly zero but not get there. If close to zero
195 // it is really zero.
196 TargetValue = Vector3.Zero;
197 }
190 CurrentValue = TargetValue; 198 CurrentValue = TargetValue;
191 MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}", 199 MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},currTgt={4},currCurr={5}",
192 BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue); 200 BSScene.DetailLogZero, UseName, origCurrVal, origTarget, TargetValue, CurrentValue);
193 } 201 }
194 202
195 return CurrentValue; 203 return correction;
204 }
205 // version of step that sets the current value before doing the step
206 public virtual Vector3 Step(float timeStep, Vector3 current)
207 {
208 CurrentValue = current;
209 return Step(timeStep);
196 } 210 }
197 public virtual Vector3 Step(float timeStep, Vector3 error) 211 public virtual Vector3 StepError(float timeStep, Vector3 error)
198 { 212 {
199 if (!Enabled) return Vector3.Zero; 213 if (!Enabled) return Vector3.Zero;
200 214
201 LastError = error;
202 Vector3 returnCorrection = Vector3.Zero; 215 Vector3 returnCorrection = Vector3.Zero;
203 if (!ErrorIsZero()) 216 if (!ErrorIsZero(error))
204 { 217 {
205 // correction = error / secondsItShouldTakeToCorrect 218 // correction = error / secondsItShouldTakeToCorrect
206 Vector3 correctionAmount; 219 Vector3 correctionAmount;
@@ -302,9 +315,10 @@ public class BSFMotor : BSMotor
302 315
303 float correction = 0f; 316 float correction = 0f;
304 float error = TargetValue - CurrentValue; 317 float error = TargetValue - CurrentValue;
318 LastError = error;
305 if (!ErrorIsZero(error)) 319 if (!ErrorIsZero(error))
306 { 320 {
307 correction = Step(timeStep, error); 321 correction = StepError(timeStep, error);
308 322
309 CurrentValue += correction; 323 CurrentValue += correction;
310 324
@@ -339,6 +353,12 @@ public class BSFMotor : BSMotor
339 else 353 else
340 { 354 {
341 // Difference between what we have and target is small. Motor is done. 355 // Difference between what we have and target is small. Motor is done.
356 if (Util.InRange<float>(TargetValue, -ErrorZeroThreshold, ErrorZeroThreshold))
357 {
358 // The target can step down to nearly zero but not get there. If close to zero
359 // it is really zero.
360 TargetValue = 0f;
361 }
342 CurrentValue = TargetValue; 362 CurrentValue = TargetValue;
343 MDetailLog("{0}, BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}", 363 MDetailLog("{0}, BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}",
344 BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue); 364 BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
@@ -347,13 +367,12 @@ public class BSFMotor : BSMotor
347 return CurrentValue; 367 return CurrentValue;
348 } 368 }
349 369
350 public virtual float Step(float timeStep, float error) 370 public virtual float StepError(float timeStep, float error)
351 { 371 {
352 if (!Enabled) return 0f; 372 if (!Enabled) return 0f;
353 373
354 LastError = error;
355 float returnCorrection = 0f; 374 float returnCorrection = 0f;
356 if (!ErrorIsZero()) 375 if (!ErrorIsZero(error))
357 { 376 {
358 // correction = error / secondsItShouldTakeToCorrect 377 // correction = error / secondsItShouldTakeToCorrect
359 float correctionAmount; 378 float correctionAmount;
@@ -440,8 +459,8 @@ public class BSPIDVMotor : BSVMotor
440 } 459 }
441 } 460 }
442 461
443 // Ignore Current and Target Values and just advance the PID computation on this error. 462 // Advance the PID computation on this error.
444 public override Vector3 Step(float timeStep, Vector3 error) 463 public override Vector3 StepError(float timeStep, Vector3 error)
445 { 464 {
446 if (!Enabled) return Vector3.Zero; 465 if (!Enabled) return Vector3.Zero;
447 466
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
index 3e80aa4..9460daf 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
@@ -45,6 +45,9 @@ public static class BSParam
45 45
46 public static float MinimumObjectMass { get; private set; } 46 public static float MinimumObjectMass { get; private set; }
47 public static float MaximumObjectMass { get; private set; } 47 public static float MaximumObjectMass { get; private set; }
48 public static float MaxLinearVelocity { get; private set; }
49 public static float MaxAngularVelocity { get; private set; }
50 public static float MaxAddForceMagnitude { get; private set; }
48 51
49 public static float LinearDamping { get; private set; } 52 public static float LinearDamping { get; private set; }
50 public static float AngularDamping { get; private set; } 53 public static float AngularDamping { get; private set; }
@@ -79,6 +82,8 @@ public static class BSParam
79 public static float AvatarStepApproachFactor { get; private set; } 82 public static float AvatarStepApproachFactor { get; private set; }
80 public static float AvatarStepForceFactor { get; private set; } 83 public static float AvatarStepForceFactor { get; private set; }
81 84
85 public static float VehicleMaxLinearVelocity { get; private set; }
86 public static float VehicleMaxAngularVelocity { get; private set; }
82 public static float VehicleAngularDamping { get; private set; } 87 public static float VehicleAngularDamping { get; private set; }
83 public static float VehicleDebuggingEnabled { get; private set; } 88 public static float VehicleDebuggingEnabled { get; private set; }
84 89
@@ -103,7 +108,6 @@ public static class BSParam
103 public const float MaxDensity = 22587f; 108 public const float MaxDensity = 22587f;
104 public const float MinRestitution = 0f; 109 public const float MinRestitution = 0f;
105 public const float MaxRestitution = 1f; 110 public const float MaxRestitution = 1f;
106 public const float MaxAddForceMagnitude = 20f;
107 111
108 // =========================================================================== 112 // ===========================================================================
109 public delegate void ParamUser(BSScene scene, IConfig conf, string paramName, float val); 113 public delegate void ParamUser(BSScene scene, IConfig conf, string paramName, float val);
@@ -232,11 +236,7 @@ public static class BSParam
232 (s,cf,p,v) => { s.m_maxUpdatesPerFrame = cf.GetInt(p, (int)v); }, 236 (s,cf,p,v) => { s.m_maxUpdatesPerFrame = cf.GetInt(p, (int)v); },
233 (s) => { return (float)s.m_maxUpdatesPerFrame; }, 237 (s) => { return (float)s.m_maxUpdatesPerFrame; },
234 (s,p,l,v) => { s.m_maxUpdatesPerFrame = (int)v; } ), 238 (s,p,l,v) => { s.m_maxUpdatesPerFrame = (int)v; } ),
235 new ParameterDefn("MaxTaintsToProcessPerStep", "Number of update taints to process before each simulation step", 239
236 500f,
237 (s,cf,p,v) => { s.m_taintsToProcessPerStep = cf.GetInt(p, (int)v); },
238 (s) => { return (float)s.m_taintsToProcessPerStep; },
239 (s,p,l,v) => { s.m_taintsToProcessPerStep = (int)v; } ),
240 new ParameterDefn("MinObjectMass", "Minimum object mass (0.0001)", 240 new ParameterDefn("MinObjectMass", "Minimum object mass (0.0001)",
241 0.0001f, 241 0.0001f,
242 (s,cf,p,v) => { MinimumObjectMass = cf.GetFloat(p, v); }, 242 (s,cf,p,v) => { MinimumObjectMass = cf.GetFloat(p, v); },
@@ -247,6 +247,22 @@ public static class BSParam
247 (s,cf,p,v) => { MaximumObjectMass = cf.GetFloat(p, v); }, 247 (s,cf,p,v) => { MaximumObjectMass = cf.GetFloat(p, v); },
248 (s) => { return (float)MaximumObjectMass; }, 248 (s) => { return (float)MaximumObjectMass; },
249 (s,p,l,v) => { MaximumObjectMass = v; } ), 249 (s,p,l,v) => { MaximumObjectMass = v; } ),
250 new ParameterDefn("MaxLinearVelocity", "Maximum velocity magnitude that can be assigned to an object",
251 1000.0f,
252 (s,cf,p,v) => { MaxLinearVelocity = cf.GetFloat(p, v); },
253 (s) => { return (float)MaxLinearVelocity; },
254 (s,p,l,v) => { MaxLinearVelocity = v; } ),
255 new ParameterDefn("MaxAngularVelocity", "Maximum rotational velocity magnitude that can be assigned to an object",
256 1000.0f,
257 (s,cf,p,v) => { MaxAngularVelocity = cf.GetFloat(p, v); },
258 (s) => { return (float)MaxAngularVelocity; },
259 (s,p,l,v) => { MaxAngularVelocity = v; } ),
260 // LL documentation says thie number should be 20f for llApplyImpulse and 200f for llRezObject
261 new ParameterDefn("MaxAddForceMagnitude", "Maximum force that can be applied by llApplyImpulse (SL says 20f)",
262 20000.0f,
263 (s,cf,p,v) => { MaxAddForceMagnitude = cf.GetFloat(p, v); },
264 (s) => { return (float)MaxAddForceMagnitude; },
265 (s,p,l,v) => { MaxAddForceMagnitude = v; } ),
250 266
251 new ParameterDefn("PID_D", "Derivitive factor for motion smoothing", 267 new ParameterDefn("PID_D", "Derivitive factor for motion smoothing",
252 2200f, 268 2200f,
@@ -423,8 +439,18 @@ public static class BSParam
423 (s) => { return AvatarStepForceFactor; }, 439 (s) => { return AvatarStepForceFactor; },
424 (s,p,l,v) => { AvatarStepForceFactor = v; } ), 440 (s,p,l,v) => { AvatarStepForceFactor = v; } ),
425 441
442 new ParameterDefn("VehicleMaxLinearVelocity", "Maximum velocity magnitude that can be assigned to a vehicle",
443 1000.0f,
444 (s,cf,p,v) => { VehicleMaxLinearVelocity = cf.GetFloat(p, v); },
445 (s) => { return (float)VehicleMaxLinearVelocity; },
446 (s,p,l,v) => { VehicleMaxLinearVelocity = v; } ),
447 new ParameterDefn("VehicleMaxAngularVelocity", "Maximum rotational velocity magnitude that can be assigned to a vehicle",
448 12.0f,
449 (s,cf,p,v) => { VehicleMaxAngularVelocity = cf.GetFloat(p, v); },
450 (s) => { return (float)VehicleMaxAngularVelocity; },
451 (s,p,l,v) => { VehicleMaxAngularVelocity = v; } ),
426 new ParameterDefn("VehicleAngularDamping", "Factor to damp vehicle angular movement per second (0.0 - 1.0)", 452 new ParameterDefn("VehicleAngularDamping", "Factor to damp vehicle angular movement per second (0.0 - 1.0)",
427 0.95f, 453 0.0f,
428 (s,cf,p,v) => { VehicleAngularDamping = cf.GetFloat(p, v); }, 454 (s,cf,p,v) => { VehicleAngularDamping = cf.GetFloat(p, v); },
429 (s) => { return VehicleAngularDamping; }, 455 (s) => { return VehicleAngularDamping; },
430 (s,p,l,v) => { VehicleAngularDamping = v; } ), 456 (s,p,l,v) => { VehicleAngularDamping = v; } ),
@@ -619,11 +645,8 @@ public static class BSParam
619 entries.Add(new PhysParameterEntry(pd.name, pd.desc)); 645 entries.Add(new PhysParameterEntry(pd.name, pd.desc));
620 } 646 }
621 647
622 // make the list in alphabetical order for estetic reasons 648 // make the list alphabetical for estetic reasons
623 entries.Sort(delegate(PhysParameterEntry ppe1, PhysParameterEntry ppe2) 649 entries.Sort((ppe1, ppe2) => { return ppe1.name.CompareTo(ppe2.name); });
624 {
625 return ppe1.name.CompareTo(ppe2.name);
626 });
627 650
628 SettableParameters = entries.ToArray(); 651 SettableParameters = entries.ToArray();
629 } 652 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index 5353c75..027c786 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -55,6 +55,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin
55 * BS.ApplyCentralForce BS.ApplyTorque 55 * BS.ApplyCentralForce BS.ApplyTorque
56 */ 56 */
57 57
58// Flags used to denote which properties updates when making UpdateProperties calls to linksets, etc.
59public enum UpdatedProperties : uint
60{
61 Position = 1 << 0,
62 Orientation = 1 << 1,
63 Velocity = 1 << 2,
64 Acceleration = 1 << 3,
65 RotationalVelocity = 1 << 4,
66 EntPropUpdates = Position | Orientation | Velocity | Acceleration | RotationalVelocity,
67}
58public abstract class BSPhysObject : PhysicsActor 68public abstract class BSPhysObject : PhysicsActor
59{ 69{
60 protected BSPhysObject() 70 protected BSPhysObject()
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index aaa6fe5..f80084a 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -311,16 +311,18 @@ public sealed class BSPrim : BSPhysObject
311 _position = value; 311 _position = value;
312 PositionSanityCheck(false); 312 PositionSanityCheck(false);
313 313
314 // A linkset might need to know if a component information changed.
315 Linkset.UpdateProperties(this, false);
316
317 PhysicsScene.TaintedObject("BSPrim.setPosition", delegate() 314 PhysicsScene.TaintedObject("BSPrim.setPosition", delegate()
318 { 315 {
319 DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation); 316 DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
320 ForcePosition = _position; 317 ForcePosition = _position;
318
319 // A linkset might need to know if a component information changed.
320 Linkset.UpdateProperties(UpdatedProperties.Position, this);
321
321 }); 322 });
322 } 323 }
323 } 324 }
325
324 public override OMV.Vector3 ForcePosition { 326 public override OMV.Vector3 ForcePosition {
325 get { 327 get {
326 _position = PhysicsScene.PE.GetPosition(PhysBody) - PositionDisplacement; 328 _position = PhysicsScene.PE.GetPosition(PhysBody) - PositionDisplacement;
@@ -335,25 +337,6 @@ public sealed class BSPrim : BSPhysObject
335 } 337 }
336 } 338 }
337 } 339 }
338 // Override to have position displacement immediately update the physical position.
339 // A feeble attempt to keep the sim and physical positions in sync
340 // Must be called at taint time.
341 public override OMV.Vector3 PositionDisplacement
342 {
343 get
344 {
345 return base.PositionDisplacement;
346 }
347 set
348 {
349 base.PositionDisplacement = value;
350 PhysicsScene.TaintedObject(PhysicsScene.InTaintTime, "BSPrim.setPosition", delegate()
351 {
352 if (PhysBody.HasPhysicalBody)
353 PhysicsScene.PE.SetTranslation(PhysBody, _position + base.PositionDisplacement, _orientation);
354 });
355 }
356 }
357 340
358 // Check that the current position is sane and, if not, modify the position to make it so. 341 // Check that the current position is sane and, if not, modify the position to make it so.
359 // Check for being below terrain and being out of bounds. 342 // Check for being below terrain and being out of bounds.
@@ -370,11 +353,11 @@ public sealed class BSPrim : BSPhysObject
370 return ret; 353 return ret;
371 } 354 }
372 355
373 float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(_position); 356 float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(RawPosition);
374 OMV.Vector3 upForce = OMV.Vector3.Zero; 357 OMV.Vector3 upForce = OMV.Vector3.Zero;
375 if (RawPosition.Z < terrainHeight) 358 if (RawPosition.Z < terrainHeight)
376 { 359 {
377 DetailLog("{0},BSPrim.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, _position, terrainHeight); 360 DetailLog("{0},BSPrim.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, RawPosition, terrainHeight);
378 float targetHeight = terrainHeight + (Size.Z / 2f); 361 float targetHeight = terrainHeight + (Size.Z / 2f);
379 // If the object is below ground it just has to be moved up because pushing will 362 // If the object is below ground it just has to be moved up because pushing will
380 // not get it through the terrain 363 // not get it through the terrain
@@ -682,12 +665,13 @@ public sealed class BSPrim : BSPhysObject
682 return; 665 return;
683 _orientation = value; 666 _orientation = value;
684 667
685 // A linkset might need to know if a component information changed.
686 Linkset.UpdateProperties(this, false);
687
688 PhysicsScene.TaintedObject("BSPrim.setOrientation", delegate() 668 PhysicsScene.TaintedObject("BSPrim.setOrientation", delegate()
689 { 669 {
690 ForceOrientation = _orientation; 670 ForceOrientation = _orientation;
671
672 // A linkset might need to know if a component information changed.
673 Linkset.UpdateProperties(UpdatedProperties.Orientation, this);
674
691 }); 675 });
692 } 676 }
693 } 677 }
@@ -989,10 +973,10 @@ public sealed class BSPrim : BSPhysObject
989 } 973 }
990 set { 974 set {
991 _rotationalVelocity = value; 975 _rotationalVelocity = value;
976 Util.ClampV(_rotationalVelocity, BSParam.MaxAngularVelocity);
992 // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity); 977 // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity);
993 PhysicsScene.TaintedObject("BSPrim.setRotationalVelocity", delegate() 978 PhysicsScene.TaintedObject("BSPrim.setRotationalVelocity", delegate()
994 { 979 {
995 DetailLog("{0},BSPrim.SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
996 ForceRotationalVelocity = _rotationalVelocity; 980 ForceRotationalVelocity = _rotationalVelocity;
997 }); 981 });
998 } 982 }
@@ -1005,7 +989,9 @@ public sealed class BSPrim : BSPhysObject
1005 _rotationalVelocity = value; 989 _rotationalVelocity = value;
1006 if (PhysBody.HasPhysicalBody) 990 if (PhysBody.HasPhysicalBody)
1007 { 991 {
992 DetailLog("{0},BSPrim.ForceRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
1008 PhysicsScene.PE.SetAngularVelocity(PhysBody, _rotationalVelocity); 993 PhysicsScene.PE.SetAngularVelocity(PhysBody, _rotationalVelocity);
994 // PhysicsScene.PE.SetInterpolationAngularVelocity(PhysBody, _rotationalVelocity);
1009 ActivateIfPhysical(false); 995 ActivateIfPhysical(false);
1010 } 996 }
1011 } 997 }
@@ -1082,7 +1068,7 @@ public sealed class BSPrim : BSPhysObject
1082 OMV.Vector3 origPosition = RawPosition; // DEBUG DEBUG (for printout below) 1068 OMV.Vector3 origPosition = RawPosition; // DEBUG DEBUG (for printout below)
1083 1069
1084 // 'movePosition' is where we'd like the prim to be at this moment. 1070 // 'movePosition' is where we'd like the prim to be at this moment.
1085 OMV.Vector3 movePosition = _targetMotor.Step(timeStep); 1071 OMV.Vector3 movePosition = RawPosition + _targetMotor.Step(timeStep);
1086 1072
1087 // If we are very close to our target, turn off the movement motor. 1073 // If we are very close to our target, turn off the movement motor.
1088 if (_targetMotor.ErrorIsZero()) 1074 if (_targetMotor.ErrorIsZero())
@@ -1193,10 +1179,14 @@ public sealed class BSPrim : BSPhysObject
1193 public override float APIDDamping { set { return; } } 1179 public override float APIDDamping { set { return; } }
1194 1180
1195 public override void AddForce(OMV.Vector3 force, bool pushforce) { 1181 public override void AddForce(OMV.Vector3 force, bool pushforce) {
1182 // Per documentation, max force is limited.
1183 OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude);
1184
1196 // Since this force is being applied in only one step, make this a force per second. 1185 // Since this force is being applied in only one step, make this a force per second.
1197 OMV.Vector3 addForce = force / PhysicsScene.LastTimeStep; 1186 addForce /= PhysicsScene.LastTimeStep;
1198 AddForce(addForce, pushforce, false); 1187 AddForce(addForce, pushforce, false /* inTaintTime */);
1199 } 1188 }
1189
1200 // Applying a force just adds this to the total force on the object. 1190 // Applying a force just adds this to the total force on the object.
1201 // This added force will only last the next simulation tick. 1191 // This added force will only last the next simulation tick.
1202 public void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) { 1192 public void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
@@ -1205,9 +1195,9 @@ public sealed class BSPrim : BSPhysObject
1205 { 1195 {
1206 if (force.IsFinite()) 1196 if (force.IsFinite())
1207 { 1197 {
1208 OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude);
1209 // DetailLog("{0},BSPrim.addForce,call,force={1}", LocalID, addForce); 1198 // DetailLog("{0},BSPrim.addForce,call,force={1}", LocalID, addForce);
1210 1199
1200 OMV.Vector3 addForce = force;
1211 PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate() 1201 PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate()
1212 { 1202 {
1213 // Bullet adds this central force to the total force for this tick 1203 // Bullet adds this central force to the total force for this tick
@@ -1598,11 +1588,6 @@ public sealed class BSPrim : BSPhysObject
1598 // Called at taint-time!!! 1588 // Called at taint-time!!!
1599 public void CreateGeomAndObject(bool forceRebuild) 1589 public void CreateGeomAndObject(bool forceRebuild)
1600 { 1590 {
1601 // If this prim is part of a linkset, we must remove and restore the physical
1602 // links if the body is rebuilt.
1603 bool needToRestoreLinkset = false;
1604 bool needToRestoreVehicle = false;
1605
1606 // Create the correct physical representation for this type of object. 1591 // Create the correct physical representation for this type of object.
1607 // Updates PhysBody and PhysShape with the new information. 1592 // Updates PhysBody and PhysShape with the new information.
1608 // Ignore 'forceRebuild'. This routine makes the right choices and changes of necessary. 1593 // Ignore 'forceRebuild'. This routine makes the right choices and changes of necessary.
@@ -1611,21 +1596,10 @@ public sealed class BSPrim : BSPhysObject
1611 // Called if the current prim body is about to be destroyed. 1596 // Called if the current prim body is about to be destroyed.
1612 // Remove all the physical dependencies on the old body. 1597 // Remove all the physical dependencies on the old body.
1613 // (Maybe someday make the changing of BSShape an event to be subscribed to by BSLinkset, ...) 1598 // (Maybe someday make the changing of BSShape an event to be subscribed to by BSLinkset, ...)
1614 needToRestoreLinkset = Linkset.RemoveBodyDependencies(this); 1599 Linkset.RemoveBodyDependencies(this);
1615 needToRestoreVehicle = _vehicle.RemoveBodyDependencies(this); 1600 _vehicle.RemoveBodyDependencies(this);
1616 }); 1601 });
1617 1602
1618 if (needToRestoreLinkset)
1619 {
1620 // If physical body dependencies were removed, restore them
1621 Linkset.RestoreBodyDependencies(this);
1622 }
1623 if (needToRestoreVehicle)
1624 {
1625 // If physical body dependencies were removed, restore them
1626 _vehicle.RestoreBodyDependencies(this);
1627 }
1628
1629 // Make sure the properties are set on the new object 1603 // Make sure the properties are set on the new object
1630 UpdatePhysicalParameters(); 1604 UpdatePhysicalParameters();
1631 return; 1605 return;
@@ -1642,17 +1616,28 @@ public sealed class BSPrim : BSPhysObject
1642 // TODO: handle physics introduced by Bullet with computed vehicle physics. 1616 // TODO: handle physics introduced by Bullet with computed vehicle physics.
1643 if (_vehicle.IsActive) 1617 if (_vehicle.IsActive)
1644 { 1618 {
1645 entprop.RotationalVelocity = OMV.Vector3.Zero; 1619 // entprop.RotationalVelocity = OMV.Vector3.Zero;
1620 }
1621
1622 // DetailLog("{0},BSPrim.UpdateProperties,entry,entprop={1}", LocalID, entprop); // DEBUG DEBUG
1623
1624 // Undo any center-of-mass displacement that might have been done.
1625 if (PositionDisplacement != OMV.Vector3.Zero)
1626 {
1627 // Correct for any rotation around the center-of-mass
1628 // TODO!!!
1629 entprop.Position -= PositionDisplacement;
1646 } 1630 }
1647 1631
1648 // Assign directly to the local variables so the normal set actions do not happen 1632 // Assign directly to the local variables so the normal set actions do not happen
1649 entprop.Position -= PositionDisplacement;
1650 _position = entprop.Position; 1633 _position = entprop.Position;
1651 _orientation = entprop.Rotation; 1634 _orientation = entprop.Rotation;
1652 _velocity = entprop.Velocity; 1635 _velocity = entprop.Velocity;
1653 _acceleration = entprop.Acceleration; 1636 _acceleration = entprop.Acceleration;
1654 _rotationalVelocity = entprop.RotationalVelocity; 1637 _rotationalVelocity = entprop.RotationalVelocity;
1655 1638
1639 // DetailLog("{0},BSPrim.UpdateProperties,afterAssign,entprop={1}", LocalID, entprop); // DEBUG DEBUG
1640
1656 // The sanity check can change the velocity and/or position. 1641 // The sanity check can change the velocity and/or position.
1657 if (IsPhysical && PositionSanityCheck(true)) 1642 if (IsPhysical && PositionSanityCheck(true))
1658 { 1643 {
@@ -1661,8 +1646,7 @@ public sealed class BSPrim : BSPhysObject
1661 } 1646 }
1662 1647
1663 OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation; // DEBUG DEBUG DEBUG 1648 OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation; // DEBUG DEBUG DEBUG
1664 DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},dir={3},vel={4},rotVel={5}", 1649 DetailLog("{0},BSPrim.UpdateProperties,call,entProp={1},dir={2}", LocalID, entprop, direction);
1665 LocalID, _position, _orientation, direction, _velocity, _rotationalVelocity);
1666 1650
1667 // remember the current and last set values 1651 // remember the current and last set values
1668 LastEntityProperties = CurrentEntityProperties; 1652 LastEntityProperties = CurrentEntityProperties;
@@ -1681,7 +1665,7 @@ public sealed class BSPrim : BSPhysObject
1681 */ 1665 */
1682 1666
1683 // The linkset implimentation might want to know about this. 1667 // The linkset implimentation might want to know about this.
1684 Linkset.UpdateProperties(this, true); 1668 Linkset.UpdateProperties(UpdatedProperties.EntPropUpdates, this);
1685 } 1669 }
1686} 1670}
1687} 1671}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index e0b4992..34fd2a0 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -81,7 +81,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
81 internal long m_simulationStep = 0; 81 internal long m_simulationStep = 0;
82 internal float NominalFrameRate { get; set; } 82 internal float NominalFrameRate { get; set; }
83 public long SimulationStep { get { return m_simulationStep; } } 83 public long SimulationStep { get { return m_simulationStep; } }
84 internal int m_taintsToProcessPerStep;
85 internal float LastTimeStep { get; private set; } 84 internal float LastTimeStep { get; private set; }
86 85
87 // Physical objects can register for prestep or poststep events 86 // Physical objects can register for prestep or poststep events
@@ -709,8 +708,8 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
709 // TriggerPreStepEvent 708 // TriggerPreStepEvent
710 // DoOneTimeTaints 709 // DoOneTimeTaints
711 // Step() 710 // Step()
712 // ProcessAndForwardCollisions 711 // ProcessAndSendToSimulatorCollisions
713 // ProcessAndForwardPropertyUpdates 712 // ProcessAndSendToSimulatorPropertyUpdates
714 // TriggerPostStepEvent 713 // TriggerPostStepEvent
715 714
716 // Calls to the PhysicsActors can't directly call into the physics engine 715 // Calls to the PhysicsActors can't directly call into the physics engine
@@ -847,8 +846,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
847 846
848 #endregion // Taints 847 #endregion // Taints
849 848
850 #region INI and command line parameter processing
851
852 #region IPhysicsParameters 849 #region IPhysicsParameters
853 // Get the list of parameters this physics engine supports 850 // Get the list of parameters this physics engine supports
854 public PhysParameterEntry[] GetParameterList() 851 public PhysParameterEntry[] GetParameterList()
@@ -945,8 +942,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
945 942
946 #endregion IPhysicsParameters 943 #endregion IPhysicsParameters
947 944
948 #endregion Runtime settable parameters
949
950 // Invoke the detailed logger and output something if it's enabled. 945 // Invoke the detailed logger and output something if it's enabled.
951 public void DetailLog(string msg, params Object[] args) 946 public void DetailLog(string msg, params Object[] args)
952 { 947 {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
index 9bfec19..801f690 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
@@ -1,23 +1,22 @@
1CURRENT PRIORITIES 1CURRENT PRIORITIES
2================================================= 2=================================================
3Mantis 6040 script http://opensimulator.org/mantis/view.php?id=6040 3Deleting a linkset while standing on the root will leave the physical shape of the root behind.
4 Msg Kayaker on OSGrid when working 4 Not sure if it is because standing on it. Done with large prim linksets.
5Child movement in linkset (don't rebuild linkset)
6Vehicle angular vertical attraction
7vehicle angular banking
8Center-of-gravity
9Vehicle angular deflection
10 Preferred orientation angular correction fix
11when should angular and linear motor targets be zeroed? when selected?
12 Need a vehicle.clear()? Or an 'else' in prestep if not physical.
5Teravus llMoveToTarget script debug 13Teravus llMoveToTarget script debug
6 Mixing of hover, buoyancy/gravity, moveToTarget, into one force 14 Mixing of hover, buoyancy/gravity, moveToTarget, into one force
7Boats floating at proper level
8Nebadon vehicles turning funny in arena 15Nebadon vehicles turning funny in arena
9limitMotorUp calibration (more down?) 16limitMotorUp calibration (more down?)
10llRotLookAt 17llRotLookAt
11llLookAt 18llLookAt
12Vehicle angular vertical attraction
13Vehicle angular deflection
14 Preferred orientation angular correction fix
15vehicle angular banking
16Avatars walking up stairs (HALF DONE) 19Avatars walking up stairs (HALF DONE)
17 Radius of the capsule affects ability to climb edges.
18Vehicle movement on terrain smoothness
19When is force introduced by SetForce removed? The prestep action could go forever.
20Boats float low in the water (DONE)
21Avatar movement 20Avatar movement
22 flying into a wall doesn't stop avatar who keeps appearing to move through the obstacle (DONE) 21 flying into a wall doesn't stop avatar who keeps appearing to move through the obstacle (DONE)
23 walking up stairs is not calibrated correctly (stairs out of Kepler cabin) 22 walking up stairs is not calibrated correctly (stairs out of Kepler cabin)
@@ -73,8 +72,12 @@ Incorporate inter-relationship of angular corrections. For instance, angularDefl
73 72
74GENERAL TODO LIST: 73GENERAL TODO LIST:
75================================================= 74=================================================
75llMoveToTarget objects are not effected by gravity until target is removed.
76Implement llSetPhysicalMaterial. 76Implement llSetPhysicalMaterial.
77 extend it with Center-of-mass, rolling friction, density
77Implement llSetForceAndTorque. 78Implement llSetForceAndTorque.
79Change BSPrim.moveToTarget to used forces rather than changing position
80 Changing position allows one to move through walls
78Implement an avatar mesh shape. The Bullet capsule is way too limited. 81Implement an avatar mesh shape. The Bullet capsule is way too limited.
79 Consider just hand creating a vertex/index array in a new BSShapeAvatar. 82 Consider just hand creating a vertex/index array in a new BSShapeAvatar.
80Verify/fix phantom, volume-detect objects do not fall to infinity. Should stop at terrain. 83Verify/fix phantom, volume-detect objects do not fall to infinity. Should stop at terrain.
@@ -310,4 +313,12 @@ Remove HeightmapInfo from terrain specification (DONE)
310 Since C++ code does not need terrain height, this structure et al are not needed. 313 Since C++ code does not need terrain height, this structure et al are not needed.
311Surfboard go wonky when turning (DONE) 314Surfboard go wonky when turning (DONE)
312 Angular motor direction is global coordinates rather than local coordinates? 315 Angular motor direction is global coordinates rather than local coordinates?
313 (Resolution: made angular motor direction correct coordinate system) \ No newline at end of file 316 (Resolution: made angular motor direction correct coordinate system)
317Mantis 6040 script http://opensimulator.org/mantis/view.php?id=6040 (DONE)
318 Msg Kayaker on OSGrid when working
319 (Resolution: LINEAR_DIRECTION is in vehicle coords. Test script does the
320 same in SL as in OS/BulletSim)
321Boats float low in the water (DONE)
322Boats floating at proper level (DONE)
323When is force introduced by SetForce removed? The prestep action could go forever. (DONE)
324 (Resolution: setForce registers a prestep action which keeps applying the force) \ No newline at end of file