aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/OdePlugin/OdePlugin.cs132
1 files changed, 98 insertions, 34 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
index 27c3a6a..4afe784 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
@@ -83,8 +83,10 @@ namespace OpenSim.Region.Physics.OdePlugin
83 private List<OdeCharacter> _characters = new List<OdeCharacter>(); 83 private List<OdeCharacter> _characters = new List<OdeCharacter>();
84 private List<OdePrim> _prims = new List<OdePrim>(); 84 private List<OdePrim> _prims = new List<OdePrim>();
85 public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>(); 85 public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
86 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
86 private d.ContactGeom[] contacts = new d.ContactGeom[30]; 87 private d.ContactGeom[] contacts = new d.ContactGeom[30];
87 private d.Contact contact; 88 private d.Contact contact;
89 private PhysicsActor PANull = new NullPhysicsActor();
88 private float step_time = 0.0f; 90 private float step_time = 0.0f;
89 public IntPtr world; 91 public IntPtr world;
90 public IntPtr space; 92 public IntPtr space;
@@ -117,36 +119,33 @@ namespace OpenSim.Region.Physics.OdePlugin
117 } 119 }
118 120
119 _heightmap = new double[258*258]; 121 _heightmap = new double[258*258];
122
120 } 123 }
121 124
122 // This function blatantly ripped off from BoxStack.cs 125 // This function blatantly ripped off from BoxStack.cs
123 private void near(IntPtr space, IntPtr g1, IntPtr g2) 126 private void near(IntPtr space, IntPtr g1, IntPtr g2)
124 { 127 {
125 // no lock here! It's invoked from within Simulate(), which is thread-locked 128 // no lock here! It's invoked from within Simulate(), which is thread-locked
129 IntPtr b1 = d.GeomGetBody(g1);
130 IntPtr b2 = d.GeomGetBody(g2);
131
132
126 if (g1 == g2) 133 if (g1 == g2)
127 return; // Can't collide with yourself 134 return; // Can't collide with yourself
135
128 136
129 IntPtr b1 = d.GeomGetBody(g1);
130 IntPtr b2 = d.GeomGetBody(g2);
131 137
132 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) 138 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
133 return; 139 return;
134 140
141
135 d.GeomClassID id = d.GeomGetClass(g1); 142 d.GeomClassID id = d.GeomGetClass(g1);
136 if (id == d.GeomClassID.TriMeshClass) 143 if (id == d.GeomClassID.TriMeshClass)
137 { 144 {
138 String name1 = null; 145
139 String name2 = null;
140 if (!geom_name_map.TryGetValue(g1, out name1))
141 {
142 name1 = "null";
143 }
144 if (!geom_name_map.TryGetValue(g2, out name2))
145 {
146 name2 = "null";
147 }
148 146
149// MainLog.Instance.Verbose("near: A collision was detected between {1} and {2}", 0, name1, name2); 147// MainLog.Instance.Verbose("near: A collision was detected between {1} and {2}", 0, name1, name2);
148 //System.Console.WriteLine("near: A collision was detected between {1} and {2}", 0, name1, name2);
150 } 149 }
151 150
152 int count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.SizeOf); 151 int count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.SizeOf);
@@ -155,6 +154,22 @@ namespace OpenSim.Region.Physics.OdePlugin
155 contact.geom = contacts[i]; 154 contact.geom = contacts[i];
156 IntPtr joint = d.JointCreateContact(world, contactgroup, ref contact); 155 IntPtr joint = d.JointCreateContact(world, contactgroup, ref contact);
157 d.JointAttach(joint, b1, b2); 156 d.JointAttach(joint, b1, b2);
157 PhysicsActor p1;
158 PhysicsActor p2;
159
160
161 if (!actor_name_map.TryGetValue(g1, out p1))
162 {
163 p1 = PANull;
164 }
165 if (!actor_name_map.TryGetValue(g2, out p2))
166 {
167 p2 = PANull;
168 }
169
170 p1.IsColliding = true;
171 p2.IsColliding = true;
172 //System.Console.WriteLine("near: A collision was detected between {1} and {2}", 0, name1, name2);
158 } 173 }
159 } 174 }
160 175
@@ -162,10 +177,19 @@ namespace OpenSim.Region.Physics.OdePlugin
162 { 177 {
163 foreach (OdeCharacter chr in _characters) 178 foreach (OdeCharacter chr in _characters)
164 { 179 {
180 chr.IsColliding = false;
181 }
182 foreach (OdeCharacter chr in _characters)
183 {
184
185
186
165 d.SpaceCollide2(space, chr.Shell, IntPtr.Zero, nearCallback); 187 d.SpaceCollide2(space, chr.Shell, IntPtr.Zero, nearCallback);
166 foreach (OdeCharacter ch2 in _characters) 188 foreach (OdeCharacter ch2 in _characters)
167 /// should be a separate space -- lots of avatars will be N**2 slow 189 /// should be a separate space -- lots of avatars will be N**2 slow
168 { 190 {
191
192
169 d.SpaceCollide2(chr.Shell, ch2.Shell, IntPtr.Zero, nearCallback); 193 d.SpaceCollide2(chr.Shell, ch2.Shell, IntPtr.Zero, nearCallback);
170 } 194 }
171 } 195 }
@@ -333,8 +357,9 @@ namespace OpenSim.Region.Physics.OdePlugin
333 { 357 {
334 foreach (OdeCharacter actor in _characters) 358 foreach (OdeCharacter actor in _characters)
335 { 359 {
336 actor.Move(timeStep); 360 actor.Move(timeStep);
337 } 361 }
362
338 collision_optimized(); 363 collision_optimized();
339 d.WorldQuickStep(world, ODE_STEPSIZE); 364 d.WorldQuickStep(world, ODE_STEPSIZE);
340 d.JointGroupEmpty(contactgroup); 365 d.JointGroupEmpty(contactgroup);
@@ -457,6 +482,8 @@ namespace OpenSim.Region.Physics.OdePlugin
457 public static float CAPSULE_RADIUS = 0.5f; 482 public static float CAPSULE_RADIUS = 0.5f;
458 public static float CAPSULE_LENGTH = 0.9f; 483 public static float CAPSULE_LENGTH = 0.9f;
459 private bool flying = false; 484 private bool flying = false;
485 private bool iscolliding = false;
486 private bool jumping = false;
460 //private float gravityAccel; 487 //private float gravityAccel;
461 public IntPtr Body; 488 public IntPtr Body;
462 private OdeScene _parent_scene; 489 private OdeScene _parent_scene;
@@ -480,6 +507,7 @@ namespace OpenSim.Region.Physics.OdePlugin
480 d.GeomSetBody(Shell, Body); 507 d.GeomSetBody(Shell, Body);
481 } 508 }
482 parent_scene.geom_name_map[Shell] = avName; 509 parent_scene.geom_name_map[Shell] = avName;
510 parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
483 } 511 }
484 512
485 public override bool Flying 513 public override bool Flying
@@ -487,7 +515,12 @@ namespace OpenSim.Region.Physics.OdePlugin
487 get { return flying; } 515 get { return flying; }
488 set { flying = value; } 516 set { flying = value; }
489 } 517 }
490 518 public override bool IsColliding
519 {
520 get { return iscolliding; }
521 set
522 {iscolliding = value;}
523 }
491 public override PhysicsVector Position 524 public override PhysicsVector Position
492 { 525 {
493 get { return _position; } 526 get { return _position; }
@@ -538,12 +571,35 @@ namespace OpenSim.Region.Physics.OdePlugin
538 571
539 public override void AddForce(PhysicsVector force) 572 public override void AddForce(PhysicsVector force)
540 { 573 {
574
575 _target_velocity.X += force.X;
576 _target_velocity.Y += force.Y;
577 _target_velocity.Z += force.Z;
578
579
541 } 580 }
581 public void doForce(PhysicsVector force)
582 {
583 d.BodyAddForce(Body, force.X, force.Y, force.Z);
584
585 // ok -- let's stand up straight!
586 d.Vector3 feet;
587 d.Vector3 head;
588 d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet);
589 d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head);
590 float posture = head.Z - feet.Z;
542 591
592 // restoring force proportional to lack of posture:
593 float servo = (2.5f - posture) * POSTURE_SERVO;
594 d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f);
595 d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f);
596
597 }
543 public override void SetMomentum(PhysicsVector momentum) 598 public override void SetMomentum(PhysicsVector momentum)
544 { 599 {
545 }
546 600
601 }
602
547 public void Move(float timeStep) 603 public void Move(float timeStep)
548 { 604 {
549 // no lock; for now it's only called from within Simulate() 605 // no lock; for now it's only called from within Simulate()
@@ -551,7 +607,7 @@ namespace OpenSim.Region.Physics.OdePlugin
551 d.Vector3 vel = d.BodyGetLinearVel(Body); 607 d.Vector3 vel = d.BodyGetLinearVel(Body);
552 608
553 // if velocity is zero, use position control; otherwise, velocity control 609 // if velocity is zero, use position control; otherwise, velocity control
554 if (_target_velocity.X == 0.0f & _target_velocity.Y == 0.0f & _target_velocity.Z == 0.0f) 610 if (_target_velocity.X == 0.0f & _target_velocity.Y == 0.0f & _target_velocity.Z == 0.0f & iscolliding)
555 { 611 {
556 // keep track of where we stopped. No more slippin' & slidin' 612 // keep track of where we stopped. No more slippin' & slidin'
557 if (!_zeroFlag) 613 if (!_zeroFlag)
@@ -569,9 +625,19 @@ namespace OpenSim.Region.Physics.OdePlugin
569 } 625 }
570 else 626 else
571 { 627 {
628
572 _zeroFlag = false; 629 _zeroFlag = false;
573 vec.X = (_target_velocity.X - vel.X)*PID_D; 630 if (iscolliding || flying)
574 vec.Y = (_target_velocity.Y - vel.Y)*PID_D; 631 {
632 vec.X = (_target_velocity.X - vel.X) * PID_D;
633 vec.Y = (_target_velocity.Y - vel.Y) * PID_D;
634 }
635 if (iscolliding && !flying && _target_velocity.Z > 0.0f)
636 {
637 d.Vector3 pos = d.BodyGetPosition(Body);
638 vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
639 }
640
575 if (flying) 641 if (flying)
576 { 642 {
577 vec.Z = (_target_velocity.Z - vel.Z)*PID_D; 643 vec.Z = (_target_velocity.Z - vel.Z)*PID_D;
@@ -581,19 +647,7 @@ namespace OpenSim.Region.Physics.OdePlugin
581 { 647 {
582 vec.Z += 10.0f; 648 vec.Z += 10.0f;
583 } 649 }
584 d.BodyAddForce(Body, vec.X, vec.Y, vec.Z); 650 doForce(vec);
585
586 // ok -- let's stand up straight!
587 d.Vector3 feet;
588 d.Vector3 head;
589 d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet);
590 d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head);
591 float posture = head.Z - feet.Z;
592
593 // restoring force proportional to lack of posture:
594 float servo = (2.5f - posture)*POSTURE_SERVO;
595 d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f);
596 d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f);
597 } 651 }
598 652
599 public void UpdatePositionAndVelocity() 653 public void UpdatePositionAndVelocity()
@@ -649,6 +703,7 @@ namespace OpenSim.Region.Physics.OdePlugin
649 private OdeScene _parent_scene; 703 private OdeScene _parent_scene;
650 public IntPtr prim_geom; 704 public IntPtr prim_geom;
651 public IntPtr _triMeshData; 705 public IntPtr _triMeshData;
706 private bool iscolliding = false;
652 707
653 public OdePrim(String primName, OdeScene parent_scene, PhysicsVector pos, PhysicsVector size, 708 public OdePrim(String primName, OdeScene parent_scene, PhysicsVector pos, PhysicsVector size,
654 Quaternion rotation, Mesh mesh, PrimitiveBaseShape pbs) 709 Quaternion rotation, Mesh mesh, PrimitiveBaseShape pbs)
@@ -661,6 +716,7 @@ namespace OpenSim.Region.Physics.OdePlugin
661 _mesh = mesh; 716 _mesh = mesh;
662 _pbs = pbs; 717 _pbs = pbs;
663 _parent_scene = parent_scene; 718 _parent_scene = parent_scene;
719
664 720
665 lock (OdeScene.OdeLock) 721 lock (OdeScene.OdeLock)
666 { 722 {
@@ -681,6 +737,7 @@ namespace OpenSim.Region.Physics.OdePlugin
681 myrot.Z = rotation.z; 737 myrot.Z = rotation.z;
682 d.GeomSetQuaternion(prim_geom, ref myrot); 738 d.GeomSetQuaternion(prim_geom, ref myrot);
683 parent_scene.geom_name_map[prim_geom] = primName; 739 parent_scene.geom_name_map[prim_geom] = primName;
740 parent_scene.actor_name_map[prim_geom] = (PhysicsActor) this;
684 // don't do .add() here; old geoms get recycled with the same hash 741 // don't do .add() here; old geoms get recycled with the same hash
685 } 742 }
686 } 743 }
@@ -708,6 +765,13 @@ namespace OpenSim.Region.Physics.OdePlugin
708 set { } 765 set { }
709 } 766 }
710 767
768 public override bool IsColliding
769 {
770 get { return iscolliding; }
771 set { iscolliding = value; }
772 }
773
774
711 public override PhysicsVector Position 775 public override PhysicsVector Position
712 { 776 {
713 get { return _position; } 777 get { return _position; }
@@ -793,4 +857,4 @@ namespace OpenSim.Region.Physics.OdePlugin
793 { 857 {
794 } 858 }
795 } 859 }
796} \ No newline at end of file 860}