aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
diff options
context:
space:
mode:
authordan miller2007-10-03 01:59:43 +0000
committerdan miller2007-10-03 01:59:43 +0000
commitd36316e1c9aac6427716a99a817546002ba3d9a1 (patch)
treee05241fa456c1f723e40bcfa14569ee20a75ea73 /OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
parent* Deleted erroneous ref (diff)
downloadopensim-SC_OLD-d36316e1c9aac6427716a99a817546002ba3d9a1.zip
opensim-SC_OLD-d36316e1c9aac6427716a99a817546002ba3d9a1.tar.gz
opensim-SC_OLD-d36316e1c9aac6427716a99a817546002ba3d9a1.tar.bz2
opensim-SC_OLD-d36316e1c9aac6427716a99a817546002ba3d9a1.tar.xz
Droppin da fyzyx bomb on ya
seriously, this is quite the update. Fixes a number of nagging physics problems, including avatar shell size/shape The internal logic is quite different, and CPU usage may be affected. Also some work remains wrt flying. Please test this rev out before you deploy widely
Diffstat (limited to 'OpenSim/Region/Physics/OdePlugin/OdePlugin.cs')
-rw-r--r--OpenSim/Region/Physics/OdePlugin/OdePlugin.cs130
1 files changed, 89 insertions, 41 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
index ddc6c4a..ac784cf 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
@@ -80,6 +80,7 @@ namespace OpenSim.Region.Physics.OdePlugin
80 80
81 public class OdeScene : PhysicsScene 81 public class OdeScene : PhysicsScene
82 { 82 {
83 private static float ODE_STEPSIZE = 0.004f;
83 private IntPtr contactgroup; 84 private IntPtr contactgroup;
84 private IntPtr LandGeom; 85 private IntPtr LandGeom;
85 private double[] _heightmap; 86 private double[] _heightmap;
@@ -91,7 +92,7 @@ namespace OpenSim.Region.Physics.OdePlugin
91 public Dictionary<IntPtr, String> geom_name_map=new Dictionary<IntPtr, String>(); 92 public Dictionary<IntPtr, String> geom_name_map=new Dictionary<IntPtr, String>();
92 private d.ContactGeom[] contacts = new d.ContactGeom[30]; 93 private d.ContactGeom[] contacts = new d.ContactGeom[30];
93 private d.Contact contact; 94 private d.Contact contact;
94 95 private float step_time=0.0f;
95 public IntPtr world; 96 public IntPtr world;
96 public IntPtr space; 97 public IntPtr space;
97 public static Object OdeLock = new Object(); 98 public static Object OdeLock = new Object();
@@ -101,12 +102,15 @@ namespace OpenSim.Region.Physics.OdePlugin
101 nearCallback = near; 102 nearCallback = near;
102 triCallback = TriCallback; 103 triCallback = TriCallback;
103 triArrayCallback = TriArrayCallback; 104 triArrayCallback = TriArrayCallback;
105 /*
104 contact.surface.mode |= d.ContactFlags.Approx1 | d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP; 106 contact.surface.mode |= d.ContactFlags.Approx1 | d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP;
105 contact.surface.mu = 10.0f; 107 contact.surface.mu = 10.0f;
106 contact.surface.bounce = 0.9f; 108 contact.surface.bounce = 0.9f;
107 contact.surface.soft_erp = 0.005f; 109 contact.surface.soft_erp = 0.005f;
108 contact.surface.soft_cfm = 0.00003f; 110 contact.surface.soft_cfm = 0.00003f;
109 111 */
112 contact.surface.mu = 250.0f;
113 contact.surface.bounce = 0.2f;
110 lock (OdeLock) 114 lock (OdeLock)
111 { 115 {
112 world = d.WorldCreate(); 116 world = d.WorldCreate();
@@ -115,6 +119,8 @@ namespace OpenSim.Region.Physics.OdePlugin
115 d.WorldSetGravity(world, 0.0f, 0.0f, -10.0f); 119 d.WorldSetGravity(world, 0.0f, 0.0f, -10.0f);
116 d.WorldSetAutoDisableFlag(world, false); 120 d.WorldSetAutoDisableFlag(world, false);
117 d.WorldSetContactSurfaceLayer(world, 0.001f); 121 d.WorldSetContactSurfaceLayer(world, 0.001f);
122 d.WorldSetQuickStepNumIterations(world, 10);
123 d.WorldSetContactMaxCorrectingVel(world, 1000.0f);
118 } 124 }
119 125
120 _heightmap = new double[65536]; 126 _heightmap = new double[65536];
@@ -164,10 +170,10 @@ namespace OpenSim.Region.Physics.OdePlugin
164 { 170 {
165 foreach (OdeCharacter chr in _characters) 171 foreach (OdeCharacter chr in _characters)
166 { 172 {
167 d.SpaceCollide2(space, chr.capsule_geom, IntPtr.Zero, nearCallback); 173 d.SpaceCollide2(space, chr.Shell, IntPtr.Zero, nearCallback);
168 foreach (OdeCharacter ch2 in _characters) /// should be a separate space -- lots of avatars will be N**2 slow 174 foreach (OdeCharacter ch2 in _characters) /// should be a separate space -- lots of avatars will be N**2 slow
169 { 175 {
170 d.SpaceCollide2(chr.capsule_geom, ch2.capsule_geom, IntPtr.Zero, nearCallback); 176 d.SpaceCollide2(chr.Shell, ch2.Shell, IntPtr.Zero, nearCallback);
171 } 177 }
172 } 178 }
173 } 179 }
@@ -307,25 +313,29 @@ namespace OpenSim.Region.Physics.OdePlugin
307 313
308 public override void Simulate(float timeStep) 314 public override void Simulate(float timeStep)
309 { 315 {
316 step_time += timeStep;
310 lock (OdeLock) 317 lock (OdeLock)
311 { 318 {
312 foreach (OdePrim p in _prims) 319 foreach (OdePrim p in _prims)
313 { 320 {
314 } 321 }
315 foreach (OdeCharacter actor in _characters) 322 int i = 0;
316 { 323 while (step_time > 0.0f)
317 actor.Move(timeStep);
318 }
319 collision_optimized();
320 for (int i = 0; i < 50; i++)
321 { 324 {
322 d.WorldQuickStep(world, timeStep * 0.02f); 325 foreach (OdeCharacter actor in _characters)
326 {
327 actor.Move(timeStep);
328 }
329 collision_optimized();
330 d.WorldQuickStep(world, ODE_STEPSIZE);
331 d.JointGroupEmpty(contactgroup);
332 step_time -= ODE_STEPSIZE;
333 i++;
323 } 334 }
324 335
325 d.JointGroupEmpty(contactgroup);
326 foreach (OdeCharacter actor in _characters) 336 foreach (OdeCharacter actor in _characters)
327 { 337 {
328 actor.UpdatePosition(); 338 actor.UpdatePositionAndVelocity();
329 } 339 }
330 } 340 }
331 } 341 }
@@ -392,30 +402,35 @@ namespace OpenSim.Region.Physics.OdePlugin
392 private d.Vector3 _zeroPosition; 402 private d.Vector3 _zeroPosition;
393 private bool _zeroFlag=false; 403 private bool _zeroFlag=false;
394 private PhysicsVector _velocity; 404 private PhysicsVector _velocity;
405 private PhysicsVector _target_velocity;
395 private PhysicsVector _acceleration; 406 private PhysicsVector _acceleration;
407 private static float PID_D=4000.0f;
408 private static float PID_P=7000.0f;
409 private static float POSTURE_SERVO = 10000.0f;
396 private bool flying = false; 410 private bool flying = false;
397 //private float gravityAccel; 411 //private float gravityAccel;
398 public IntPtr BoundingCapsule; 412 public IntPtr Body;
399 private OdeScene _parent_scene; 413 private OdeScene _parent_scene;
400 public IntPtr capsule_geom; 414 public IntPtr Shell;
401 public d.Mass capsule_mass; 415 public d.Mass ShellMass;
402 416
403 public OdeCharacter(String avName, OdeScene parent_scene, PhysicsVector pos) 417 public OdeCharacter(String avName, OdeScene parent_scene, PhysicsVector pos)
404 { 418 {
405 _velocity = new PhysicsVector(); 419 _velocity = new PhysicsVector();
420 _target_velocity = new PhysicsVector();
406 _position = pos; 421 _position = pos;
407 _acceleration = new PhysicsVector(); 422 _acceleration = new PhysicsVector();
408 _parent_scene = parent_scene; 423 _parent_scene = parent_scene;
409 lock (OdeScene.OdeLock) 424 lock (OdeScene.OdeLock)
410 { 425 {
411 d.MassSetCapsule(out capsule_mass, 50.0f, 3, 0.5f, 2f); 426 Shell = d.CreateCapsule(parent_scene.space, 0.4f, 1.0f);
412 capsule_geom = d.CreateSphere(parent_scene.space, 1.0f); /// not a typo! Spheres roll, capsules tumble 427 d.MassSetCapsule(out ShellMass, 50.0f, 3, 0.4f, 1.0f);
413 BoundingCapsule = d.BodyCreate(parent_scene.world); 428 Body = d.BodyCreate(parent_scene.world);
414 d.BodySetMass(BoundingCapsule, ref capsule_mass); 429 d.BodySetMass(Body, ref ShellMass);
415 d.BodySetPosition(BoundingCapsule, pos.X, pos.Y, pos.Z); 430 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
416 d.GeomSetBody(capsule_geom, BoundingCapsule); 431 d.GeomSetBody(Shell, Body);
417 } 432 }
418 parent_scene.geom_name_map[capsule_geom]=avName; 433 parent_scene.geom_name_map[Shell]=avName;
419 434
420 } 435 }
421 436
@@ -441,7 +456,7 @@ namespace OpenSim.Region.Physics.OdePlugin
441 { 456 {
442 lock (OdeScene.OdeLock) 457 lock (OdeScene.OdeLock)
443 { 458 {
444 d.BodySetPosition(BoundingCapsule, value.X, value.Y, value.Z); 459 d.BodySetPosition(Body, value.X, value.Y, value.Z);
445 _position = value; 460 _position = value;
446 } 461 }
447 } 462 }
@@ -467,7 +482,7 @@ namespace OpenSim.Region.Physics.OdePlugin
467 } 482 }
468 set 483 set
469 { 484 {
470 _velocity = value; 485 _target_velocity = value;
471 } 486 }
472 } 487 }
473 488
@@ -522,38 +537,58 @@ namespace OpenSim.Region.Physics.OdePlugin
522 { 537 {
523 // no lock; for now it's only called from within Simulate() 538 // no lock; for now it's only called from within Simulate()
524 PhysicsVector vec = new PhysicsVector(); 539 PhysicsVector vec = new PhysicsVector();
525 d.Vector3 vel = d.BodyGetLinearVel(BoundingCapsule); 540 d.Vector3 vel = d.BodyGetLinearVel(Body);
526 541
527 // if velocity is zero, use position control; otherwise, velocity control 542 // if velocity is zero, use position control; otherwise, velocity control
528 if (_velocity.X == 0.0f & _velocity.Y == 0.0f & _velocity.Z == 0.0f & !flying) 543 if (_target_velocity.X == 0.0f & _target_velocity.Y == 0.0f & _target_velocity.Z == 0.0f)
529 { 544 {
530 // keep track of where we stopped. No more slippin' & slidin' 545 // keep track of where we stopped. No more slippin' & slidin'
531 if (!_zeroFlag) 546 if (!_zeroFlag)
532 { 547 {
533 _zeroFlag = true; 548 _zeroFlag = true;
534 _zeroPosition = d.BodyGetPosition(BoundingCapsule); 549 _zeroPosition = d.BodyGetPosition(Body);
550 }
551 d.Vector3 pos = d.BodyGetPosition(Body);
552 vec.X = (_target_velocity.X - vel.X) * PID_D + (_zeroPosition.X - pos.X) * PID_P;
553 vec.Y = (_target_velocity.Y - vel.Y) * PID_D + (_zeroPosition.Y - pos.Y) * PID_P;
554 if (flying)
555 {
556 vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
535 } 557 }
536 d.Vector3 pos = d.BodyGetPosition(BoundingCapsule);
537 vec.X = (_velocity.X - vel.X) * 75000.0f + (_zeroPosition.X - pos.X) * 120000.0f;
538 vec.Y = (_velocity.Y - vel.Y) * 75000.0f + (_zeroPosition.Y - pos.Y) * 120000.0f;
539 } 558 }
540 else 559 else
541 { 560 {
542 _zeroFlag = false; 561 _zeroFlag = false;
543 vec.X = (_velocity.X - vel.X) * 75000.0f; 562 vec.X = (_target_velocity.X - vel.X) * PID_D;
544 vec.Y = (_velocity.Y - vel.Y) * 75000.0f; 563 vec.Y = (_target_velocity.Y - vel.Y) * PID_D;
545 if (flying) 564 if (flying)
546 { 565 {
547 vec.Z = (_velocity.Z - vel.Z) * 75000.0f; 566 vec.Z = (_target_velocity.Z - vel.Z) * PID_D;
548 } 567 }
549 } 568 }
550 d.BodyAddForce(this.BoundingCapsule, vec.X, vec.Y, vec.Z); 569 if (flying)
570 {
571 vec.Z += 10.0f;
572 }
573 d.BodyAddForce(this.Body, vec.X, vec.Y, vec.Z);
574
575 // ok -- let's stand up straight!
576 d.Vector3 feet;
577 d.Vector3 head;
578 d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet);
579 d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head);
580 float posture = head.Z - feet.Z;
581
582 // restoring force proportional to lack of posture:
583 float servo = (2.5f-posture) * POSTURE_SERVO;
584 d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f);
585 d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f);
551 } 586 }
552 587
553 public void UpdatePosition() 588 public void UpdatePositionAndVelocity()
554 { 589 {
555 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! 590 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
556 d.Vector3 vec = d.BodyGetPosition(BoundingCapsule); 591 d.Vector3 vec = d.BodyGetPosition(Body);
557 592
558 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) 593 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
559 if (vec.X < 0.0f) vec.X = 0.0f; 594 if (vec.X < 0.0f) vec.X = 0.0f;
@@ -564,16 +599,29 @@ namespace OpenSim.Region.Physics.OdePlugin
564 this._position.X = vec.X; 599 this._position.X = vec.X;
565 this._position.Y = vec.Y; 600 this._position.Y = vec.Y;
566 this._position.Z = vec.Z; 601 this._position.Z = vec.Z;
602
603 if (_zeroFlag)
604 {
605 _velocity.X = 0.0f;
606 _velocity.Y = 0.0f;
607 _velocity.Z = 0.0f;
608 }
609 else
610 {
611 vec = d.BodyGetLinearVel(Body);
612 _velocity.X = vec.X;
613 _velocity.Y = vec.Y;
614 _velocity.Z = vec.Z;
615 }
567 } 616 }
568 617
569 public void Destroy() 618 public void Destroy()
570 { 619 {
571 lock (OdeScene.OdeLock) 620 lock (OdeScene.OdeLock)
572 { 621 {
573 d.GeomDestroy(this.capsule_geom); 622 d.GeomDestroy(this.Shell);
574 Console.WriteLine("+++ removing geom"); 623 this._parent_scene.geom_name_map.Remove(this.Shell);
575 this._parent_scene.geom_name_map.Remove(this.capsule_geom); 624 d.BodyDestroy(this.Body);
576 d.BodyDestroy(this.BoundingCapsule);
577 } 625 }
578 } 626 }
579 } 627 }