aboutsummaryrefslogtreecommitdiffstatshomepage
diff options
context:
space:
mode:
Diffstat (limited to '')
-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 }