aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/UbitOdePlugin
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/UbitOdePlugin')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs710
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs16
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs4
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs183
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs358
5 files changed, 921 insertions, 350 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
index f5bf05d..bb04ea7 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -74,19 +74,32 @@ namespace OpenSim.Region.Physics.OdePlugin
74 74
75 private Vector3 _position; 75 private Vector3 _position;
76 private Vector3 _zeroPosition; 76 private Vector3 _zeroPosition;
77 private bool _zeroFlag = false;
78 private Vector3 _velocity; 77 private Vector3 _velocity;
79 private Vector3 _target_velocity; 78 private Vector3 _target_velocity;
80 private Vector3 _acceleration; 79 private Vector3 _acceleration;
81 private Vector3 m_rotationalVelocity; 80 private Vector3 m_rotationalVelocity;
81 private Vector3 m_size;
82 private Quaternion m_orientation;
83 private Quaternion m_orientation2D;
82 private float m_mass = 80f; 84 private float m_mass = 80f;
83 public float m_density = 60f; 85 public float m_density = 60f;
84 private bool m_pidControllerActive = true; 86 private bool m_pidControllerActive = true;
85 public float PID_D = 800.0f; 87
86 public float PID_P = 900.0f; 88 const float basePID_D = 0.55f; // scaled for unit mass unit time (2200 /(50*80))
87 //private static float POSTURE_SERVO = 10000.0f; 89 const float basePID_P = 0.225f; // scaled for unit mass unit time (900 /(50*80))
88 public float CAPSULE_RADIUS = 0.37f; 90 public float PID_D;
89 public float CAPSULE_LENGTH = 2.140599f; 91 public float PID_P;
92
93 private float timeStep;
94 private float invtimeStep;
95
96 private float m_feetOffset = 0;
97 private float feetOff = 0;
98 private float feetSZ = 0.5f;
99 const float feetScale = 0.8f;
100 private float boneOff = 0;
101 private float m_lastVelocitySqr = 0;
102
90 public float walkDivisor = 1.3f; 103 public float walkDivisor = 1.3f;
91 public float runDivisor = 0.8f; 104 public float runDivisor = 0.8f;
92 private bool flying = false; 105 private bool flying = false;
@@ -94,6 +107,9 @@ namespace OpenSim.Region.Physics.OdePlugin
94 private bool m_iscollidingGround = false; 107 private bool m_iscollidingGround = false;
95 private bool m_iscollidingObj = false; 108 private bool m_iscollidingObj = false;
96 private bool m_alwaysRun = false; 109 private bool m_alwaysRun = false;
110
111 private bool _zeroFlag = false;
112
97 private int m_requestedUpdateFrequency = 0; 113 private int m_requestedUpdateFrequency = 0;
98 private uint m_localID = 0; 114 private uint m_localID = 0;
99 public bool m_returnCollisions = false; 115 public bool m_returnCollisions = false;
@@ -111,6 +127,7 @@ namespace OpenSim.Region.Physics.OdePlugin
111 int m_colliderfilter = 0; 127 int m_colliderfilter = 0;
112 int m_colliderGroundfilter = 0; 128 int m_colliderGroundfilter = 0;
113 int m_colliderObjectfilter = 0; 129 int m_colliderObjectfilter = 0;
130 bool m_collisionException = false;
114 131
115 // Default we're a Character 132 // Default we're a Character
116 private CollisionCategories m_collisionCategories = (CollisionCategories.Character); 133 private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
@@ -123,10 +140,18 @@ namespace OpenSim.Region.Physics.OdePlugin
123 // we do land collisions not ode | CollisionCategories.Land); 140 // we do land collisions not ode | CollisionCategories.Land);
124 public IntPtr Body = IntPtr.Zero; 141 public IntPtr Body = IntPtr.Zero;
125 private OdeScene _parent_scene; 142 private OdeScene _parent_scene;
126 public IntPtr Shell = IntPtr.Zero; 143 private IntPtr topbox = IntPtr.Zero;
144 private IntPtr midbox = IntPtr.Zero;
145 private IntPtr feetbox = IntPtr.Zero;
146 private IntPtr bbox = IntPtr.Zero;
147 public IntPtr collider = IntPtr.Zero;
148
127 public IntPtr Amotor = IntPtr.Zero; 149 public IntPtr Amotor = IntPtr.Zero;
150
128 public d.Mass ShellMass; 151 public d.Mass ShellMass;
129// public bool collidelock = false; 152
153
154
130 155
131 public int m_eventsubscription = 0; 156 public int m_eventsubscription = 0;
132 private int m_cureventsubscription = 0; 157 private int m_cureventsubscription = 0;
@@ -139,9 +164,15 @@ namespace OpenSim.Region.Physics.OdePlugin
139 164
140 float mu; 165 float mu;
141 166
142 public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float density, float walk_divisor, float rundivisor) 167
168
169 public OdeCharacter(uint localID, String avName, OdeScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor)
143 { 170 {
144 m_uuid = UUID.Random(); 171 m_uuid = UUID.Random();
172 m_localID = localID;
173
174 timeStep = parent_scene.ODE_STEPSIZE;
175 invtimeStep = 1 / timeStep;
145 176
146 if (pos.IsFinite()) 177 if (pos.IsFinite())
147 { 178 {
@@ -163,22 +194,37 @@ namespace OpenSim.Region.Physics.OdePlugin
163 194
164 _parent_scene = parent_scene; 195 _parent_scene = parent_scene;
165 196
166 PID_D = pid_d; 197
167 PID_P = pid_p; 198 m_size.X = pSize.X;
168 CAPSULE_RADIUS = capsule_radius; 199 m_size.Y = pSize.Y;
200 m_size.Z = pSize.Z;
201
202 if(m_size.X <0.01f)
203 m_size.X = 0.01f;
204 if(m_size.Y <0.01f)
205 m_size.Y = 0.01f;
206 if(m_size.Z <0.01f)
207 m_size.Z = 0.01f;
208
209 m_feetOffset = pfeetOffset;
210 m_orientation = Quaternion.Identity;
211 m_orientation2D = Quaternion.Identity;
169 m_density = density; 212 m_density = density;
170 m_mass = 80f; // sure we have a default
171 213
172 // force lower density for testing 214 // force lower density for testing
173 m_density = 3.0f; 215 m_density = 3.0f;
174 216
217 m_density *= 1.4f; // scale to have mass similar to capsule
218
175 mu = parent_scene.AvatarFriction; 219 mu = parent_scene.AvatarFriction;
176 220
177 walkDivisor = walk_divisor; 221 walkDivisor = walk_divisor;
178 runDivisor = rundivisor; 222 runDivisor = rundivisor;
179 223
180 CAPSULE_LENGTH = size.Z * 1.15f - CAPSULE_RADIUS * 2.0f; 224 m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default
181 //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); 225
226 PID_D = basePID_D * m_mass * invtimeStep;
227 PID_P = basePID_P * m_mass * invtimeStep;
182 228
183 m_isPhysical = false; // current status: no ODE information exists 229 m_isPhysical = false; // current status: no ODE information exists
184 230
@@ -261,7 +307,7 @@ namespace OpenSim.Region.Physics.OdePlugin
261 set 307 set
262 { 308 {
263 flying = value; 309 flying = value;
264 // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying); 310// m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying);
265 } 311 }
266 } 312 }
267 313
@@ -305,25 +351,25 @@ namespace OpenSim.Region.Physics.OdePlugin
305 get { return m_iscollidingGround; } 351 get { return m_iscollidingGround; }
306 set 352 set
307 { 353 {
308 /* we now control this 354/* we now control this
309 if (value) 355 if (value)
310 { 356 {
311 m_colliderGroundfilter += 2; 357 m_colliderGroundfilter += 2;
312 if (m_colliderGroundfilter > 2) 358 if (m_colliderGroundfilter > 2)
313 m_colliderGroundfilter = 2; 359 m_colliderGroundfilter = 2;
314 } 360 }
315 else 361 else
316 { 362 {
317 m_colliderGroundfilter--; 363 m_colliderGroundfilter--;
318 if (m_colliderGroundfilter < 0) 364 if (m_colliderGroundfilter < 0)
319 m_colliderGroundfilter = 0; 365 m_colliderGroundfilter = 0;
320 } 366 }
321 367
322 if (m_colliderGroundfilter == 0) 368 if (m_colliderGroundfilter == 0)
323 m_iscollidingGround = false; 369 m_iscollidingGround = false;
324 else 370 else
325 m_iscollidingGround = true; 371 m_iscollidingGround = true;
326 */ 372 */
327 } 373 }
328 374
329 } 375 }
@@ -355,7 +401,7 @@ namespace OpenSim.Region.Physics.OdePlugin
355 else 401 else
356 m_iscollidingObj = true; 402 m_iscollidingObj = true;
357 403
358 // m_iscollidingObj = value; 404// m_iscollidingObj = value;
359 405
360 if (m_iscollidingObj) 406 if (m_iscollidingObj)
361 m_pidControllerActive = false; 407 m_pidControllerActive = false;
@@ -420,13 +466,21 @@ namespace OpenSim.Region.Physics.OdePlugin
420 /// </summary> 466 /// </summary>
421 public override Vector3 Size 467 public override Vector3 Size
422 { 468 {
423 get { 469 get
424 float d = CAPSULE_RADIUS * 2; 470 {
425 return new Vector3(d, d, (CAPSULE_LENGTH +d)/1.15f); } 471 return m_size;
472 }
426 set 473 set
427 { 474 {
428 if (value.IsFinite()) 475 if (value.IsFinite())
429 { 476 {
477 if(value.X <0.01f)
478 value.X = 0.01f;
479 if(value.Y <0.01f)
480 value.Y = 0.01f;
481 if(value.Z <0.01f)
482 value.Z = 0.01f;
483
430 AddChange(changes.Size, value); 484 AddChange(changes.Size, value);
431 } 485 }
432 else 486 else
@@ -436,6 +490,28 @@ namespace OpenSim.Region.Physics.OdePlugin
436 } 490 }
437 } 491 }
438 492
493 public override void setAvatarSize(Vector3 size, float feetOffset)
494 {
495 if (size.IsFinite())
496 {
497 if (size.X < 0.01f)
498 size.X = 0.01f;
499 if (size.Y < 0.01f)
500 size.Y = 0.01f;
501 if (size.Z < 0.01f)
502 size.Z = 0.01f;
503
504 strAvatarSize st = new strAvatarSize();
505 st.size = size;
506 st.offset = feetOffset;
507 AddChange(changes.AvatarSize, st);
508 }
509 else
510 {
511 m_log.Warn("[PHYSICS]: Got a NaN AvatarSize from Scene on a Character");
512 }
513
514 }
439 /// <summary> 515 /// <summary>
440 /// This creates the Avatar's physical Surrogate at the position supplied 516 /// This creates the Avatar's physical Surrogate at the position supplied
441 /// </summary> 517 /// </summary>
@@ -452,8 +528,7 @@ namespace OpenSim.Region.Physics.OdePlugin
452 { 528 {
453 get 529 get
454 { 530 {
455 float AVvolume = (float)(Math.PI * CAPSULE_RADIUS * CAPSULE_RADIUS * (1.3333333333f * CAPSULE_RADIUS + CAPSULE_LENGTH)); 531 return m_mass;
456 return m_density * AVvolume;
457 } 532 }
458 } 533 }
459 public override void link(PhysicsActor obj) 534 public override void link(PhysicsActor obj)
@@ -571,9 +646,13 @@ namespace OpenSim.Region.Physics.OdePlugin
571 646
572 public override Quaternion Orientation 647 public override Quaternion Orientation
573 { 648 {
574 get { return Quaternion.Identity; } 649 get { return m_orientation; }
575 set 650 set
576 { 651 {
652// fakeori = value;
653// givefakeori++;
654 value.Normalize();
655 AddChange(changes.Orientation, value);
577 } 656 }
578 } 657 }
579 658
@@ -625,54 +704,116 @@ namespace OpenSim.Region.Physics.OdePlugin
625 AddChange(changes.Momentum, momentum); 704 AddChange(changes.Momentum, momentum);
626 } 705 }
627 706
707 private void ajustCollider()
708 {
709 float vq = _velocity.LengthSquared();
710 if (m_lastVelocitySqr != vq)
711 {
712 m_lastVelocitySqr = vq;
713 if (vq > 100.0f)
714 {
715 Vector3 off = _velocity;
716 float t = 0.5f * timeStep;
717 off = off * t;
718 d.GeomSetOffsetPosition(bbox, off.X, off.Y, off.Z);
719 off.X = 2.0f * (m_size.X + Math.Abs(off.X));
720 off.Y = 2.0f * (m_size.Y + Math.Abs(off.Y));
721 off.Z = m_size.Z + 2.0f * Math.Abs(off.Z);
722 d.GeomBoxSetLengths(bbox, off.X, off.Y, off.Z);
723
724 d.GeomSetCategoryBits(bbox, (uint)m_collisionCategories);
725 d.GeomSetCollideBits(bbox, (uint)m_collisionFlags);
726 d.GeomSetCategoryBits(topbox, 0);
727 d.GeomSetCollideBits(topbox, 0);
728 d.GeomSetCategoryBits(midbox, 0);
729 d.GeomSetCollideBits(midbox, 0);
730 d.GeomSetCategoryBits(feetbox, 0);
731 d.GeomSetCollideBits(feetbox, 0);
732 }
733 else
734 {
735 d.GeomSetCategoryBits(bbox, 0);
736 d.GeomSetCollideBits(bbox, 0);
737 d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories);
738 d.GeomSetCollideBits(topbox, (uint)m_collisionFlags);
739 d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories);
740 d.GeomSetCollideBits(midbox, (uint)m_collisionFlags);
741 d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories);
742 d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags);
743 }
744 }
745 }
628 746
629 // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
630 // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only
631 // place that is safe to call this routine AvatarGeomAndBodyCreation.
632 private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) 747 private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
633 { 748 {
634 _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); 749 // sizes one day should came from visual parameters
635 if (CAPSULE_LENGTH <= 0) 750 float sx = m_size.X;
636 { 751 float sy = m_size.Y;
637 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); 752 float sz = m_size.Z;
638 CAPSULE_LENGTH = 0.01f;
639 753
640 } 754 float topsx = sx * 0.9f;
755 float midsx = sx;
756 float feetsx = sx * feetScale;
757 float bonesx = sx * 0.2f;
641 758
642 if (CAPSULE_RADIUS <= 0) 759 float topsy = sy * 0.4f;
643 { 760 float midsy = sy;
644 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); 761 float feetsy = sy * feetScale * 0.8f;
645 CAPSULE_RADIUS = 0.01f; 762 float bonesy = feetsy * 0.2f;
646 763
647 } 764 float topsz = sz * 0.15f;
648 Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH); 765 float feetsz = sz * 0.45f;
766 if (feetsz > 0.6f)
767 feetsz = 0.6f;
768
769 float midsz = sz - topsz - feetsz;
770 float bonesz = sz;
771
772 float bot = -sz * 0.5f + m_feetOffset;
773
774 boneOff = bot + 0.3f;
775
776 float feetz = bot + feetsz * 0.5f;
777 bot += feetsz;
649 778
650 d.GeomSetCategoryBits(Shell, (uint)m_collisionCategories); 779 feetOff = bot;
651 d.GeomSetCollideBits(Shell, (uint)m_collisionFlags); 780 feetSZ = feetsz;
652 781
653 d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH); 782 float midz = bot + midsz * 0.5f;
783 bot += midsz;
784 float topz = bot + topsz * 0.5f;
654 785
655 m_mass = ShellMass.mass; // update mass 786 _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace);
656 787
657 // rescale PID parameters 788 collider = d.HashSpaceCreate(_parent_scene.CharsSpace);
658 PID_D = _parent_scene.avPIDD; 789 d.HashSpaceSetLevels(collider, -4, 3);
659 PID_P = _parent_scene.avPIDP; 790 d.SpaceSetSublevel(collider, 3);
791 d.SpaceSetCleanup(collider, false);
792 d.GeomSetCategoryBits(collider, (uint)m_collisionCategories);
793 d.GeomSetCollideBits(collider, (uint)m_collisionFlags);
660 794
661 // rescale PID parameters so that this aren't affected by mass 795 feetbox = d.CreateBox(collider, feetsx, feetsy, feetsz);
662 // and so don't get unstable for some masses 796 midbox = d.CreateBox(collider, midsx, midsy, midsz);
663 // also scale by ode time step so you don't need to refix them 797 topbox = d.CreateBox(collider, topsx, topsy, topsz);
798 bbox = d.CreateBox(collider, m_size.X, m_size.Y, m_size.Z);
664 799
665 PID_D /= 50 * 80; //scale to original mass of around 80 and 50 ODE fps 800 m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass
666 PID_D *= m_mass / _parent_scene.ODE_STEPSIZE; 801
667 PID_P /= 50 * 80; 802 d.MassSetBoxTotal(out ShellMass, m_mass, m_size.X, m_size.Y, m_size.Z);
668 PID_P *= m_mass / _parent_scene.ODE_STEPSIZE; 803
804 PID_D = basePID_D * m_mass / _parent_scene.ODE_STEPSIZE;
805 PID_P = basePID_P * m_mass / _parent_scene.ODE_STEPSIZE;
669 806
670 Body = d.BodyCreate(_parent_scene.world); 807 Body = d.BodyCreate(_parent_scene.world);
671 808
672 _zeroFlag = false; 809 _zeroFlag = false;
810 m_collisionException = false;
673 m_pidControllerActive = true; 811 m_pidControllerActive = true;
674 m_freemove = false; 812 m_freemove = false;
675 813
814 _velocity = Vector3.Zero;
815 m_lastVelocitySqr = 0;
816
676 d.BodySetAutoDisableFlag(Body, false); 817 d.BodySetAutoDisableFlag(Body, false);
677 d.BodySetPosition(Body, npositionX, npositionY, npositionZ); 818 d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
678 819
@@ -681,7 +822,17 @@ namespace OpenSim.Region.Physics.OdePlugin
681 _position.Z = npositionZ; 822 _position.Z = npositionZ;
682 823
683 d.BodySetMass(Body, ref ShellMass); 824 d.BodySetMass(Body, ref ShellMass);
684 d.GeomSetBody(Shell, Body); 825 d.GeomSetBody(feetbox, Body);
826 d.GeomSetBody(midbox, Body);
827 d.GeomSetBody(topbox, Body);
828 d.GeomSetBody(bbox, Body);
829
830 d.GeomSetOffsetPosition(feetbox, 0, 0, feetz);
831 d.GeomSetOffsetPosition(midbox, 0, 0, midz);
832 d.GeomSetOffsetPosition(topbox, 0, 0, topz);
833
834 ajustCollider();
835
685 836
686 // The purpose of the AMotor here is to keep the avatar's physical 837 // The purpose of the AMotor here is to keep the avatar's physical
687 // surrogate from rotating while moving 838 // surrogate from rotating while moving
@@ -741,15 +892,222 @@ namespace OpenSim.Region.Physics.OdePlugin
741 Body = IntPtr.Zero; 892 Body = IntPtr.Zero;
742 } 893 }
743 894
744 //kill the Geometry 895 //kill the Geoms
745 if (Shell != IntPtr.Zero) 896 if (topbox != IntPtr.Zero)
746 { 897 {
747// _parent_scene.geom_name_map.Remove(Shell); 898 _parent_scene.actor_name_map.Remove(topbox);
748 _parent_scene.actor_name_map.Remove(Shell); 899 _parent_scene.waitForSpaceUnlock(collider);
749 _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); 900 d.GeomDestroy(topbox);
750 d.GeomDestroy(Shell); 901 topbox = IntPtr.Zero;
751 Shell = IntPtr.Zero;
752 } 902 }
903 if (midbox != IntPtr.Zero)
904 {
905 _parent_scene.actor_name_map.Remove(midbox);
906 _parent_scene.waitForSpaceUnlock(collider);
907 d.GeomDestroy(midbox);
908 midbox = IntPtr.Zero;
909 }
910 if (feetbox != IntPtr.Zero)
911 {
912 _parent_scene.actor_name_map.Remove(feetbox);
913 _parent_scene.waitForSpaceUnlock(collider);
914 d.GeomDestroy(feetbox);
915 feetbox = IntPtr.Zero;
916 }
917
918 if (bbox != IntPtr.Zero)
919 {
920 _parent_scene.actor_name_map.Remove(bbox);
921 _parent_scene.waitForSpaceUnlock(collider);
922 d.GeomDestroy(bbox);
923 bbox = IntPtr.Zero;
924 }
925
926 if (collider != IntPtr.Zero)
927 {
928 d.SpaceDestroy(collider);
929 collider = IntPtr.Zero;
930 }
931
932 }
933
934 //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z
935 public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot)
936 {
937 float sin = 2.0f * rot.Z * rot.W;
938 float cos = rot.W * rot.W - rot.Z * rot.Z;
939 float tx = x;
940
941 x = tx * cos - y * sin;
942 y = tx * sin + y * cos;
943 }
944 public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
945 {
946 float tx = x;
947 x = tx * cos - y * sin;
948 y = tx * sin + y * cos;
949 }
950 public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
951 {
952 float tx = x;
953 x = tx * cos + y * sin;
954 y = -tx * sin + y * cos;
955 }
956
957 public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot)
958 {
959 float sin = - 2.0f * rot.Z * rot.W;
960 float cos = rot.W * rot.W - rot.Z * rot.Z;
961 float tx = x;
962
963 x = tx * cos - y * sin;
964 y = tx * sin + y * cos;
965 }
966
967
968 public bool Collide(IntPtr me, bool reverse, ref d.ContactGeom contact, ref bool feetcollision)
969 {
970 feetcollision = false;
971 if (m_collisionException)
972 return false;
973
974 Vector3 offset;
975
976 if (me == bbox) // if moving fast
977 {
978 // force a full inelastic collision
979 m_collisionException = true;
980
981 offset = m_size * m_orientation2D;
982
983 offset.X = (float)Math.Abs(offset.X) * 0.5f + contact.depth;
984 offset.Y = (float)Math.Abs(offset.Y) * 0.5f + contact.depth;
985 offset.Z = (float)Math.Abs(offset.Z) * 0.5f + contact.depth;
986
987 if (reverse)
988 {
989 offset.X *= -contact.normal.X;
990 offset.Y *= -contact.normal.Y;
991 offset.Z *= -contact.normal.Z;
992 }
993 else
994 {
995 offset.X *= contact.normal.X;
996 offset.Y *= contact.normal.Y;
997 offset.Z *= contact.normal.Z;
998 }
999
1000 offset.X += contact.pos.X;
1001 offset.Y += contact.pos.Y;
1002 offset.Z += contact.pos.Z;
1003
1004 _position = offset;
1005 return false;
1006 }
1007
1008 offset.X = contact.pos.X - _position.X;
1009 offset.Y = contact.pos.Y - _position.Y;
1010
1011 if (me == topbox)
1012 {
1013 offset.Z = contact.pos.Z - _position.Z;
1014
1015 offset.Normalize();
1016
1017 if (reverse)
1018 {
1019 contact.normal.X = offset.X;
1020 contact.normal.Y = offset.Y;
1021 contact.normal.Z = offset.Z;
1022 }
1023 else
1024 {
1025 contact.normal.X = -offset.X;
1026 contact.normal.Y = -offset.Y;
1027 contact.normal.Z = -offset.Z;
1028 }
1029 return true;
1030 }
1031
1032 if (me == midbox)
1033 {
1034 if (Math.Abs(contact.normal.Z) > 0.95f)
1035 offset.Z = contact.pos.Z - _position.Z;
1036 else
1037 offset.Z = contact.normal.Z;
1038
1039 offset.Normalize();
1040
1041 if (reverse)
1042 {
1043 contact.normal.X = offset.X;
1044 contact.normal.Y = offset.Y;
1045 contact.normal.Z = offset.Z;
1046 }
1047 else
1048 {
1049 contact.normal.X = -offset.X;
1050 contact.normal.Y = -offset.Y;
1051 contact.normal.Z = -offset.Z;
1052 }
1053
1054 return true;
1055 }
1056
1057 else if (me == feetbox)
1058 {
1059 float h = contact.pos.Z - _position.Z;
1060
1061 if (Math.Abs(contact.normal.Z) > 0.95f)
1062 {
1063 if (contact.normal.Z > 0)
1064 contact.normal.Z = 1.0f;
1065 else
1066 contact.normal.Z = -1.0f;
1067 contact.normal.X = 0.0f;
1068 contact.normal.Y = 0.0f;
1069 feetcollision = true;
1070 if (h < boneOff)
1071 IsColliding = true;
1072 return true;
1073 }
1074
1075 offset.Z = h - feetOff; // distance from top of feetbox
1076
1077 if (offset.Z > 0)
1078 return false;
1079
1080 if (offset.Z > -0.01)
1081 {
1082 offset.X = 0;
1083 offset.Y = 0;
1084 offset.Z = -1.0f;
1085 }
1086 else
1087 {
1088 offset.Normalize();
1089 }
1090
1091 if (reverse)
1092 {
1093 contact.normal.X = offset.X;
1094 contact.normal.Y = offset.Y;
1095 contact.normal.Z = offset.Z;
1096 }
1097 else
1098 {
1099 contact.normal.X = -offset.X;
1100 contact.normal.Y = -offset.Y;
1101 contact.normal.Z = -offset.Z;
1102 }
1103 feetcollision = true;
1104 if (h < boneOff)
1105 IsColliding = true;
1106 }
1107 else
1108 return false;
1109
1110 return true;
753 } 1111 }
754 1112
755 /// <summary> 1113 /// <summary>
@@ -757,11 +1115,28 @@ namespace OpenSim.Region.Physics.OdePlugin
757 /// This is the avatar's movement control + PID Controller 1115 /// This is the avatar's movement control + PID Controller
758 /// </summary> 1116 /// </summary>
759 /// <param name="timeStep"></param> 1117 /// <param name="timeStep"></param>
760 public void Move(float timeStep, List<OdeCharacter> defects) 1118 public void Move(List<OdeCharacter> defects)
761 { 1119 {
762 if (Body == IntPtr.Zero) 1120 if (Body == IntPtr.Zero)
763 return; 1121 return;
764 1122
1123 if (m_collisionException)
1124 {
1125 d.BodySetPosition(Body,_position.X, _position.Y, _position.Z);
1126 d.BodySetLinearVel(Body, 0, 0, 0);
1127
1128 float v = _velocity.Length();
1129 if (v != 0)
1130 {
1131 v = 5.0f / v;
1132 _velocity = _velocity * v;
1133 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
1134 }
1135 ajustCollider();
1136 m_collisionException = false;
1137 return;
1138 }
1139
765 d.Vector3 dtmp = d.BodyGetPosition(Body); 1140 d.Vector3 dtmp = d.BodyGetPosition(Body);
766 Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); 1141 Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
767 1142
@@ -769,10 +1144,10 @@ namespace OpenSim.Region.Physics.OdePlugin
769 // so force it back to identity 1144 // so force it back to identity
770 1145
771 d.Quaternion qtmp; 1146 d.Quaternion qtmp;
772 qtmp.W = 1; 1147 qtmp.W = m_orientation2D.W;
773 qtmp.X = 0; 1148 qtmp.X = m_orientation2D.X;
774 qtmp.Y = 0; 1149 qtmp.Y = m_orientation2D.Y;
775 qtmp.Z = 0; 1150 qtmp.Z = m_orientation2D.Z;
776 d.BodySetQuaternion(Body, ref qtmp); 1151 d.BodySetQuaternion(Body, ref qtmp);
777 1152
778 if (m_pidControllerActive == false) 1153 if (m_pidControllerActive == false)
@@ -835,10 +1210,10 @@ namespace OpenSim.Region.Physics.OdePlugin
835 1210
836 //****************************************** 1211 //******************************************
837 // colide with land 1212 // colide with land
838 d.AABB aabb;
839 d.GeomGetAABB(Shell, out aabb);
840 float chrminZ = aabb.MinZ;
841 1213
1214 d.AABB aabb;
1215 d.GeomGetAABB(feetbox, out aabb);
1216 float chrminZ = aabb.MinZ; ; // move up a bit
842 Vector3 posch = localpos; 1217 Vector3 posch = localpos;
843 1218
844 float ftmp; 1219 float ftmp;
@@ -881,11 +1256,12 @@ namespace OpenSim.Region.Physics.OdePlugin
881 contact.PenetrationDepth = depth; 1256 contact.PenetrationDepth = depth;
882 contact.Position.X = localpos.X; 1257 contact.Position.X = localpos.X;
883 contact.Position.Y = localpos.Y; 1258 contact.Position.Y = localpos.Y;
884 contact.Position.Z = chrminZ; 1259 contact.Position.Z = terrainheight;
885 contact.SurfaceNormal.X = 0f; 1260 contact.SurfaceNormal.X = 0.0f;
886 contact.SurfaceNormal.Y = 0f; 1261 contact.SurfaceNormal.Y = 0.0f;
887 contact.SurfaceNormal.Z = -1f; 1262 contact.SurfaceNormal.Z = -1f;
888 contact.RelativeSpeed = -vel.Z; 1263 contact.RelativeSpeed = -vel.Z;
1264 contact.CharacterFeet = true;
889 AddCollisionEvent(0, contact); 1265 AddCollisionEvent(0, contact);
890 1266
891 vec.Z *= 0.5f; 1267 vec.Z *= 0.5f;
@@ -904,6 +1280,7 @@ namespace OpenSim.Region.Physics.OdePlugin
904 m_iscollidingGround = false; 1280 m_iscollidingGround = false;
905 } 1281 }
906 1282
1283
907 //****************************************** 1284 //******************************************
908 1285
909 bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f); 1286 bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f);
@@ -1039,21 +1416,58 @@ namespace OpenSim.Region.Physics.OdePlugin
1039 } 1416 }
1040 1417
1041 // update our local ideia of position velocity and aceleration 1418 // update our local ideia of position velocity and aceleration
1419 // _position = localpos;
1042 _position = localpos; 1420 _position = localpos;
1421
1043 if (_zeroFlag) 1422 if (_zeroFlag)
1044 { 1423 {
1045 _velocity = Vector3.Zero; 1424 _velocity = Vector3.Zero;
1046 _acceleration = Vector3.Zero; 1425 _acceleration = Vector3.Zero;
1426 m_rotationalVelocity = Vector3.Zero;
1047 } 1427 }
1048 else 1428 else
1049 { 1429 {
1050 _acceleration = _velocity; // previus velocity 1430 Vector3 a =_velocity; // previus velocity
1051 _velocity = vel; 1431 SetSmooth(ref _velocity, ref vel, 2);
1052 _acceleration = (vel - _acceleration) / timeStep; 1432 a = (_velocity - a) * invtimeStep;
1433 SetSmooth(ref _acceleration, ref a, 2);
1434
1435 dtmp = d.BodyGetAngularVel(Body);
1436 m_rotationalVelocity.X = 0f;
1437 m_rotationalVelocity.Y = 0f;
1438 m_rotationalVelocity.Z = dtmp.Z;
1439 Math.Round(m_rotationalVelocity.Z,3);
1053 } 1440 }
1054 1441 ajustCollider();
1442 }
1443
1444 public void round(ref Vector3 v, int digits)
1445 {
1446 v.X = (float)Math.Round(v.X, digits);
1447 v.Y = (float)Math.Round(v.Y, digits);
1448 v.Z = (float)Math.Round(v.Z, digits);
1449 }
1450
1451 public void SetSmooth(ref Vector3 dst, ref Vector3 value)
1452 {
1453 dst.X = 0.1f * dst.X + 0.9f * value.X;
1454 dst.Y = 0.1f * dst.Y + 0.9f * value.Y;
1455 dst.Z = 0.1f * dst.Z + 0.9f * value.Z;
1055 } 1456 }
1056 1457
1458 public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits)
1459 {
1460 dst.X = 0.4f * dst.X + 0.6f * value.X;
1461 dst.X = (float)Math.Round(dst.X, rounddigits);
1462
1463 dst.Y = 0.4f * dst.Y + 0.6f * value.Y;
1464 dst.Y = (float)Math.Round(dst.Y, rounddigits);
1465
1466 dst.Z = 0.4f * dst.Z + 0.6f * value.Z;
1467 dst.Z = (float)Math.Round(dst.Z, rounddigits);
1468 }
1469
1470
1057 /// <summary> 1471 /// <summary>
1058 /// Updates the reported position and velocity. 1472 /// Updates the reported position and velocity.
1059 /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording 1473 /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording
@@ -1176,20 +1590,15 @@ namespace OpenSim.Region.Physics.OdePlugin
1176 { 1590 {
1177 if (NewStatus) 1591 if (NewStatus)
1178 { 1592 {
1179 // Create avatar capsule and related ODE data 1593 AvatarGeomAndBodyDestroy();
1180 if ((Shell != IntPtr.Zero))
1181 {
1182 // a lost shell ?
1183 m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
1184 + (Shell != IntPtr.Zero ? "Shell " : "")
1185 + (Body != IntPtr.Zero ? "Body " : "")
1186 + (Amotor != IntPtr.Zero ? "Amotor " : ""));
1187 AvatarGeomAndBodyDestroy();
1188 }
1189 1594
1190 AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); 1595 AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
1191 1596
1192 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; 1597 _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
1598 _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this;
1599 _parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
1600 _parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
1601 _parent_scene.actor_name_map[bbox] = (PhysicsActor)this;
1193 _parent_scene.AddCharacter(this); 1602 _parent_scene.AddCharacter(this);
1194 } 1603 }
1195 else 1604 else
@@ -1218,39 +1627,40 @@ namespace OpenSim.Region.Physics.OdePlugin
1218 { 1627 {
1219 } 1628 }
1220 1629
1221 private void changeSize(Vector3 Size) 1630 private void changeAvatarSize(strAvatarSize st)
1222 { 1631 {
1223 if (Size.IsFinite()) 1632 m_feetOffset = st.offset;
1633 changeSize(st.size);
1634 }
1635
1636 private void changeSize(Vector3 pSize)
1637 {
1638 if (pSize.IsFinite())
1224 { 1639 {
1225 float caplen = Size.Z; 1640 // for now only look to Z changes since viewers also don't change X and Y
1641 if (pSize.Z != m_size.Z)
1642 {
1643 AvatarGeomAndBodyDestroy();
1226 1644
1227 caplen = caplen * 1.15f - CAPSULE_RADIUS * 2.0f;
1228 1645
1229 if (caplen != CAPSULE_LENGTH) 1646 float oldsz = m_size.Z;
1230 { 1647 m_size = pSize;
1231 if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
1232 {
1233 AvatarGeomAndBodyDestroy();
1234 1648
1235 float prevCapsule = CAPSULE_LENGTH;
1236 CAPSULE_LENGTH = caplen;
1237 1649
1238 AvatarGeomAndBodyCreation(_position.X, _position.Y, 1650 AvatarGeomAndBodyCreation(_position.X, _position.Y,
1239 _position.Z + (CAPSULE_LENGTH - prevCapsule) * 0.5f); 1651 _position.Z + (m_size.Z - oldsz) * 0.5f);
1240 1652
1241 Velocity = Vector3.Zero; 1653 Velocity = Vector3.Zero;
1654
1242 1655
1243 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; 1656 _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
1244 } 1657 _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this;
1245 else 1658 _parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
1246 { 1659 _parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
1247 m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " 1660 _parent_scene.actor_name_map[bbox] = (PhysicsActor)this;
1248 + (Shell == IntPtr.Zero ? "Shell " : "")
1249 + (Body == IntPtr.Zero ? "Body " : "")
1250 + (Amotor == IntPtr.Zero ? "Amotor " : ""));
1251 }
1252 } 1661 }
1253 m_freemove = false; 1662 m_freemove = false;
1663 m_collisionException = false;
1254 m_pidControllerActive = true; 1664 m_pidControllerActive = true;
1255 } 1665 }
1256 else 1666 else
@@ -1270,6 +1680,36 @@ namespace OpenSim.Region.Physics.OdePlugin
1270 1680
1271 private void changeOrientation(Quaternion newOri) 1681 private void changeOrientation(Quaternion newOri)
1272 { 1682 {
1683 if (m_orientation != newOri)
1684 {
1685 m_orientation = newOri; // keep a copy for core use
1686 // but only use rotations around Z
1687
1688 m_orientation2D.W = newOri.W;
1689 m_orientation2D.Z = newOri.Z;
1690
1691 float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z;
1692 if (t > 0)
1693 {
1694 t = 1.0f / (float)Math.Sqrt(t);
1695 m_orientation2D.W *= t;
1696 m_orientation2D.Z *= t;
1697 }
1698 else
1699 {
1700 m_orientation2D.W = 1.0f;
1701 m_orientation2D.Z = 0f;
1702 }
1703 m_orientation2D.Y = 0f;
1704 m_orientation2D.X = 0f;
1705
1706 d.Quaternion myrot = new d.Quaternion();
1707 myrot.X = m_orientation2D.X;
1708 myrot.Y = m_orientation2D.Y;
1709 myrot.Z = m_orientation2D.Z;
1710 myrot.W = m_orientation2D.W;
1711 d.BodySetQuaternion(Body, ref myrot);
1712 }
1273 } 1713 }
1274 1714
1275 private void changeVelocity(Vector3 newVel) 1715 private void changeVelocity(Vector3 newVel)
@@ -1351,6 +1791,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1351 1791
1352 if (Body != IntPtr.Zero) 1792 if (Body != IntPtr.Zero)
1353 d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z); 1793 d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z);
1794 ajustCollider();
1354 } 1795 }
1355 1796
1356 private void donullchange() 1797 private void donullchange()
@@ -1359,7 +1800,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1359 1800
1360 public bool DoAChange(changes what, object arg) 1801 public bool DoAChange(changes what, object arg)
1361 { 1802 {
1362 if (Shell == IntPtr.Zero && what != changes.Add && what != changes.Remove) 1803 if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove)
1363 { 1804 {
1364 return false; 1805 return false;
1365 } 1806 }
@@ -1425,6 +1866,10 @@ namespace OpenSim.Region.Physics.OdePlugin
1425 changeSize((Vector3)arg); 1866 changeSize((Vector3)arg);
1426 break; 1867 break;
1427 1868
1869 case changes.AvatarSize:
1870 changeAvatarSize((strAvatarSize)arg);
1871 break;
1872
1428 case changes.Momentum: 1873 case changes.Momentum:
1429 changeMomentum((Vector3)arg); 1874 changeMomentum((Vector3)arg);
1430 break; 1875 break;
@@ -1472,5 +1917,12 @@ namespace OpenSim.Region.Physics.OdePlugin
1472 { 1917 {
1473 _parent_scene.AddChange((PhysicsActor)this, what, arg); 1918 _parent_scene.AddChange((PhysicsActor)this, what, arg);
1474 } 1919 }
1920
1921 private struct strAvatarSize
1922 {
1923 public Vector3 size;
1924 public float offset;
1925 }
1926
1475 } 1927 }
1476} 1928}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
index dc247a9..faa9488 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -84,7 +84,7 @@ namespace OpenSim.Region.Physics.OdePlugin
84 84
85 private Vector3 _position; 85 private Vector3 _position;
86 private Vector3 _velocity; 86 private Vector3 _velocity;
87 private Vector3 _torque; 87 private Vector3 m_torque;
88 private Vector3 m_lastVelocity; 88 private Vector3 m_lastVelocity;
89 private Vector3 m_lastposition; 89 private Vector3 m_lastposition;
90 private Vector3 m_rotationalVelocity; 90 private Vector3 m_rotationalVelocity;
@@ -597,7 +597,7 @@ namespace OpenSim.Region.Physics.OdePlugin
597 if (!IsPhysical || Body == IntPtr.Zero) 597 if (!IsPhysical || Body == IntPtr.Zero)
598 return Vector3.Zero; 598 return Vector3.Zero;
599 599
600 return _torque; 600 return m_torque;
601 } 601 }
602 602
603 set 603 set
@@ -2425,10 +2425,10 @@ namespace OpenSim.Region.Physics.OdePlugin
2425 { 2425 {
2426 if (!childPrim) 2426 if (!childPrim)
2427 { 2427 {
2428 m_force = Vector3.Zero; 2428// m_force = Vector3.Zero;
2429 m_forceacc = Vector3.Zero; 2429 m_forceacc = Vector3.Zero;
2430 m_angularForceacc = Vector3.Zero; 2430 m_angularForceacc = Vector3.Zero;
2431 _torque = Vector3.Zero; 2431// m_torque = Vector3.Zero;
2432 _velocity = Vector3.Zero; 2432 _velocity = Vector3.Zero;
2433 _acceleration = Vector3.Zero; 2433 _acceleration = Vector3.Zero;
2434 m_rotationalVelocity = Vector3.Zero; 2434 m_rotationalVelocity = Vector3.Zero;
@@ -2521,7 +2521,7 @@ namespace OpenSim.Region.Physics.OdePlugin
2521 prm.m_collisionCategories = m_collisionCategories; 2521 prm.m_collisionCategories = m_collisionCategories;
2522 prm.m_collisionFlags = m_collisionFlags; 2522 prm.m_collisionFlags = m_collisionFlags;
2523 2523
2524 if (prm.prim_geom != null) 2524 if (prm.prim_geom != IntPtr.Zero)
2525 { 2525 {
2526 2526
2527 if (prm.m_NoColide) 2527 if (prm.m_NoColide)
@@ -2542,7 +2542,7 @@ namespace OpenSim.Region.Physics.OdePlugin
2542// ((OdePrim)_parent).ChildSelectedChange(true); 2542// ((OdePrim)_parent).ChildSelectedChange(true);
2543 2543
2544 2544
2545 if (prim_geom != null) 2545 if (prim_geom != IntPtr.Zero)
2546 { 2546 {
2547 if (m_NoColide) 2547 if (m_NoColide)
2548 { 2548 {
@@ -2968,7 +2968,7 @@ namespace OpenSim.Region.Physics.OdePlugin
2968 d.BodyEnable(Body); 2968 d.BodyEnable(Body);
2969 2969
2970 } 2970 }
2971 _torque = newtorque; 2971 m_torque = newtorque;
2972 } 2972 }
2973 } 2973 }
2974 2974
@@ -3364,7 +3364,7 @@ namespace OpenSim.Region.Physics.OdePlugin
3364 3364
3365 Vector3 trq; 3365 Vector3 trq;
3366 3366
3367 trq = _torque; 3367 trq = m_torque;
3368 trq += m_angularForceacc; 3368 trq += m_angularForceacc;
3369 m_angularForceacc = Vector3.Zero; 3369 m_angularForceacc = Vector3.Zero;
3370 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) 3370 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
index 799a324..561ab1c 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
@@ -324,7 +324,10 @@ namespace OpenSim.Region.Physics.OdePlugin
324 { 324 {
325 // Collide tests 325 // Collide tests
326 if ((CurrentRayFilter & FilterActiveSpace) != 0) 326 if ((CurrentRayFilter & FilterActiveSpace) != 0)
327 {
327 d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); 328 d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
329 d.SpaceCollide2(ray, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
330 }
328 if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) 331 if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
329 d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback); 332 d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
330 if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) 333 if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
@@ -552,7 +555,6 @@ namespace OpenSim.Region.Physics.OdePlugin
552 break; 555 break;
553 556
554 default: 557 default:
555 return;
556 break; 558 break;
557 } 559 }
558 } 560 }
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs
new file mode 100644
index 0000000..225bff8
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs
@@ -0,0 +1,183 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27// Ubit 2012
28using System;
29using System.Collections.Generic;
30using System.Reflection;
31using System.Runtime.InteropServices;
32using System.Text;
33using OpenSim.Framework;
34using OpenSim.Region.Physics.Manager;
35using OdeAPI;
36using log4net;
37using OpenMetaverse;
38
39namespace OpenSim.Region.Physics.OdePlugin
40{
41 /// <summary>
42 /// </summary>
43 public class ODESitAvatar
44 {
45 private OdeScene m_scene;
46 private ODERayCastRequestManager m_raymanager;
47
48 public ODESitAvatar(OdeScene pScene, ODERayCastRequestManager raymanager)
49 {
50 m_scene = pScene;
51 m_raymanager = raymanager;
52 }
53
54 private static Vector3 SitAjust = new Vector3(0, 0, 0.4f);
55
56 public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse)
57 {
58 if (!m_scene.haveActor(actor) || !(actor is OdePrim) || ((OdePrim)actor).prim_geom == IntPtr.Zero)
59 {
60 PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity);
61 return;
62 }
63
64 IntPtr geom = ((OdePrim)actor).prim_geom;
65
66 d.Vector3 dtmp = d.GeomGetPosition(geom);
67 Vector3 geopos;
68 geopos.X = dtmp.X;
69 geopos.Y = dtmp.Y;
70 geopos.Z = dtmp.Z;
71
72
73 d.AABB aabb;
74 Quaternion ori;
75 d.Quaternion qtmp;
76 d.GeomCopyQuaternion(geom, out qtmp);
77 Quaternion geomOri;
78 geomOri.X = qtmp.X;
79 geomOri.Y = qtmp.Y;
80 geomOri.Z = qtmp.Z;
81 geomOri.W = qtmp.W;
82 Quaternion geomInvOri;
83 geomInvOri.X = -qtmp.X;
84 geomInvOri.Y = -qtmp.Y;
85 geomInvOri.Z = -qtmp.Z;
86 geomInvOri.W = qtmp.W;
87
88 Vector3 target = geopos + offset;
89 Vector3 rayDir = target - avCameraPosition;
90 float raylen = rayDir.Length();
91 float t = 1 / raylen;
92 rayDir.X *= t;
93 rayDir.Y *= t;
94 rayDir.Z *= t;
95
96 raylen += 0.5f;
97 List<ContactResult> rayResults;
98
99 rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir , raylen, 1);
100 if (rayResults.Count == 0 || rayResults[0].ConsumerID != actor.LocalID)
101 {
102 d.GeomGetAABB(geom,out aabb);
103 offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z);
104 ori = geomInvOri;
105 offset *= geomInvOri;
106
107 PhysicsSitResponse(1, actor.LocalID, offset, ori);
108 return;
109 }
110
111 offset = rayResults[0].Pos - geopos;
112 double ang;
113 float s;
114 float c;
115
116 d.GeomClassID geoclass = d.GeomGetClass(geom);
117
118 if (geoclass == d.GeomClassID.SphereClass)
119 {
120 float r = d.GeomSphereGetRadius(geom);
121
122 offset.Normalize();
123 offset *= r;
124
125 ang = Math.Atan2(offset.Y, offset.X);
126 ang *= 0.5d;
127 s = (float)Math.Sin(ang);
128 c = (float)Math.Cos(ang);
129
130 ori = new Quaternion(0, 0, s, c);
131
132 if (r < 0.4f)
133 {
134 offset = new Vector3(0, 0, r);
135 }
136 else if (offset.Z < 0.4f)
137 {
138 t = offset.Z;
139 float rsq = r * r;
140
141 t = 1.0f / (rsq - t * t);
142 offset.X *= t;
143 offset.Y *= t;
144 offset.Z = 0.4f;
145 t = rsq - 0.16f;
146 offset.X *= t;
147 offset.Y *= t;
148 }
149
150 offset += avOffset * ori;
151
152 ori = geomInvOri * ori;
153 offset *= geomInvOri;
154
155 PhysicsSitResponse(1, actor.LocalID, offset, ori);
156 return;
157 }
158
159 Vector3 norm = rayResults[0].Normal;
160
161 if (norm.Z < 0)
162 {
163 PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity);
164 return;
165 }
166
167 ang = Math.Atan2(-rayDir.Y, -rayDir.X);
168 ang *= 0.5d;
169 s = (float)Math.Sin(ang);
170 c = (float)Math.Cos(ang);
171
172 ori = new Quaternion(0, 0, s, c);
173
174 offset += avOffset * ori;
175
176 ori = geomInvOri * ori;
177 offset *= geomInvOri;
178
179 PhysicsSitResponse(1, actor.LocalID, offset, ori);
180 return;
181 }
182 }
183} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
index 54bc29f..fbf2f0d 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -147,6 +147,7 @@ namespace OpenSim.Region.Physics.OdePlugin
147 PIDHoverActive, 147 PIDHoverActive,
148 148
149 Size, 149 Size,
150 AvatarSize,
150 Shape, 151 Shape,
151 PhysRepData, 152 PhysRepData,
152 AddPhysRep, 153 AddPhysRep,
@@ -177,7 +178,9 @@ namespace OpenSim.Region.Physics.OdePlugin
177 public changes what; 178 public changes what;
178 public Object arg; 179 public Object arg;
179 } 180 }
180 181
182
183
181 public class OdeScene : PhysicsScene 184 public class OdeScene : PhysicsScene
182 { 185 {
183 private readonly ILog m_log; 186 private readonly ILog m_log;
@@ -224,9 +227,6 @@ namespace OpenSim.Region.Physics.OdePlugin
224// private IntPtr WaterHeightmapData = IntPtr.Zero; 227// private IntPtr WaterHeightmapData = IntPtr.Zero;
225// private GCHandle WaterMapHandler = new GCHandle(); 228// private GCHandle WaterMapHandler = new GCHandle();
226 229
227 public float avPIDD = 2200f; // make it visible
228 public float avPIDP = 900f; // make it visible
229 private float avCapRadius = 0.37f;
230 private float avDensity = 3f; 230 private float avDensity = 3f;
231 private float avMovementDivisorWalk = 1.3f; 231 private float avMovementDivisorWalk = 1.3f;
232 private float avMovementDivisorRun = 0.8f; 232 private float avMovementDivisorRun = 0.8f;
@@ -299,9 +299,12 @@ namespace OpenSim.Region.Physics.OdePlugin
299 299
300 public IntPtr TopSpace; // the global space 300 public IntPtr TopSpace; // the global space
301 public IntPtr ActiveSpace; // space for active prims 301 public IntPtr ActiveSpace; // space for active prims
302 public IntPtr CharsSpace; // space for active prims
302 public IntPtr StaticSpace; // space for the static things around 303 public IntPtr StaticSpace; // space for the static things around
303 public IntPtr GroundSpace; // space for ground 304 public IntPtr GroundSpace; // space for ground
304 305
306 public IntPtr SharedRay;
307
305 // some speedup variables 308 // some speedup variables
306 private int spaceGridMaxX; 309 private int spaceGridMaxX;
307 private int spaceGridMaxY; 310 private int spaceGridMaxY;
@@ -359,8 +362,7 @@ namespace OpenSim.Region.Physics.OdePlugin
359 362
360 nearCallback = near; 363 nearCallback = near;
361 364
362 m_rayCastManager = new ODERayCastRequestManager(this); 365 m_rayCastManager = new ODERayCastRequestManager(this);
363
364 366
365 lock (OdeLock) 367 lock (OdeLock)
366 { 368 {
@@ -372,21 +374,25 @@ namespace OpenSim.Region.Physics.OdePlugin
372 374
373 // now the major subspaces 375 // now the major subspaces
374 ActiveSpace = d.HashSpaceCreate(TopSpace); 376 ActiveSpace = d.HashSpaceCreate(TopSpace);
377 CharsSpace = d.HashSpaceCreate(TopSpace);
375 StaticSpace = d.HashSpaceCreate(TopSpace); 378 StaticSpace = d.HashSpaceCreate(TopSpace);
376 GroundSpace = d.HashSpaceCreate(TopSpace); 379 GroundSpace = d.HashSpaceCreate(TopSpace);
377 } 380 }
378 catch 381 catch
379 { 382 {
380 // i must RtC#FM 383 // i must RtC#FM
384 // i did!
381 } 385 }
382 386
383 d.HashSpaceSetLevels(TopSpace, -2, 8); 387 d.HashSpaceSetLevels(TopSpace, -2, 8);
384 d.HashSpaceSetLevels(ActiveSpace, -2, 8); 388 d.HashSpaceSetLevels(ActiveSpace, -2, 8);
389 d.HashSpaceSetLevels(CharsSpace, -4, 3);
385 d.HashSpaceSetLevels(StaticSpace, -2, 8); 390 d.HashSpaceSetLevels(StaticSpace, -2, 8);
386 d.HashSpaceSetLevels(GroundSpace, 0, 8); 391 d.HashSpaceSetLevels(GroundSpace, 0, 8);
387 392
388 // demote to second level 393 // demote to second level
389 d.SpaceSetSublevel(ActiveSpace, 1); 394 d.SpaceSetSublevel(ActiveSpace, 1);
395 d.SpaceSetSublevel(CharsSpace, 1);
390 d.SpaceSetSublevel(StaticSpace, 1); 396 d.SpaceSetSublevel(StaticSpace, 1);
391 d.SpaceSetSublevel(GroundSpace, 1); 397 d.SpaceSetSublevel(GroundSpace, 1);
392 398
@@ -396,11 +402,24 @@ namespace OpenSim.Region.Physics.OdePlugin
396 CollisionCategories.Phantom | 402 CollisionCategories.Phantom |
397 CollisionCategories.VolumeDtc 403 CollisionCategories.VolumeDtc
398 )); 404 ));
399 d.GeomSetCollideBits(ActiveSpace, 0); 405 d.GeomSetCollideBits(ActiveSpace, (uint)(CollisionCategories.Space |
406 CollisionCategories.Geom |
407 CollisionCategories.Character |
408 CollisionCategories.Phantom |
409 CollisionCategories.VolumeDtc
410 ));
411 d.GeomSetCategoryBits(CharsSpace, (uint)(CollisionCategories.Space |
412 CollisionCategories.Geom |
413 CollisionCategories.Character |
414 CollisionCategories.Phantom |
415 CollisionCategories.VolumeDtc
416 ));
417 d.GeomSetCollideBits(CharsSpace, 0);
418
400 d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space | 419 d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space |
401 CollisionCategories.Geom | 420 CollisionCategories.Geom |
402 CollisionCategories.Land | 421// CollisionCategories.Land |
403 CollisionCategories.Water | 422// CollisionCategories.Water |
404 CollisionCategories.Phantom | 423 CollisionCategories.Phantom |
405 CollisionCategories.VolumeDtc 424 CollisionCategories.VolumeDtc
406 )); 425 ));
@@ -412,6 +431,8 @@ namespace OpenSim.Region.Physics.OdePlugin
412 contactgroup = d.JointGroupCreate(0); 431 contactgroup = d.JointGroupCreate(0);
413 //contactgroup 432 //contactgroup
414 433
434 SharedRay = d.CreateRay(TopSpace, 1.0f);
435
415 d.WorldSetAutoDisableFlag(world, false); 436 d.WorldSetAutoDisableFlag(world, false);
416 } 437 }
417 } 438 }
@@ -468,7 +489,6 @@ namespace OpenSim.Region.Physics.OdePlugin
468 avDensity = physicsconfig.GetFloat("av_density", avDensity); 489 avDensity = physicsconfig.GetFloat("av_density", avDensity);
469 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk); 490 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk);
470 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun); 491 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun);
471 avCapRadius = physicsconfig.GetFloat("av_capsule_radius", avCapRadius);
472 492
473 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision); 493 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision);
474 494
@@ -494,7 +514,7 @@ namespace OpenSim.Region.Physics.OdePlugin
494 odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f); 514 odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f);
495 515
496 ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); 516 ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
497 GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); 517 GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf);
498 518
499 m_materialContactsData[(int)Material.Stone].mu = 0.8f; 519 m_materialContactsData[(int)Material.Stone].mu = 0.8f;
500 m_materialContactsData[(int)Material.Stone].bounce = 0.4f; 520 m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
@@ -718,35 +738,35 @@ namespace OpenSim.Region.Physics.OdePlugin
718 738
719 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) 739 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
720 return; 740 return;
721/* 741 /*
722// debug 742 // debug
723 PhysicsActor dp2; 743 PhysicsActor dp2;
724 if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) 744 if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
725 { 745 {
726 d.AABB aabb; 746 d.AABB aabb;
727 d.GeomGetAABB(g2, out aabb); 747 d.GeomGetAABB(g2, out aabb);
728 float x = aabb.MaxX - aabb.MinX; 748 float x = aabb.MaxX - aabb.MinX;
729 float y = aabb.MaxY - aabb.MinY; 749 float y = aabb.MaxY - aabb.MinY;
730 float z = aabb.MaxZ - aabb.MinZ; 750 float z = aabb.MaxZ - aabb.MinZ;
731 if (x > 60.0f || y > 60.0f || z > 60.0f) 751 if (x > 60.0f || y > 60.0f || z > 60.0f)
732 { 752 {
733 if (!actor_name_map.TryGetValue(g2, out dp2)) 753 if (!actor_name_map.TryGetValue(g2, out dp2))
734 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); 754 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
735 else 755 else
736 m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})", 756 m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})",
737 dp2.Name, dp2.Size, x, y, z, 757 dp2.Name, dp2.Size, x, y, z,
738 dp2.Position.ToString(), 758 dp2.Position.ToString(),
739 dp2.Orientation.ToString(), 759 dp2.Orientation.ToString(),
740 dp2.Orientation.Length()); 760 dp2.Orientation.Length());
741 return; 761 return;
742 } 762 }
743 } 763 }
744// 764 //
745*/ 765 */
746 766
747 767
748 if(d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || 768 if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
749 d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc) 769 d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
750 { 770 {
751 int cflags; 771 int cflags;
752 unchecked 772 unchecked
@@ -761,7 +781,7 @@ namespace OpenSim.Region.Physics.OdePlugin
761 catch (SEHException) 781 catch (SEHException)
762 { 782 {
763 m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim."); 783 m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim.");
764// ode.drelease(world); 784 // ode.drelease(world);
765 base.TriggerPhysicsBasedRestart(); 785 base.TriggerPhysicsBasedRestart();
766 } 786 }
767 catch (Exception e) 787 catch (Exception e)
@@ -801,26 +821,25 @@ namespace OpenSim.Region.Physics.OdePlugin
801 821
802 // get first contact 822 // get first contact
803 d.ContactGeom curContact = new d.ContactGeom(); 823 d.ContactGeom curContact = new d.ContactGeom();
824
804 if (!GetCurContactGeom(0, ref curContact)) 825 if (!GetCurContactGeom(0, ref curContact))
805 return; 826 return;
806 // for now it's the one with max depth 827
807 ContactPoint maxDepthContact = new ContactPoint(
808 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
809 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
810 curContact.depth
811 );
812 // do volume detection case 828 // do volume detection case
813 if ( 829 if ((p1.IsVolumeDtc || p2.IsVolumeDtc))
814 (p1.IsVolumeDtc || p2.IsVolumeDtc))
815 { 830 {
831 ContactPoint maxDepthContact = new ContactPoint(
832 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
833 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
834 curContact.depth, false
835 );
836
816 collision_accounting_events(p1, p2, maxDepthContact); 837 collision_accounting_events(p1, p2, maxDepthContact);
817 return; 838 return;
818 } 839 }
819 840
820 // big messy collision analises 841 // big messy collision analises
821 842
822 Vector3 normoverride = Vector3.Zero; //damm c#
823
824 float mu = 0; 843 float mu = 0;
825 float bounce = 0; 844 float bounce = 0;
826 float cfm = 0.0001f; 845 float cfm = 0.0001f;
@@ -831,34 +850,15 @@ namespace OpenSim.Region.Physics.OdePlugin
831 ContactData contactdata1 = new ContactData(0, 0, false); 850 ContactData contactdata1 = new ContactData(0, 0, false);
832 ContactData contactdata2 = new ContactData(0, 0, false); 851 ContactData contactdata2 = new ContactData(0, 0, false);
833 852
834 bool dop1foot = false; 853 bool dop1ava = false;
835 bool dop2foot = false; 854 bool dop2ava = false;
836 bool ignore = false; 855 bool ignore = false;
837 bool AvanormOverride = false;
838 856
839 switch (p1.PhysicsActorType) 857 switch (p1.PhysicsActorType)
840 { 858 {
841 case (int)ActorTypes.Agent: 859 case (int)ActorTypes.Agent:
842 { 860 {
843 AvanormOverride = true; 861 dop1ava = true;
844 Vector3 tmp = p2.Position - p1.Position;
845 normoverride = p2.Velocity - p1.Velocity;
846 mu = normoverride.LengthSquared();
847
848 if (mu > 1e-6)
849 {
850 mu = 1.0f / (float)Math.Sqrt(mu);
851 normoverride *= mu;
852 mu = Vector3.Dot(tmp, normoverride);
853 if (mu > 0)
854 normoverride *= -1;
855 }
856 else
857 {
858 tmp.Normalize();
859 normoverride = -tmp;
860 }
861
862 switch (p2.PhysicsActorType) 862 switch (p2.PhysicsActorType)
863 { 863 {
864 case (int)ActorTypes.Agent: 864 case (int)ActorTypes.Agent:
@@ -869,7 +869,6 @@ namespace OpenSim.Region.Physics.OdePlugin
869 case (int)ActorTypes.Prim: 869 case (int)ActorTypes.Prim:
870 if (p2.Velocity.LengthSquared() > 0.0f) 870 if (p2.Velocity.LengthSquared() > 0.0f)
871 p2.CollidingObj = true; 871 p2.CollidingObj = true;
872 dop1foot = true;
873 break; 872 break;
874 873
875 default: 874 default:
@@ -883,30 +882,9 @@ namespace OpenSim.Region.Physics.OdePlugin
883 switch (p2.PhysicsActorType) 882 switch (p2.PhysicsActorType)
884 { 883 {
885 case (int)ActorTypes.Agent: 884 case (int)ActorTypes.Agent:
886 AvanormOverride = true;
887
888 Vector3 tmp = p2.Position - p1.Position;
889 normoverride = p2.Velocity - p1.Velocity;
890 mu = normoverride.LengthSquared();
891 if (mu > 1e-6)
892 {
893 mu = 1.0f / (float)Math.Sqrt(mu);
894 normoverride *= mu;
895 mu = Vector3.Dot(tmp, normoverride);
896 if (mu > 0)
897 normoverride *= -1;
898 }
899 else
900 {
901 tmp.Normalize();
902 normoverride = -tmp;
903 }
904 885
905 bounce = 0; 886 dop2ava = true;
906 mu = 0;
907 cfm = 0.0001f;
908 887
909 dop2foot = true;
910 if (p1.Velocity.LengthSquared() > 0.0f) 888 if (p1.Velocity.LengthSquared() > 0.0f)
911 p1.CollidingObj = true; 889 p1.CollidingObj = true;
912 break; 890 break;
@@ -1011,146 +989,78 @@ namespace OpenSim.Region.Physics.OdePlugin
1011 default: 989 default:
1012 break; 990 break;
1013 } 991 }
992
1014 if (ignore) 993 if (ignore)
1015 return; 994 return;
1016 995
1017 IntPtr Joint;
1018 996
1019 int i = 0; 997 d.ContactGeom maxContact = curContact;
1020 while(true) 998// if (IgnoreNegSides && curContact.side1 < 0)
1021 { 999// maxContact.depth = float.MinValue;
1022 1000
1023 if (IgnoreNegSides && curContact.side1 < 0) 1001 d.ContactGeom minContact = curContact;
1024 { 1002// if (IgnoreNegSides && curContact.side1 < 0)
1025 if (++i >= count) 1003// minContact.depth = float.MaxValue;
1026 break; 1004
1005 IntPtr Joint;
1006 bool FeetCollision = false;
1007 int ncontacts = 0;
1027 1008
1028 if (!GetCurContactGeom(i, ref curContact))
1029 break;
1030 }
1031 else
1032 1009
1010 int i = 0;
1011
1012 while (true)
1033 { 1013 {
1014 if (m_global_contactcount >= maxContactsbeforedeath)
1015 break;
1034 1016
1035 if (AvanormOverride) 1017// if (!(IgnoreNegSides && curContact.side1 < 0))
1036 { 1018 {
1037 if (curContact.depth > 0.3f) 1019 bool noskip = true;
1020 if (dop1ava)
1021 {
1022 if (!(((OdeCharacter)p1).Collide(g1,false, ref curContact, ref FeetCollision)))
1023
1024 noskip = false;
1025 }
1026 else if (dop2ava)
1038 { 1027 {
1039 if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f) 1028 if (!(((OdeCharacter)p2).Collide(g2,true, ref curContact, ref FeetCollision)))
1040 p1.IsColliding = true; 1029 noskip = false;
1041 if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f)
1042 p2.IsColliding = true;
1043 curContact.normal.X = normoverride.X;
1044 curContact.normal.Y = normoverride.Y;
1045 curContact.normal.Z = normoverride.Z;
1046 } 1030 }
1047 1031
1048 else 1032 if (noskip)
1049 { 1033 {
1050 if (dop1foot) 1034 m_global_contactcount++;
1051 { 1035 ncontacts++;
1052 float sz = p1.Size.Z;
1053 Vector3 vtmp = p1.Position;
1054 float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f;
1055 if (ppos > 0f)
1056 {
1057 if (!p1.Flying)
1058 {
1059 d.AABB aabb;
1060 d.GeomGetAABB(g2, out aabb);
1061 float tmp = vtmp.Z - sz * .18f;
1062
1063 if (aabb.MaxZ < tmp)
1064 {
1065 vtmp.X = curContact.pos.X - vtmp.X;
1066 vtmp.Y = curContact.pos.Y - vtmp.Y;
1067 vtmp.Z = -0.2f;
1068 vtmp.Normalize();
1069 curContact.normal.X = vtmp.X;
1070 curContact.normal.Y = vtmp.Y;
1071 curContact.normal.Z = vtmp.Z;
1072 }
1073 }
1074 }
1075 else
1076 p1.IsColliding = true;
1077 1036
1078 } 1037 Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale);
1038 d.JointAttach(Joint, b1, b2);
1079 1039
1080 if (dop2foot) 1040 if (curContact.depth > maxContact.depth)
1081 { 1041 maxContact = curContact;
1082 float sz = p2.Size.Z;
1083 Vector3 vtmp = p2.Position;
1084 float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f;
1085 if (ppos > 0f)
1086 {
1087 if (!p2.Flying)
1088 {
1089 d.AABB aabb;
1090 d.GeomGetAABB(g1, out aabb);
1091 float tmp = vtmp.Z - sz * .18f;
1092
1093 if (aabb.MaxZ < tmp)
1094 {
1095 vtmp.X = curContact.pos.X - vtmp.X;
1096 vtmp.Y = curContact.pos.Y - vtmp.Y;
1097 vtmp.Z = -0.2f;
1098 vtmp.Normalize();
1099 curContact.normal.X = vtmp.X;
1100 curContact.normal.Y = vtmp.Y;
1101 curContact.normal.Z = vtmp.Z;
1102 }
1103 }
1104 }
1105 else
1106 p2.IsColliding = true;
1107 1042
1108 } 1043 if (curContact.depth < minContact.depth)
1044 minContact = curContact;
1109 } 1045 }
1110 } 1046 }
1111 1047
1112 Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale);
1113 d.JointAttach(Joint, b1, b2);
1114
1115 if (++m_global_contactcount >= maxContactsbeforedeath)
1116 break;
1117
1118 if (++i >= count) 1048 if (++i >= count)
1119 break; 1049 break;
1120 1050
1121 if (!GetCurContactGeom(i, ref curContact)) 1051 if (!GetCurContactGeom(i, ref curContact))
1122 break; 1052 break;
1123
1124 if (curContact.depth > maxDepthContact.PenetrationDepth)
1125 {
1126 maxDepthContact.Position.X = curContact.pos.X;
1127 maxDepthContact.Position.Y = curContact.pos.Y;
1128 maxDepthContact.Position.Z = curContact.pos.Z;
1129 maxDepthContact.SurfaceNormal.X = curContact.normal.X;
1130 maxDepthContact.SurfaceNormal.Y = curContact.normal.Y;
1131 maxDepthContact.SurfaceNormal.Z = curContact.normal.Z;
1132 maxDepthContact.PenetrationDepth = curContact.depth;
1133 }
1134 } 1053 }
1135 }
1136
1137 collision_accounting_events(p1, p2, maxDepthContact);
1138 1054
1139/* 1055 if (ncontacts > 0)
1140 if (notskipedcount > geomContactPointsStartthrottle)
1141 { 1056 {
1142 // If there are more then 3 contact points, it's likely 1057 ContactPoint maxDepthContact = new ContactPoint(
1143 // that we've got a pile of objects, so ... 1058 new Vector3(maxContact.pos.X, maxContact.pos.Y, maxContact.pos.Z),
1144 // We don't want to send out hundreds of terse updates over and over again 1059 new Vector3(minContact.normal.X, minContact.normal.Y, minContact.normal.Z),
1145 // so lets throttle them and send them again after it's somewhat sorted out. 1060 maxContact.depth, FeetCollision
1146 this needs checking so out for now 1061 );
1147 if (b1 != IntPtr.Zero) 1062 collision_accounting_events(p1, p2, maxDepthContact);
1148 p1.ThrottleUpdates = true;
1149 if (b2 != IntPtr.Zero)
1150 p2.ThrottleUpdates = true;
1151
1152 } 1063 }
1153 */
1154 } 1064 }
1155 1065
1156 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) 1066 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
@@ -1234,14 +1144,17 @@ namespace OpenSim.Region.Physics.OdePlugin
1234 { 1144 {
1235 foreach (OdeCharacter chr in _characters) 1145 foreach (OdeCharacter chr in _characters)
1236 { 1146 {
1237 if (chr == null || chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero) 1147 if (chr == null || chr.Body == IntPtr.Zero)
1238 continue; 1148 continue;
1239 1149
1240 chr.IsColliding = false; 1150 chr.IsColliding = false;
1241 // chr.CollidingGround = false; not done here 1151 // chr.CollidingGround = false; not done here
1242 chr.CollidingObj = false; 1152 chr.CollidingObj = false;
1243 // do colisions with static space 1153 // do colisions with static space
1244 d.SpaceCollide2(StaticSpace, chr.Shell, IntPtr.Zero, nearCallback); 1154 d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback);
1155
1156 // chars with chars
1157 d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback);
1245 // no coll with gnd 1158 // no coll with gnd
1246 } 1159 }
1247 } 1160 }
@@ -1283,16 +1196,25 @@ namespace OpenSim.Region.Physics.OdePlugin
1283 m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space"); 1196 m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space");
1284 } 1197 }
1285 } 1198 }
1286 // finally colide active things amoung them 1199 // colide active amoung them
1287 try 1200 try
1288 { 1201 {
1289 d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback); 1202 d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
1290 } 1203 }
1291 catch (AccessViolationException) 1204 catch (AccessViolationException)
1292 { 1205 {
1206 m_log.Warn("[PHYSICS]: Unable to collide Active with Characters space");
1207 }
1208 // and with chars
1209 try
1210 {
1211 d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback);
1212 }
1213 catch (AccessViolationException)
1214 {
1293 m_log.Warn("[PHYSICS]: Unable to collide in Active space"); 1215 m_log.Warn("[PHYSICS]: Unable to collide in Active space");
1294 } 1216 }
1295// _perloopContact.Clear(); 1217 // _perloopContact.Clear();
1296 } 1218 }
1297 1219
1298 #endregion 1220 #endregion
@@ -1328,13 +1250,13 @@ namespace OpenSim.Region.Physics.OdePlugin
1328 1250
1329 #region Add/Remove Entities 1251 #region Add/Remove Entities
1330 1252
1331 public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying) 1253 public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying)
1332 { 1254 {
1333 Vector3 pos; 1255 Vector3 pos;
1334 pos.X = position.X; 1256 pos.X = position.X;
1335 pos.Y = position.Y; 1257 pos.Y = position.Y;
1336 pos.Z = position.Z; 1258 pos.Z = position.Z;
1337 OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, avDensity, avMovementDivisorWalk, avMovementDivisorRun); 1259 OdeCharacter newAv = new OdeCharacter(localID,avName, this, pos, size, feetOffset, avDensity, avMovementDivisorWalk, avMovementDivisorRun);
1338 newAv.Flying = isFlying; 1260 newAv.Flying = isFlying;
1339 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; 1261 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
1340 1262
@@ -1777,7 +1699,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1777 foreach (OdeCharacter actor in _characters) 1699 foreach (OdeCharacter actor in _characters)
1778 { 1700 {
1779 if (actor != null) 1701 if (actor != null)
1780 actor.Move(ODE_STEPSIZE, defects); 1702 actor.Move(defects);
1781 } 1703 }
1782 if (defects.Count != 0) 1704 if (defects.Count != 0)
1783 { 1705 {
@@ -2788,5 +2710,17 @@ namespace OpenSim.Region.Physics.OdePlugin
2788 } 2710 }
2789 return new List<ContactResult>(); 2711 return new List<ContactResult>();
2790 } 2712 }
2713
2714 public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)
2715 {
2716 Util.FireAndForget( delegate
2717 {
2718 ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager);
2719 if(sitAvatar != null)
2720 sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse);
2721 });
2722 return 1;
2723 }
2724
2791 } 2725 }
2792} 2726}
generated by cgit v1.1 at 2024-10-24 15:25:04 +0000