aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics
diff options
context:
space:
mode:
authorMelanie2012-04-23 21:13:08 +0100
committerMelanie2012-04-23 21:13:08 +0100
commit159b3b27ca734ab523c47d2ab65397595b1c0a8e (patch)
tree8038d76c48e48b000e7f96eeade3d335db68daaa /OpenSim/Region/Physics
parentMerge branch 'master' into careminster (diff)
parentubitODE - do own timing control (as chODE does) until heartbeat does it right (diff)
downloadopensim-SC-159b3b27ca734ab523c47d2ab65397595b1c0a8e.zip
opensim-SC-159b3b27ca734ab523c47d2ab65397595b1c0a8e.tar.gz
opensim-SC-159b3b27ca734ab523c47d2ab65397595b1c0a8e.tar.bz2
opensim-SC-159b3b27ca734ab523c47d2ab65397595b1c0a8e.tar.xz
Merge branch 'master' of ssh://melanie@3dhosting.de/var/git/careminster into careminster
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs23
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs746
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs215
3 files changed, 508 insertions, 476 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
index ec4be58..6ffcb9e 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -686,6 +686,9 @@ namespace OpenSim.Region.Physics.OdePlugin
686 686
687 Body = d.BodyCreate(_parent_scene.world); 687 Body = d.BodyCreate(_parent_scene.world);
688 688
689 _zeroFlag = false;
690 m_pidControllerActive = true;
691
689 d.BodySetAutoDisableFlag(Body, false); 692 d.BodySetAutoDisableFlag(Body, false);
690 d.BodySetPosition(Body, npositionX, npositionY, npositionZ); 693 d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
691 694
@@ -862,12 +865,12 @@ namespace OpenSim.Region.Physics.OdePlugin
862 float depth = terrainheight - chrminZ; 865 float depth = terrainheight - chrminZ;
863 if (!flying) 866 if (!flying)
864 { 867 {
865 vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 60; 868 vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50;
866 } 869 }
867 else 870 else
868 vec.Z = depth * PID_P * 60; 871 vec.Z = depth * PID_P * 50;
869 872
870 if (depth < 0.2f) 873 if (depth < 0.1f)
871 { 874 {
872 m_iscolliding = true; 875 m_iscolliding = true;
873 m_colliderfilter = 2; 876 m_colliderfilter = 2;
@@ -1009,9 +1012,17 @@ namespace OpenSim.Region.Physics.OdePlugin
1009 1012
1010 // update our local ideia of position velocity and aceleration 1013 // update our local ideia of position velocity and aceleration
1011 _position = localpos; 1014 _position = localpos;
1012 _acceleration = _velocity; // previus velocity 1015 if (_zeroFlag)
1013 _velocity = vel; 1016 {
1014 _acceleration = (vel - _acceleration) / timeStep; 1017 _velocity = Vector3.Zero;
1018 _acceleration = Vector3.Zero;
1019 }
1020 else
1021 {
1022 _acceleration = _velocity; // previus velocity
1023 _velocity = vel;
1024 _acceleration = (vel - _acceleration) / timeStep;
1025 }
1015 1026
1016 } 1027 }
1017 1028
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
index 5467b9f..dc6c18d 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -68,9 +68,17 @@ namespace OpenSim.Region.Physics.OdePlugin
68 private bool m_fakeisphysical; 68 private bool m_fakeisphysical;
69 private bool m_isphantom; 69 private bool m_isphantom;
70 private bool m_fakeisphantom; 70 private bool m_fakeisphantom;
71 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
72 private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
71 73
72 protected bool m_building; 74 protected bool m_building;
73 protected bool m_forcePosOrRotation; 75 protected bool m_forcePosOrRotation;
76 private bool m_iscolliding;
77
78 internal bool m_isSelected;
79 private bool m_delaySelect;
80 private bool m_lastdoneSelected;
81 internal bool m_outbounds;
74 82
75 private Quaternion m_lastorientation = new Quaternion(); 83 private Quaternion m_lastorientation = new Quaternion();
76 private Quaternion _orientation; 84 private Quaternion _orientation;
@@ -90,10 +98,12 @@ namespace OpenSim.Region.Physics.OdePlugin
90 private Vector3 m_forceacc; 98 private Vector3 m_forceacc;
91 private Vector3 m_angularForceacc; 99 private Vector3 m_angularForceacc;
92 100
101 private float m_invTimeStep = 50.0f;
102 private float m_timeStep = .02f;
103
104
93 private Vector3 m_PIDTarget; 105 private Vector3 m_PIDTarget;
94 private float m_PIDTau; 106 private float m_PIDTau;
95 private float PID_D = 35f;
96 private float PID_G = 25f;
97 private bool m_usePID; 107 private bool m_usePID;
98 108
99 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), 109 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
@@ -153,14 +163,6 @@ namespace OpenSim.Region.Physics.OdePlugin
153 163
154 private List<OdePrim> childrenPrim = new List<OdePrim>(); 164 private List<OdePrim> childrenPrim = new List<OdePrim>();
155 165
156 private bool m_iscolliding;
157
158 public bool m_isSelected;
159 private bool m_delaySelect;
160 private bool m_lastdoneSelected;
161 public bool m_outbounds;
162
163 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
164 166
165 private bool m_throttleUpdates; 167 private bool m_throttleUpdates;
166 private int throttleCounter; 168 private int throttleCounter;
@@ -223,9 +225,12 @@ namespace OpenSim.Region.Physics.OdePlugin
223 225
224 public override bool IsVolumeDtc 226 public override bool IsVolumeDtc
225 { 227 {
226 set { return; } 228 get { return m_fakeisVolumeDetect; }
227 get { return m_isVolumeDetect; } 229 set
228 230 {
231 m_fakeisVolumeDetect = value;
232 AddChange(changes.VolumeDtc, value);
233 }
229 } 234 }
230 235
231 236
@@ -234,10 +239,7 @@ namespace OpenSim.Region.Physics.OdePlugin
234 get { return m_fakeisphantom; } 239 get { return m_fakeisphantom; }
235 set 240 set
236 { 241 {
237 m_fakeisphantom = value; // we show imediatly to outside that we changed physical 242 m_fakeisphantom = value;
238 // and also to stop imediatly some updates
239 // but real change will only happen in taintprocessing
240
241 AddChange(changes.Phantom, value); 243 AddChange(changes.Phantom, value);
242 } 244 }
243 } 245 }
@@ -427,7 +429,8 @@ namespace OpenSim.Region.Physics.OdePlugin
427 429
428 public override void SetVolumeDetect(int param) 430 public override void SetVolumeDetect(int param)
429 { 431 {
430 AddChange(changes.VolumeDtc, (param != 0)); 432 m_fakeisVolumeDetect = (param != 0);
433 AddChange(changes.VolumeDtc, m_fakeisVolumeDetect);
431 } 434 }
432 435
433 public override Vector3 GeometricCenter 436 public override Vector3 GeometricCenter
@@ -631,7 +634,6 @@ namespace OpenSim.Region.Physics.OdePlugin
631 Vector3 pv = Vector3.Zero; 634 Vector3 pv = Vector3.Zero;
632 if (_zeroFlag) 635 if (_zeroFlag)
633 return pv; 636 return pv;
634 m_lastUpdateSent = false;
635 637
636 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) 638 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
637 return pv; 639 return pv;
@@ -685,12 +687,50 @@ namespace OpenSim.Region.Physics.OdePlugin
685 } 687 }
686 688
687 public override bool PIDActive { set { m_usePID = value; } } 689 public override bool PIDActive { set { m_usePID = value; } }
688 public override float PIDTau { set { m_PIDTau = value; } } 690 public override float PIDTau
691 {
692 set
693 {
694 if (value <= 0)
695 m_PIDTau = 0;
696 else
697 {
698 float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
699 if (value < mint)
700 m_PIDTau = mint;
701 else
702 m_PIDTau = value;
703 }
704 }
705 }
689 706
690 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } 707 public override float PIDHoverHeight
708 {
709 set
710 {
711 m_PIDHoverHeight = value;
712 if (value == 0)
713 m_useHoverPID = false;
714 }
715 }
691 public override bool PIDHoverActive { set { m_useHoverPID = value; } } 716 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
692 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } 717 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
693 public override float PIDHoverTau { set { m_PIDHoverTau = value; } } 718 public override float PIDHoverTau
719 {
720 set
721 {
722 if (value <= 0)
723 m_PIDHoverTau = 0;
724 else
725 {
726 float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
727 if (value < mint)
728 m_PIDHoverTau = mint;
729 else
730 m_PIDHoverTau = value;
731 }
732 }
733 }
694 734
695 public override Quaternion APIDTarget { set { return; } } 735 public override Quaternion APIDTarget { set { return; } }
696 736
@@ -761,7 +801,7 @@ namespace OpenSim.Region.Physics.OdePlugin
761 { 801 {
762 if (force.IsFinite()) 802 if (force.IsFinite())
763 { 803 {
764 AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE); 804 AddChange(changes.AddForce, force * m_invTimeStep);
765 } 805 }
766 else 806 else
767 { 807 {
@@ -774,7 +814,7 @@ namespace OpenSim.Region.Physics.OdePlugin
774 { 814 {
775 if (force.IsFinite()) 815 if (force.IsFinite())
776 { 816 {
777 AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE); 817 AddChange(changes.AddAngForce, force * m_invTimeStep);
778 } 818 }
779 else 819 else
780 { 820 {
@@ -911,8 +951,9 @@ namespace OpenSim.Region.Physics.OdePlugin
911 _position = pos; 951 _position = pos;
912 givefakepos = 0; 952 givefakepos = 0;
913 953
914 PID_D = parent_scene.bodyPIDD; 954 m_timeStep = parent_scene.ODE_STEPSIZE;
915 PID_G = parent_scene.bodyPIDG; 955 m_invTimeStep = 1f / m_timeStep;
956
916 m_density = parent_scene.geomDefaultDensity; 957 m_density = parent_scene.geomDefaultDensity;
917 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; 958 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
918 body_autodisable_frames = parent_scene.bodyFramesAutoDisable; 959 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
@@ -958,6 +999,7 @@ namespace OpenSim.Region.Physics.OdePlugin
958 m_fakeisphysical = m_isphysical; 999 m_fakeisphysical = m_isphysical;
959 1000
960 m_isVolumeDetect = false; 1001 m_isVolumeDetect = false;
1002 m_fakeisVolumeDetect = false;
961 1003
962 m_force = Vector3.Zero; 1004 m_force = Vector3.Zero;
963 1005
@@ -1066,7 +1108,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1066 prm.m_collisionCategories = CollisionCategories.Selected; 1108 prm.m_collisionCategories = CollisionCategories.Selected;
1067 prm.m_collisionFlags = 0; 1109 prm.m_collisionFlags = 0;
1068 } 1110 }
1069 else if (prm.IsVolumeDtc) 1111 else if (prm.m_isVolumeDetect)
1070 { 1112 {
1071 prm.m_collisionCategories = CollisionCategories.VolumeDtc; 1113 prm.m_collisionCategories = CollisionCategories.VolumeDtc;
1072 if (m_isphysical) 1114 if (m_isphysical)
@@ -1445,14 +1487,14 @@ namespace OpenSim.Region.Physics.OdePlugin
1445 hasOOBoffsetFromMesh = false; 1487 hasOOBoffsetFromMesh = false;
1446 CalcPrimBodyData(); 1488 CalcPrimBodyData();
1447 } 1489 }
1448 1490/*
1449 private void ChildSetGeom(OdePrim odePrim) 1491 private void ChildSetGeom(OdePrim odePrim)
1450 { 1492 {
1451 // well.. 1493 // well..
1452 DestroyBody(); 1494 DestroyBody();
1453 MakeBody(); 1495 MakeBody();
1454 } 1496 }
1455 1497*/
1456 //sets non physical prim m_targetSpace to right space in spaces grid for static prims 1498 //sets non physical prim m_targetSpace to right space in spaces grid for static prims
1457 // should only be called for non physical prims unless they are becoming non physical 1499 // should only be called for non physical prims unless they are becoming non physical
1458 private void SetInStaticSpace(OdePrim prim) 1500 private void SetInStaticSpace(OdePrim prim)
@@ -2650,6 +2692,31 @@ namespace OpenSim.Region.Physics.OdePlugin
2650 ApplyCollisionCatFlags(); 2692 ApplyCollisionCatFlags();
2651 } 2693 }
2652 2694
2695/* not in use
2696 internal void ChildSelectedChange(bool childSelect)
2697 {
2698 if(childPrim)
2699 return;
2700
2701 if (childSelect == m_isSelected)
2702 return;
2703
2704 if (childSelect)
2705 {
2706 DoSelectedStatus(true);
2707 }
2708
2709 else
2710 {
2711 foreach (OdePrim prm in childrenPrim)
2712 {
2713 if (prm.m_isSelected)
2714 return;
2715 }
2716 DoSelectedStatus(false);
2717 }
2718 }
2719*/
2653 private void changeSelectedStatus(bool newval) 2720 private void changeSelectedStatus(bool newval)
2654 { 2721 {
2655 if (m_lastdoneSelected == newval) 2722 if (m_lastdoneSelected == newval)
@@ -2706,6 +2773,9 @@ namespace OpenSim.Region.Physics.OdePlugin
2706 prm.m_delaySelect = false; 2773 prm.m_delaySelect = false;
2707 } 2774 }
2708 } 2775 }
2776// else if (_parent != null)
2777// ((OdePrim)_parent).ChildSelectedChange(true);
2778
2709 2779
2710 if (prim_geom != null) 2780 if (prim_geom != null)
2711 { 2781 {
@@ -2741,8 +2811,13 @@ namespace OpenSim.Region.Physics.OdePlugin
2741 } 2811 }
2742 else 2812 else
2743 { 2813 {
2744 if (!childPrim && Body != IntPtr.Zero && !m_disabled) 2814 if (!childPrim)
2745 d.BodyEnable(Body); 2815 {
2816 if (Body != IntPtr.Zero && !m_disabled)
2817 d.BodyEnable(Body);
2818 }
2819// else if (_parent != null)
2820// ((OdePrim)_parent).ChildSelectedChange(false);
2746 2821
2747 UpdateCollisionCatFlags(); 2822 UpdateCollisionCatFlags();
2748 ApplyCollisionCatFlags(); 2823 ApplyCollisionCatFlags();
@@ -2929,7 +3004,6 @@ namespace OpenSim.Region.Physics.OdePlugin
2929 givefakeori--; 3004 givefakeori--;
2930 if (givefakeori < 0) 3005 if (givefakeori < 0)
2931 givefakeori = 0; 3006 givefakeori = 0;
2932
2933 resetCollisionAccounting(); 3007 resetCollisionAccounting();
2934 } 3008 }
2935 3009
@@ -3084,9 +3158,10 @@ namespace OpenSim.Region.Physics.OdePlugin
3084 d.BodyEnable(Body); 3158 d.BodyEnable(Body);
3085 } 3159 }
3086 3160
3087 private void changeAddForce(Vector3 force) 3161
3162 private void changeAddImpulse(Vector3 impulse)
3088 { 3163 {
3089 m_forceacc += force; 3164 m_forceacc += impulse * m_invTimeStep;
3090 if (!m_isSelected) 3165 if (!m_isSelected)
3091 { 3166 {
3092 lock (this) 3167 lock (this)
@@ -3105,9 +3180,10 @@ namespace OpenSim.Region.Physics.OdePlugin
3105 } 3180 }
3106 } 3181 }
3107 3182
3108 private void changeAddAngularForce(Vector3 aforce) 3183 // actually angular impulse
3184 private void changeAddAngularImpulse(Vector3 aimpulse)
3109 { 3185 {
3110 m_angularForceacc += aforce; 3186 m_angularForceacc += aimpulse * m_invTimeStep;
3111 if (!m_isSelected) 3187 if (!m_isSelected)
3112 { 3188 {
3113 lock (this) 3189 lock (this)
@@ -3145,6 +3221,7 @@ namespace OpenSim.Region.Physics.OdePlugin
3145 private void changeVolumedetetion(bool newVolDtc) 3221 private void changeVolumedetetion(bool newVolDtc)
3146 { 3222 {
3147 m_isVolumeDetect = newVolDtc; 3223 m_isVolumeDetect = newVolDtc;
3224 m_fakeisVolumeDetect = newVolDtc;
3148 UpdateCollisionCatFlags(); 3225 UpdateCollisionCatFlags();
3149 ApplyCollisionCatFlags(); 3226 ApplyCollisionCatFlags();
3150 } 3227 }
@@ -3229,326 +3306,246 @@ namespace OpenSim.Region.Physics.OdePlugin
3229 if (!childPrim && m_isphysical && Body != IntPtr.Zero && 3306 if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
3230 !m_disabled && !m_isSelected && !m_building && !m_outbounds) 3307 !m_disabled && !m_isSelected && !m_building && !m_outbounds)
3231 { 3308 {
3232 if (d.BodyIsEnabled(Body)) 3309 if (!d.BodyIsEnabled(Body))
3233 { 3310 {
3234 float timestep = _parent_scene.ODE_STEPSIZE; 3311 // let vehicles sleep
3235 3312 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
3236 // check outside region
3237 d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator
3238
3239 if (lpos.Z < -100 || lpos.Z > 100000f)
3240 {
3241 m_outbounds = true;
3242
3243 lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
3244 _acceleration.X = 0;
3245 _acceleration.Y = 0;
3246 _acceleration.Z = 0;
3247
3248 _velocity.X = 0;
3249 _velocity.Y = 0;
3250 _velocity.Z = 0;
3251 m_rotationalVelocity.X = 0;
3252 m_rotationalVelocity.Y = 0;
3253 m_rotationalVelocity.Z = 0;
3254
3255 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3256 d.BodySetAngularVel(Body, 0, 0, 0); // stop it
3257 d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
3258 m_lastposition = _position;
3259 m_lastorientation = _orientation;
3260
3261 base.RequestPhysicsterseUpdate();
3262
3263 m_throttleUpdates = false;
3264 throttleCounter = 0;
3265 _zeroFlag = true;
3266
3267 disableBodySoft(); // disable it and colisions
3268 base.RaiseOutOfBounds(_position);
3269 return; 3313 return;
3270 }
3271 3314
3272 if (lpos.X < 0f) 3315 if (++bodydisablecontrol < 20)
3273 {
3274 _position.X = Util.Clip(lpos.X, -2f, -0.1f);
3275 m_outbounds = true;
3276 }
3277 else if (lpos.X > _parent_scene.WorldExtents.X)
3278 {
3279 _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
3280 m_outbounds = true;
3281 }
3282 if (lpos.Y < 0f)
3283 {
3284 _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
3285 m_outbounds = true;
3286 }
3287 else if (lpos.Y > _parent_scene.WorldExtents.Y)
3288 {
3289 _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
3290 m_outbounds = true;
3291 }
3292
3293 if (m_outbounds)
3294 {
3295 m_lastposition = _position;
3296 m_lastorientation = _orientation;
3297
3298 d.Vector3 dtmp = d.BodyGetAngularVel(Body);
3299 m_rotationalVelocity.X = dtmp.X;
3300 m_rotationalVelocity.Y = dtmp.Y;
3301 m_rotationalVelocity.Z = dtmp.Z;
3302
3303 dtmp = d.BodyGetLinearVel(Body);
3304 _velocity.X = dtmp.X;
3305 _velocity.Y = dtmp.Y;
3306 _velocity.Z = dtmp.Z;
3307
3308 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3309 d.BodySetAngularVel(Body, 0, 0, 0);
3310 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3311 disableBodySoft(); // stop collisions
3312 base.RequestPhysicsterseUpdate();
3313 return; 3316 return;
3314 }
3315 3317
3316 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) 3318 bodydisablecontrol = 0;
3317 { 3319 d.BodyEnable(Body);
3318 // 'VEHICLES' are dealt with in ODEDynamics.cs 3320 }
3319 m_vehicle.Step();
3320 }
3321 else
3322 {
3323 float fx = 0;
3324 float fy = 0;
3325 float fz = 0;
3326 3321
3327 float m_mass = _mass; 3322 // check outside region
3323 d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator
3328 3324
3329 // fz = 0f; 3325 if (lpos.Z < -100 || lpos.Z > 100000f)
3330 //m_log.Info(m_collisionFlags.ToString()); 3326 {
3331 if (m_usePID) 3327 m_outbounds = true;
3332 {
3333 3328
3334 // If the PID Controller isn't active then we set our force 3329 lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
3335 // calculating base velocity to the current position 3330 _acceleration.X = 0;
3331 _acceleration.Y = 0;
3332 _acceleration.Z = 0;
3336 3333
3337 if ((m_PIDTau < 1) && (m_PIDTau != 0)) 3334 _velocity.X = 0;
3338 { 3335 _velocity.Y = 0;
3339 //PID_G = PID_G / m_PIDTau; 3336 _velocity.Z = 0;
3340 m_PIDTau = 1; 3337 m_rotationalVelocity.X = 0;
3341 } 3338 m_rotationalVelocity.Y = 0;
3339 m_rotationalVelocity.Z = 0;
3342 3340
3343 if ((PID_G - m_PIDTau) <= 0) 3341 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3344 { 3342 d.BodySetAngularVel(Body, 0, 0, 0); // stop it
3345 PID_G = m_PIDTau + 1; 3343 d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
3346 } 3344 m_lastposition = _position;
3345 m_lastorientation = _orientation;
3347 3346
3348 d.Vector3 vel = d.BodyGetLinearVel(Body); 3347 base.RequestPhysicsterseUpdate();
3349 d.Vector3 pos = d.BodyGetPosition(Body);
3350 _target_velocity =
3351 new Vector3(
3352 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
3353 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
3354 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
3355 );
3356 3348
3357 // if velocity is zero, use position control; otherwise, velocity control 3349 throttleCounter = 0;
3350 _zeroFlag = true;
3358 3351
3359 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) 3352 disableBodySoft(); // disable it and colisions
3360 { 3353 base.RaiseOutOfBounds(_position);
3361 // keep track of where we stopped. No more slippin' & slidin' 3354 return;
3362 3355 }
3363 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3364 // react to the physics scene by moving it's position.
3365 // Avatar to Avatar collisions
3366 // Prim to avatar collisions
3367
3368 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
3369 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
3370 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
3371 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3372 d.BodySetLinearVel(Body, 0, 0, 0);
3373 d.BodyAddForce(Body, 0, 0, fz);
3374 return;
3375 }
3376 else
3377 {
3378 _zeroFlag = false;
3379 3356
3380 // We're flying and colliding with something 3357 if (lpos.X < 0f)
3381 fx = ((_target_velocity.X) - vel.X) * (PID_D); 3358 {
3382 fy = ((_target_velocity.Y) - vel.Y) * (PID_D); 3359 _position.X = Util.Clip(lpos.X, -2f, -0.1f);
3360 m_outbounds = true;
3361 }
3362 else if (lpos.X > _parent_scene.WorldExtents.X)
3363 {
3364 _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
3365 m_outbounds = true;
3366 }
3367 if (lpos.Y < 0f)
3368 {
3369 _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
3370 m_outbounds = true;
3371 }
3372 else if (lpos.Y > _parent_scene.WorldExtents.Y)
3373 {
3374 _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
3375 m_outbounds = true;
3376 }
3383 3377
3384 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; 3378 if (m_outbounds)
3379 {
3380 m_lastposition = _position;
3381 m_lastorientation = _orientation;
3385 3382
3386 fz = ((_target_velocity.Z - vel.Z) * (PID_D)); 3383 d.Vector3 dtmp = d.BodyGetAngularVel(Body);
3387 } 3384 m_rotationalVelocity.X = dtmp.X;
3388 } // end if (m_usePID) 3385 m_rotationalVelocity.Y = dtmp.Y;
3386 m_rotationalVelocity.Z = dtmp.Z;
3389 3387
3390 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller 3388 dtmp = d.BodyGetLinearVel(Body);
3391 else if (m_useHoverPID) 3389 _velocity.X = dtmp.X;
3392 { 3390 _velocity.Y = dtmp.Y;
3393 //Console.WriteLine("Hover " + Name); 3391 _velocity.Z = dtmp.Z;
3394 3392
3395 // If we're using the PID controller, then we have no gravity 3393 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3394 d.BodySetAngularVel(Body, 0, 0, 0);
3395 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3396 disableBodySoft(); // stop collisions
3397 base.RequestPhysicsterseUpdate();
3398 return;
3399 }
3396 3400
3397 // no lock; for now it's only called from within Simulate() 3401 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
3402 {
3403 // 'VEHICLES' are dealt with in ODEDynamics.cs
3404 m_vehicle.Step();
3405 return;
3406 }
3398 3407
3399 // If the PID Controller isn't active then we set our force 3408 float fx = 0;
3400 // calculating base velocity to the current position 3409 float fy = 0;
3410 float fz = 0;
3401 3411
3402 if ((m_PIDTau < 1)) 3412 float m_mass = _mass;
3403 {
3404 PID_G = PID_G / m_PIDTau;
3405 }
3406 3413
3407 if ((PID_G - m_PIDTau) <= 0) 3414 if (m_usePID && m_PIDTau > 0)
3408 { 3415 {
3409 PID_G = m_PIDTau + 1; 3416 // for now position error
3410 } 3417 _target_velocity =
3418 new Vector3(
3419 (m_PIDTarget.X - lpos.X),
3420 (m_PIDTarget.Y - lpos.Y),
3421 (m_PIDTarget.Z - lpos.Z)
3422 );
3411 3423
3412 // Where are we, and where are we headed? 3424 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f))
3413 d.Vector3 pos = d.BodyGetPosition(Body); 3425 {
3414 d.Vector3 vel = d.BodyGetLinearVel(Body); 3426 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3427 d.BodySetLinearVel(Body, 0, 0, 0);
3428 return;
3429 }
3430 else
3431 {
3432 _zeroFlag = false;
3415 3433
3416 // Non-Vehicles have a limited set of Hover options. 3434 float tmp = 1 / m_PIDTau;
3417 // determine what our target height really is based on HoverType 3435 _target_velocity *= tmp;
3418 switch (m_PIDHoverType)
3419 {
3420 case PIDHoverType.Ground:
3421 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3422 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3423 break;
3424 case PIDHoverType.GroundAndWater:
3425 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3426 m_waterHeight = _parent_scene.GetWaterLevel();
3427 if (m_groundHeight > m_waterHeight)
3428 {
3429 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3430 }
3431 else
3432 {
3433 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3434 }
3435 break;
3436 3436
3437 } // end switch (m_PIDHoverType) 3437 // apply limits
3438 tmp = _target_velocity.Length();
3439 if (tmp > 50.0f)
3440 {
3441 tmp = 50 / tmp;
3442 _target_velocity *= tmp;
3443 }
3444 else if (tmp < 0.05f)
3445 {
3446 tmp = 0.05f / tmp;
3447 _target_velocity *= tmp;
3448 }
3438 3449
3450 d.Vector3 vel = d.BodyGetLinearVel(Body);
3451 fx = (_target_velocity.X - vel.X) * m_invTimeStep;
3452 fy = (_target_velocity.Y - vel.Y) * m_invTimeStep;
3453 fz = (_target_velocity.Z - vel.Z) * m_invTimeStep;
3454 }
3455 } // end if (m_usePID)
3439 3456
3440 _target_velocity = 3457 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
3441 new Vector3(0.0f, 0.0f, 3458 else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0)
3442 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) 3459 {
3443 );
3444 3460
3445 // if velocity is zero, use position control; otherwise, velocity control 3461 // Non-Vehicles have a limited set of Hover options.
3462 // determine what our target height really is based on HoverType
3446 3463
3447 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) 3464 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y);
3448 {
3449 // keep track of where we stopped. No more slippin' & slidin'
3450 3465
3451 // We only want to deactivate the PID Controller if we think we want to have our surrogate 3466 switch (m_PIDHoverType)
3452 // react to the physics scene by moving it's position. 3467 {
3453 // Avatar to Avatar collisions 3468 case PIDHoverType.Ground:
3454 // Prim to avatar collisions 3469 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3470 break;
3455 3471
3456 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); 3472 case PIDHoverType.GroundAndWater:
3457 d.BodySetLinearVel(Body, vel.X, vel.Y, 0); 3473 m_waterHeight = _parent_scene.GetWaterLevel();
3458 // ? d.BodyAddForce(Body, 0, 0, fz); 3474 if (m_groundHeight > m_waterHeight)
3459 return; 3475 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3460 }
3461 else 3476 else
3462 { 3477 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3463 _zeroFlag = false; 3478 break;
3479 } // end switch (m_PIDHoverType)
3464 3480
3465 // We're flying and colliding with something 3481 // don't go underground unless volumedetector
3466 fz = ((_target_velocity.Z - vel.Z) * (PID_D));
3467 }
3468 }
3469 else
3470 {
3471 float b = (1.0f - m_buoyancy);
3472 fx = _parent_scene.gravityx * b;
3473 fy = _parent_scene.gravityy * b;
3474 fz = _parent_scene.gravityz * b;
3475 }
3476
3477 fx *= m_mass;
3478 fy *= m_mass;
3479 fz *= m_mass;
3480 3482
3481 // constant force 3483 if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect)
3482 fx += m_force.X; 3484 {
3483 fy += m_force.Y; 3485 d.Vector3 vel = d.BodyGetLinearVel(Body);
3484 fz += m_force.Z;
3485
3486 fx += m_forceacc.X;
3487 fy += m_forceacc.Y;
3488 fz += m_forceacc.Z;
3489 3486
3490 m_forceacc = Vector3.Zero; 3487 fz = (m_targetHoverHeight - lpos.Z);
3491 3488
3492 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); 3489 // if error is zero, use position control; otherwise, velocity control
3493 if (fx != 0 || fy != 0 || fz != 0) 3490 if (Math.Abs(fz) < 0.01f)
3494 { 3491 {
3495 d.BodyAddForce(Body, fx, fy, fz); 3492 d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight);
3496 //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); 3493 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
3494 return;
3497 } 3495 }
3496 else
3497 {
3498 _zeroFlag = false;
3499 fz /= m_PIDHoverTau;
3498 3500
3499 Vector3 trq; 3501 float tmp = Math.Abs(fz);
3502 if (tmp > 50)
3503 fz = 50 * Math.Sign(fz);
3504 else if (tmp < 0.1)
3505 fz = 0.1f * Math.Sign(fz);
3500 3506
3501 trq = _torque; 3507 fz = ((fz - vel.Z) * m_invTimeStep);
3502 trq += m_angularForceacc;
3503 m_angularForceacc = Vector3.Zero;
3504 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
3505 {
3506 d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
3507 } 3508 }
3508 } 3509 }
3510 }
3511 else
3512 {
3513 float b = (1.0f - m_buoyancy);
3514 fx = _parent_scene.gravityx * b;
3515 fy = _parent_scene.gravityy * b;
3516 fz = _parent_scene.gravityz * b;
3517 }
3509 3518
3510 // update our ideia of velocities and acelerations 3519 fx *= m_mass;
3511 d.Quaternion ori; 3520 fy *= m_mass;
3512 d.Vector3 dtmpu; 3521 fz *= m_mass;
3513
3514 _position.X = lpos.X;
3515 _position.Y = lpos.Y;
3516 _position.Z = lpos.Z;
3517
3518 d.GeomCopyQuaternion(prim_geom, out ori);
3519 _orientation.X = ori.X;
3520 _orientation.Y = ori.Y;
3521 _orientation.Z = ori.Z;
3522 _orientation.W = ori.W;
3523
3524 _acceleration = _velocity;
3525 3522
3526 dtmpu = d.BodyGetLinearVel(Body); 3523 // constant force
3527 _velocity.X = dtmpu.X; 3524 fx += m_force.X;
3528 _velocity.Y = dtmpu.Y; 3525 fy += m_force.Y;
3529 _velocity.Z = dtmpu.Z; 3526 fz += m_force.Z;
3530 3527
3531 float invts = 1 / timestep; 3528 fx += m_forceacc.X;
3532 _acceleration = (_velocity - _acceleration) * invts; 3529 fy += m_forceacc.Y;
3530 fz += m_forceacc.Z;
3533 3531
3534 dtmpu = d.BodyGetAngularVel(Body); 3532 m_forceacc = Vector3.Zero;
3535 m_rotationalVelocity.X = dtmpu.X;
3536 m_rotationalVelocity.Y = dtmpu.Y;
3537 m_rotationalVelocity.Z = dtmpu.Z;
3538 }
3539 3533
3540 else // body disabled/sleeping 3534 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3535 if (fx != 0 || fy != 0 || fz != 0)
3541 { 3536 {
3542 // let vehicles sleep 3537 d.BodyAddForce(Body, fx, fy, fz);
3543 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) 3538 //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3544 return; 3539 }
3545 3540
3546 if (++bodydisablecontrol < 20) 3541 Vector3 trq;
3547 return;
3548 3542
3549 bodydisablecontrol = 0; 3543 trq = _torque;
3550 d.BodyEnable(Body); 3544 trq += m_angularForceacc;
3551 return; 3545 m_angularForceacc = Vector3.Zero;
3546 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
3547 {
3548 d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
3552 } 3549 }
3553 } 3550 }
3554 else 3551 else
@@ -3560,92 +3557,113 @@ namespace OpenSim.Region.Physics.OdePlugin
3560 } 3557 }
3561 } 3558 }
3562 3559
3563 3560 public void UpdatePositionAndVelocity()
3564 public void UpdatePositionAndVelocity(float simulatedtime)
3565 { 3561 {
3566 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! 3562 if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero)
3567 if (_parent == null && !m_disabled && !m_building && !m_outbounds)
3568 { 3563 {
3569 if (Body != IntPtr.Zero) 3564 if (d.BodyIsEnabled(Body) || !_zeroFlag)
3570 { 3565 {
3571 bool lastZeroFlag = _zeroFlag; 3566 bool lastZeroFlag = _zeroFlag;
3572 3567
3573 if ((Math.Abs(m_lastposition.X - _position.X) < 0.01) 3568 d.Vector3 lpos;
3574 && (Math.Abs(m_lastposition.Y - _position.Y) < 0.01) 3569 d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator
3575 && (Math.Abs(m_lastposition.Z - _position.Z) < 0.01) 3570
3576 && (Math.Abs(m_lastorientation.X - _orientation.X) < 0.0001) 3571 d.Quaternion ori;
3577 && (Math.Abs(m_lastorientation.Y - _orientation.Y) < 0.0001) 3572 d.GeomCopyQuaternion(prim_geom, out ori);
3578 && (Math.Abs(m_lastorientation.Z - _orientation.Z) < 0.0001) 3573
3574 // decide if moving
3575 // use positions since this are integrated quantities
3576 // tolerance values depende a lot on simulation noise...
3577 // use simple math.abs since we dont need to be exact
3578
3579 if (
3580 (Math.Abs(_position.X - lpos.X) < 0.001f)
3581 && (Math.Abs(_position.Y - lpos.Y) < 0.001f)
3582 && (Math.Abs(_position.Z - lpos.Z) < 0.001f)
3583 && (Math.Abs(_orientation.X - ori.X) < 0.0001f)
3584 && (Math.Abs(_orientation.Y - ori.Y) < 0.0001f)
3585 && (Math.Abs(_orientation.Z - ori.Z) < 0.0001f) // ignore W
3579 ) 3586 )
3580 { 3587 {
3581 _zeroFlag = true; 3588 _zeroFlag = true;
3582 m_throttleUpdates = false;
3583 } 3589 }
3584 else 3590 else
3585 {
3586 _zeroFlag = false; 3591 _zeroFlag = false;
3587 m_lastUpdateSent = false;
3588 }
3589 3592
3590 if (_zeroFlag) 3593 // update velocities and aceleration
3594 if (!(_zeroFlag && lastZeroFlag))
3591 { 3595 {
3592 m_lastposition = _position; 3596 d.Vector3 vel = d.BodyGetLinearVel(Body);
3593 m_lastorientation = _orientation;
3594 3597
3595 _velocity = Vector3.Zero; 3598 _acceleration = _velocity;
3596 _acceleration = Vector3.Zero;
3597 m_rotationalVelocity = Vector3.Zero;
3598 3599
3599 if (!m_lastUpdateSent) 3600 if ((Math.Abs(vel.X) < 0.001f) &&
3601 (Math.Abs(vel.Y) < 0.001f) &&
3602 (Math.Abs(vel.Z) < 0.001f))
3600 { 3603 {
3601 m_throttleUpdates = false; 3604 _velocity = Vector3.Zero;
3602 throttleCounter = 0; 3605 float t = -m_invTimeStep;
3603 3606 _acceleration = _acceleration * t;
3604 base.RequestPhysicsterseUpdate();
3605
3606 m_lastUpdateSent = true;
3607 } 3607 }
3608 } 3608 else
3609 else
3610 {
3611 if (lastZeroFlag != _zeroFlag)
3612 { 3609 {
3613 base.RequestPhysicsterseUpdate(); 3610 _velocity.X = vel.X;
3611 _velocity.Y = vel.Y;
3612 _velocity.Z = vel.Z;
3613 _acceleration = (_velocity - _acceleration) * m_invTimeStep;
3614 } 3614 }
3615 3615
3616 m_lastUpdateSent = false; 3616 if ((Math.Abs(_acceleration.X) < 0.01f) &&
3617 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) 3617 (Math.Abs(_acceleration.Y) < 0.01f) &&
3618 (Math.Abs(_acceleration.Z) < 0.01f))
3618 { 3619 {
3619 m_lastposition = _position; 3620 _acceleration = Vector3.Zero;
3620 m_lastorientation = _orientation; 3621 }
3621 m_lastVelocity = _velocity; 3622
3622 base.RequestPhysicsterseUpdate(); 3623 if ((Math.Abs(_orientation.X - ori.X) < 0.0001) &&
3624 (Math.Abs(_orientation.Y - ori.Y) < 0.0001) &&
3625 (Math.Abs(_orientation.Z - ori.Z) < 0.0001)
3626 )
3627 {
3628 m_rotationalVelocity = Vector3.Zero;
3623 } 3629 }
3624 else 3630 else
3625 { 3631 {
3626 throttleCounter++; 3632 vel = d.BodyGetAngularVel(Body);
3633 m_rotationalVelocity.X = vel.X;
3634 m_rotationalVelocity.Y = vel.Y;
3635 m_rotationalVelocity.Z = vel.Z;
3627 } 3636 }
3628 } 3637 }
3629 }
3630 else if (!m_lastUpdateSent || !_zeroFlag)
3631 {
3632 // Not a body.. so Make sure the client isn't interpolating
3633 _velocity = Vector3.Zero;
3634 _acceleration = Vector3.Zero;
3635 m_rotationalVelocity = Vector3.Zero;
3636 m_lastVelocity = Vector3.Zero;
3637
3638 _zeroFlag = true;
3639 3638
3640 if (!m_lastUpdateSent) 3639 if (_zeroFlag)
3641 { 3640 {
3642 m_throttleUpdates = false; 3641 if (lastZeroFlag)
3643 throttleCounter = 0; 3642 {
3644 3643 _velocity = Vector3.Zero;
3645 base.RequestPhysicsterseUpdate(); 3644 _acceleration = Vector3.Zero;
3645 m_rotationalVelocity = Vector3.Zero;
3646 }
3646 3647
3647 m_lastUpdateSent = true; 3648 if (!m_lastUpdateSent)
3649 {
3650 base.RequestPhysicsterseUpdate();
3651 if (lastZeroFlag)
3652 m_lastUpdateSent = true;
3653 }
3654 return;
3648 } 3655 }
3656
3657 _position.X = lpos.X;
3658 _position.Y = lpos.Y;
3659 _position.Z = lpos.Z;
3660
3661 _orientation.X = ori.X;
3662 _orientation.Y = ori.Y;
3663 _orientation.Z = ori.Z;
3664 _orientation.W = ori.W;
3665 base.RequestPhysicsterseUpdate();
3666 m_lastUpdateSent = false;
3649 } 3667 }
3650 } 3668 }
3651 } 3669 }
@@ -3803,11 +3821,11 @@ namespace OpenSim.Region.Physics.OdePlugin
3803 break; 3821 break;
3804 3822
3805 case changes.AddForce: 3823 case changes.AddForce:
3806 changeAddForce((Vector3)arg); 3824 changeAddImpulse((Vector3)arg);
3807 break; 3825 break;
3808 3826
3809 case changes.AddAngForce: 3827 case changes.AddAngForce:
3810 changeAddAngularForce((Vector3)arg); 3828 changeAddAngularImpulse((Vector3)arg);
3811 break; 3829 break;
3812 3830
3813 case changes.AngLock: 3831 case changes.AngLock:
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
index 9ca2d3f..cf74f14 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -189,9 +189,12 @@ namespace OpenSim.Region.Physics.OdePlugin
189 private const uint m_regionHeight = Constants.RegionSize; 189 private const uint m_regionHeight = Constants.RegionSize;
190 190
191 public float ODE_STEPSIZE = 0.020f; 191 public float ODE_STEPSIZE = 0.020f;
192 public float HalfOdeStep = 0.01f;
192 private float metersInSpace = 25.6f; 193 private float metersInSpace = 25.6f;
193 private float m_timeDilation = 1.0f; 194 private float m_timeDilation = 1.0f;
194 195
196 DateTime m_lastframe;
197
195 public float gravityx = 0f; 198 public float gravityx = 0f;
196 public float gravityy = 0f; 199 public float gravityy = 0f;
197 public float gravityz = -9.8f; 200 public float gravityz = -9.8f;
@@ -485,6 +488,8 @@ namespace OpenSim.Region.Physics.OdePlugin
485 } 488 }
486 } 489 }
487 490
491 HalfOdeStep = ODE_STEPSIZE * 0.5f;
492
488 ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); 493 ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
489 GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); 494 GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf);
490 495
@@ -521,7 +526,7 @@ namespace OpenSim.Region.Physics.OdePlugin
521 d.WorldSetAngularDamping(world, 0.001f); 526 d.WorldSetAngularDamping(world, 0.001f);
522 d.WorldSetAngularDampingThreshold(world, 0f); 527 d.WorldSetAngularDampingThreshold(world, 0f);
523 d.WorldSetLinearDampingThreshold(world, 0f); 528 d.WorldSetLinearDampingThreshold(world, 0f);
524 d.WorldSetMaxAngularSpeed(world, 256f); 529 d.WorldSetMaxAngularSpeed(world, 100f);
525 530
526 d.WorldSetCFM(world,1e-6f); // a bit harder than default 531 d.WorldSetCFM(world,1e-6f); // a bit harder than default
527 //d.WorldSetCFM(world, 1e-4f); // a bit harder than default 532 //d.WorldSetCFM(world, 1e-4f); // a bit harder than default
@@ -564,6 +569,7 @@ namespace OpenSim.Region.Physics.OdePlugin
564 // let this now be real maximum values 569 // let this now be real maximum values
565 spaceGridMaxX--; 570 spaceGridMaxX--;
566 spaceGridMaxY--; 571 spaceGridMaxY--;
572 m_lastframe = DateTime.UtcNow;
567 } 573 }
568 574
569 internal void waitForSpaceUnlock(IntPtr space) 575 internal void waitForSpaceUnlock(IntPtr space)
@@ -1685,35 +1691,30 @@ namespace OpenSim.Region.Physics.OdePlugin
1685 /// <returns></returns> 1691 /// <returns></returns>
1686 public override float Simulate(float timeStep) 1692 public override float Simulate(float timeStep)
1687 { 1693 {
1688 int statstart;
1689 int statchanges = 0;
1690 int statchmove = 0;
1691 int statactmove = 0;
1692 int statray = 0;
1693 int statcol = 0;
1694 int statstep = 0;
1695 int statmovchar = 0;
1696 int statmovprim;
1697 int totjcontact = 0;
1698 1694
1695 DateTime now = DateTime.UtcNow;
1696 TimeSpan SinceLastFrame = now - m_lastframe;
1697 m_lastframe = now;
1698 timeStep = (float)SinceLastFrame.TotalSeconds;
1699
1699 // acumulate time so we can reduce error 1700 // acumulate time so we can reduce error
1700 step_time += timeStep; 1701 step_time += timeStep;
1701 1702
1702 if (step_time < ODE_STEPSIZE) 1703 if (step_time < HalfOdeStep)
1703 return 0; 1704 return 0;
1704 1705
1705 if (framecount >= int.MaxValue) 1706 if (framecount < 0)
1706 framecount = 0; 1707 framecount = 0;
1707 1708
1708 framecount++; 1709 framecount++;
1709 1710
1710 int curphysiteractions = m_physicsiterations; 1711 int curphysiteractions;
1711 1712
1713 // if in trouble reduce step resolution
1712 if (step_time >= m_SkipFramesAtms) 1714 if (step_time >= m_SkipFramesAtms)
1713 { 1715 curphysiteractions = m_physicsiterations / 2;
1714 // if in trouble reduce step resolution 1716 else
1715 curphysiteractions /= 2; 1717 curphysiteractions = m_physicsiterations;
1716 }
1717 1718
1718 int nodeframes = 0; 1719 int nodeframes = 0;
1719 1720
@@ -1733,13 +1734,10 @@ namespace OpenSim.Region.Physics.OdePlugin
1733 base.TriggerPhysicsBasedRestart(); 1734 base.TriggerPhysicsBasedRestart();
1734 } 1735 }
1735 1736
1736 1737 while (step_time >= HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever
1737 while (step_time >= ODE_STEPSIZE && nodeframes < 10) //limit number of steps so we don't say here for ever
1738 { 1738 {
1739 try 1739 try
1740 { 1740 {
1741 statstart = Util.EnvironmentTickCount();
1742
1743 // clear pointer/counter to contacts to pass into joints 1741 // clear pointer/counter to contacts to pass into joints
1744 m_global_contactcount = 0; 1742 m_global_contactcount = 0;
1745 1743
@@ -1778,17 +1776,39 @@ namespace OpenSim.Region.Physics.OdePlugin
1778 1776
1779 } 1777 }
1780 1778
1781 statchanges += Util.EnvironmentTickCountSubtract(statstart); 1779 // Move characters
1780 lock (_characters)
1781 {
1782 List<OdeCharacter> defects = new List<OdeCharacter>();
1783 foreach (OdeCharacter actor in _characters)
1784 {
1785 if (actor != null)
1786 actor.Move(ODE_STEPSIZE, defects);
1787 }
1788 if (defects.Count != 0)
1789 {
1790 foreach (OdeCharacter defect in defects)
1791 {
1792 RemoveCharacter(defect);
1793 }
1794 }
1795 }
1796
1797 // Move other active objects
1798 lock (_activegroups)
1799 {
1800 foreach (OdePrim aprim in _activegroups)
1801 {
1802 aprim.Move();
1803 }
1804 }
1782 1805
1783 statactmove += Util.EnvironmentTickCountSubtract(statstart);
1784 //if ((framecount % m_randomizeWater) == 0) 1806 //if ((framecount % m_randomizeWater) == 0)
1785 // randomizeWater(waterlevel); 1807 // randomizeWater(waterlevel);
1786 1808
1787 m_rayCastManager.ProcessQueuedRequests(); 1809 m_rayCastManager.ProcessQueuedRequests();
1788 1810
1789 statray += Util.EnvironmentTickCountSubtract(statstart);
1790 collision_optimized(); 1811 collision_optimized();
1791 statcol += Util.EnvironmentTickCountSubtract(statstart);
1792 1812
1793 lock (_collisionEventPrim) 1813 lock (_collisionEventPrim)
1794 { 1814 {
@@ -1813,38 +1833,39 @@ namespace OpenSim.Region.Physics.OdePlugin
1813 } 1833 }
1814 } 1834 }
1815 1835
1836 // do a ode simulation step
1816 d.WorldQuickStep(world, ODE_STEPSIZE); 1837 d.WorldQuickStep(world, ODE_STEPSIZE);
1817 statstep += Util.EnvironmentTickCountSubtract(statstart); 1838 d.JointGroupEmpty(contactgroup);
1839
1840 // update managed ideia of physical data and do updates to core
1841 /*
1842 lock (_characters)
1843 {
1844 foreach (OdeCharacter actor in _characters)
1845 {
1846 if (actor != null)
1847 {
1848 if (actor.bad)
1849 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
1850
1851 actor.UpdatePositionAndVelocity();
1852 }
1853 }
1854 }
1855 */
1818 1856
1819 // Move characters 1857 lock (_activegroups)
1820 lock (_characters)
1821 { 1858 {
1822 List<OdeCharacter> defects = new List<OdeCharacter>();
1823 foreach (OdeCharacter actor in _characters)
1824 {
1825 if (actor != null)
1826 actor.Move(ODE_STEPSIZE, defects);
1827 }
1828 if (defects.Count != 0)
1829 { 1859 {
1830 foreach (OdeCharacter defect in defects) 1860 foreach (OdePrim actor in _activegroups)
1831 { 1861 {
1832 RemoveCharacter(defect); 1862 if (actor.IsPhysical)
1863 {
1864 actor.UpdatePositionAndVelocity();
1865 }
1833 } 1866 }
1834 } 1867 }
1835 } 1868 }
1836 statchmove += Util.EnvironmentTickCountSubtract(statstart);
1837
1838 // Move other active objects
1839 lock (_activegroups)
1840 {
1841 foreach (OdePrim aprim in _activegroups)
1842 {
1843 aprim.Move();
1844 }
1845 }
1846
1847 //ode.dunlock(world);
1848 } 1869 }
1849 catch (Exception e) 1870 catch (Exception e)
1850 { 1871 {
@@ -1852,32 +1873,11 @@ namespace OpenSim.Region.Physics.OdePlugin
1852// ode.dunlock(world); 1873// ode.dunlock(world);
1853 } 1874 }
1854 1875
1855 d.JointGroupEmpty(contactgroup);
1856 totjcontact += m_global_contactcount;
1857 1876
1858 step_time -= ODE_STEPSIZE; 1877 step_time -= ODE_STEPSIZE;
1859 nodeframes++; 1878 nodeframes++;
1860 } 1879 }
1861 1880
1862 statstart = Util.EnvironmentTickCount();
1863
1864/*
1865// now included in characters move() and done at ode rate
1866// maybe be needed later if we need to do any extra work at hearbeat rate
1867 lock (_characters)
1868 {
1869 foreach (OdeCharacter actor in _characters)
1870 {
1871 if (actor != null)
1872 {
1873 if (actor.bad)
1874 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
1875
1876 actor.UpdatePositionAndVelocity();
1877 }
1878 }
1879 }
1880*/
1881 lock (_badCharacter) 1881 lock (_badCharacter)
1882 { 1882 {
1883 if (_badCharacter.Count > 0) 1883 if (_badCharacter.Count > 0)
@@ -1890,22 +1890,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1890 _badCharacter.Clear(); 1890 _badCharacter.Clear();
1891 } 1891 }
1892 } 1892 }
1893 statmovchar = Util.EnvironmentTickCountSubtract(statstart);
1894
1895 lock (_activegroups)
1896 {
1897 {
1898 foreach (OdePrim actor in _activegroups)
1899 {
1900 if (actor.IsPhysical)
1901 {
1902 actor.UpdatePositionAndVelocity((float)nodeframes * ODE_STEPSIZE);
1903 }
1904 }
1905 }
1906 }
1907
1908 statmovprim = Util.EnvironmentTickCountSubtract(statstart);
1909 1893
1910 int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); 1894 int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
1911 int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); 1895 int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
@@ -1932,15 +1916,17 @@ namespace OpenSim.Region.Physics.OdePlugin
1932 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); 1916 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
1933 } 1917 }
1934 1918
1935 // think time dilation is not a physics issue alone.. but ok let's fake something 1919 // think time dilation as to do with dinamic step size that we dont' have
1936 if (step_time < ODE_STEPSIZE) // we did the required loops 1920 // even so tell something to world
1921 if (nodeframes < 10) // we did the requested loops
1937 m_timeDilation = 1.0f; 1922 m_timeDilation = 1.0f;
1938 else 1923 else if (step_time > 0)
1939 { // we didn't forget the lost ones and let user know something 1924 {
1940 m_timeDilation = 1 - step_time / timeStep; 1925 m_timeDilation = timeStep / step_time;
1941 if (m_timeDilation < 0) 1926 if (m_timeDilation > 1)
1942 m_timeDilation = 0; 1927 m_timeDilation = 1;
1943 step_time = 0; 1928 if (step_time > m_SkipFramesAtms)
1929 step_time = 0;
1944 } 1930 }
1945 } 1931 }
1946 1932
@@ -2007,7 +1993,7 @@ namespace OpenSim.Region.Physics.OdePlugin
2007 } 1993 }
2008 else // out world use external height 1994 else // out world use external height
2009 { 1995 {
2010 ix = regsize - 1; 1996 ix = regsize - 2;
2011 dx = 0; 1997 dx = 0;
2012 } 1998 }
2013 if (y < regsize - 1) 1999 if (y < regsize - 1)
@@ -2017,7 +2003,7 @@ namespace OpenSim.Region.Physics.OdePlugin
2017 } 2003 }
2018 else 2004 else
2019 { 2005 {
2020 iy = regsize - 1; 2006 iy = regsize - 2;
2021 dy = 0; 2007 dy = 0;
2022 } 2008 }
2023 } 2009 }
@@ -2034,7 +2020,7 @@ namespace OpenSim.Region.Physics.OdePlugin
2034 } 2020 }
2035 else // out world use external height 2021 else // out world use external height
2036 { 2022 {
2037 iy = regsize - 1; 2023 iy = regsize - 2;
2038 dy = 0; 2024 dy = 0;
2039 } 2025 }
2040 if (y < regsize - 1) 2026 if (y < regsize - 1)
@@ -2044,7 +2030,7 @@ namespace OpenSim.Region.Physics.OdePlugin
2044 } 2030 }
2045 else 2031 else
2046 { 2032 {
2047 ix = regsize - 1; 2033 ix = regsize - 2;
2048 dx = 0; 2034 dx = 0;
2049 } 2035 }
2050 } 2036 }
@@ -2057,18 +2043,35 @@ namespace OpenSim.Region.Physics.OdePlugin
2057 iy += ix; // all indexes have iy + ix 2043 iy += ix; // all indexes have iy + ix
2058 2044
2059 float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; 2045 float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
2046 /*
2047 if ((dx + dy) <= 1.0f)
2048 {
2049 h0 = ((float)heights[iy]); // 0,0 vertice
2050 h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
2051 h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
2052 }
2053 else
2054 {
2055 h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
2056 h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
2057 h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
2058 }
2059 */
2060 h0 = ((float)heights[iy]); // 0,0 vertice
2060 2061
2061 if ((dx + dy) <= 1.0f) 2062 if ((dy > dx))
2062 { 2063 {
2063 h0 = ((float)heights[iy]); // 0,0 vertice 2064 iy += regsize;
2064 h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 2065 h2 = (float)heights[iy]; // 0,1 vertice
2065 h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 2066 h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0
2067 h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1
2066 } 2068 }
2067 else 2069 else
2068 { 2070 {
2069 h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice 2071 iy++;
2070 h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 2072 h2 = (float)heights[iy]; // vertice 1,0
2071 h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 2073 h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0
2074 h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0
2072 } 2075 }
2073 2076
2074 return h0 + h1 + h2; 2077 return h0 + h1 + h2;
generated by cgit v1.1 at 2024-12-23 22:21:38 +0000