aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs746
1 files changed, 382 insertions, 364 deletions
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:
generated by cgit v1.1 at 2024-12-21 10:18:33 +0000