mantis 8420: ubOde awake up lazy body

2 files changed, 73 insertions, 57 deletions

diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
index 4e18522..842bc0b 100644
--- a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
@@ -3380,7 +3380,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                         {
                             SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
                             SafeNativeMethods.BodyEnable(Body);
-                        }                            
+                        }
                     }
                 }
                 m_collisionscore = 0;
@@ -3504,6 +3504,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                 return;
 
             m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value);
+            if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
+            {
+                SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                SafeNativeMethods.BodyEnable(Body);
+            }
         }
 
         private void changeVehicleVectorParam(strVehicleVectorParam vp)
@@ -3511,6 +3516,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             if (m_vehicle == null)
                 return;
             m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);
+            if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
+            {
+                SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                SafeNativeMethods.BodyEnable(Body);
+            }
         }
 
         private void changeVehicleRotationParam(strVehicleQuatParam qp)
@@ -3518,6 +3528,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             if (m_vehicle == null)
                 return;
             m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value);
+            if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
+            {
+                SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                SafeNativeMethods.BodyEnable(Body);
+            }
         }
 
         private void changeVehicleFlags(strVehicleBoolParam bp)
@@ -3525,48 +3540,93 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             if (m_vehicle == null)
                 return;
             m_vehicle.ProcessVehicleFlags(bp.param, bp.value);
+            if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
+            {
+                SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                SafeNativeMethods.BodyEnable(Body);
+            }
         }
 
         private void changeBuoyancy(float b)
         {
             m_buoyancy = b;
+            if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
+            {
+                SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                SafeNativeMethods.BodyEnable(Body);
+            }
         }
 
         private void changePIDTarget(Vector3 trg)
         {
             m_PIDTarget = trg;
+            if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
+            {
+                SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                SafeNativeMethods.BodyEnable(Body);
+            }
         }
 
         private void changePIDTau(float tau)
         {
             m_PIDTau = tau;
+            if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
+            {
+                SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                SafeNativeMethods.BodyEnable(Body);
+            }
         }
 
         private void changePIDActive(bool val)
         {
             m_usePID = val;
+            if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
+            {
+                SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                SafeNativeMethods.BodyEnable(Body);
+            }
         }
 
         private void changePIDHoverHeight(float val)
         {
-          m_PIDHoverHeight = val;
-          if (val == 0)
-            m_useHoverPID = false;
+            m_PIDHoverHeight = val;
+            if (val == 0)
+                m_useHoverPID = false;
+            if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
+            {
+                SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                SafeNativeMethods.BodyEnable(Body);
+            }
         }
 
         private void changePIDHoverType(PIDHoverType type)
         {
             m_PIDHoverType = type;
+            if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
+            {
+                SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                SafeNativeMethods.BodyEnable(Body);
+            }
         }
 
         private void changePIDHoverTau(float tau)
         {
             m_PIDHoverTau = tau;
+            if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
+            {
+                SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                SafeNativeMethods.BodyEnable(Body);
+            }
         }
 
         private void changePIDHoverActive(bool active)
         {
             m_useHoverPID = active;
+            if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
+            {
+                SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                SafeNativeMethods.BodyEnable(Body);
+            }
         }
 
         private void changeInertia(PhysicsInertiaData inertia)
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
index 567de23..c231787 100644
--- a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
@@ -288,10 +288,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
 
         public IConfigSource m_config;
 
-        public bool physics_logging = false;
-        public int physics_logging_interval = 0;
-        public bool physics_logging_append_existing_logfile = false;
-
         public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
 
         private ODERayCastRequestManager m_rayCastManager;
@@ -469,10 +465,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                     geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity);
 //                    bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);
 
-                    physics_logging = physicsconfig.GetBoolean("physics_logging", false);
-                    physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
-                    physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
-
                     minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset);
                     maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject);
 
@@ -633,51 +625,15 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                 return;
             }
 
-            // get geom bodies to check if we already a joint contact
-            // guess this shouldn't happen now
-            IntPtr b1 = SafeNativeMethods.GeomGetBody(g1);
-            IntPtr b2 = SafeNativeMethods.GeomGetBody(g2);
 
-            // d.GeomClassID id = d.GeomGetClass(g1);
 
             // Figure out how many contact points we have
             int count = 0;
             try
             {
-                // Colliding Geom To Geom
-                // This portion of the function 'was' blatantly ripped off from BoxStack.cs
-
                 if (g1 == g2)
                     return; // Can't collide with yourself
 
-//                if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
-//                    return;
-                /*
-                // debug
-                                PhysicsActor dp2;
-                                if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
-                                {
-                                    d.AABB aabb;
-                                    d.GeomGetAABB(g2, out aabb);
-                                    float x = aabb.MaxX - aabb.MinX;
-                                    float y = aabb.MaxY - aabb.MinY;
-                                    float z = aabb.MaxZ - aabb.MinZ;
-                                    if (x > 60.0f || y > 60.0f || z > 60.0f)
-                                    {
-                                        if (!actor_name_map.TryGetValue(g2, out dp2))
-                                            m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
-                                        else
-                                            m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})",
-                                                dp2.Name, dp2.Size, x, y, z,
-                                                dp2.Position.ToString(),
-                                                dp2.Orientation.ToString(),
-                                                dp2.Orientation.Length());
-                                        return;
-                                    }
-                                }
-                //
-                */
-
                 if (SafeNativeMethods.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
                     SafeNativeMethods.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
                 {
@@ -707,29 +663,26 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             if (count == 0)
                 return;
 
+            // get first contact
+            SafeNativeMethods.ContactGeom curContact = new SafeNativeMethods.ContactGeom();
+            if (!GetCurContactGeom(0, ref curContact))
+                return;
+
             // try get physical actors
             PhysicsActor p1;
-            PhysicsActor p2;
-
             if (!actor_name_map.TryGetValue(g1, out p1))
             {
                 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1");
                 return;
             }
 
+            PhysicsActor p2;
             if (!actor_name_map.TryGetValue(g2, out p2))
             {
                 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
                 return;
             }
 
-
-            // get first contact
-            SafeNativeMethods.ContactGeom curContact = new SafeNativeMethods.ContactGeom();
-
-            if (!GetCurContactGeom(0, ref curContact))
-                return;
-
             ContactPoint maxDepthContact = new ContactPoint();
 
             // do volume detection case
@@ -879,6 +832,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             if(dop1ava || dop2ava)
                 smoothMesh = false;
 
+            IntPtr b1 = SafeNativeMethods.GeomGetBody(g1);
+            IntPtr b2 = SafeNativeMethods.GeomGetBody(g2);
+
             while (true)
             {
                 noskip = true;