diff options
Diffstat (limited to '')
-rw-r--r-- | OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 23 | ||||
-rw-r--r-- | OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 746 | ||||
-rw-r--r-- | OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 215 |
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; |