* Added a routine to check if a PhysicsVector and Quaternion is finite

* Now validating input to the Physics scene and warning when something is awry.
* This should help nail down that Non Finite Avatar Position Detected issue.

3 files changed, 193 insertions, 45 deletions

diff --git a/OpenSim/Region/Physics/Manager/PhysicsVector.cs b/OpenSim/Region/Physics/Manager/PhysicsVector.cs
index bbd6464..2a4ac5e 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsVector.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsVector.cs
@@ -153,6 +153,20 @@ namespace OpenSim.Region.Physics.Manager
             return v*f;
         }
 
+        public static bool isFinite(PhysicsVector v)
+        {
+            if (v == null)
+                return false;
+            if (Single.IsInfinity(v.X) || Single.IsNaN(v.X))
+                return false;
+            if (Single.IsInfinity(v.Y) || Single.IsNaN(v.Y))
+                return false;
+            if (Single.IsInfinity(v.Z) || Single.IsNaN(v.Z))
+                return false;
+
+            return true;
+        }
+
         public virtual bool IsIdentical(PhysicsVector v, float tolerance)
         {
             PhysicsVector diff = this - v;
diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index bf8ca0e..c37b632 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -380,10 +380,23 @@ namespace OpenSim.Region.Physics.OdePlugin
             set
             {
                 if (Body == IntPtr.Zero || Shell == IntPtr.Zero)
-                    _position.X = value.X; _position.Y = value.Y; _position.Z = value.Z;
-                  
-                m_taintPosition.X = value.X; m_taintPosition.Y = value.Y; m_taintPosition.Z = value.Z;
-                _parent_scene.AddPhysicsActorTaint(this);
+                {
+                    if (PhysicsVector.isFinite(value))
+                    {
+                        _position.X = value.X;
+                        _position.Y = value.Y;
+                        _position.Z = value.Z;
+
+                        m_taintPosition.X = value.X;
+                        m_taintPosition.Y = value.Y;
+                        m_taintPosition.Z = value.Z;
+                        _parent_scene.AddPhysicsActorTaint(this);
+                    }
+                    else
+                    {
+                        m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
+                    }
+                }
             }
         }
 
@@ -402,15 +415,22 @@ namespace OpenSim.Region.Physics.OdePlugin
             get { return new PhysicsVector(CAPSULE_RADIUS*2, CAPSULE_RADIUS*2, CAPSULE_LENGTH); }
             set
             {
-                m_pidControllerActive = true;
-               
+                if (PhysicsVector.isFinite(value))
+                {
+                    m_pidControllerActive = true;
+
                     PhysicsVector SetSize = value;
-                    m_tainted_CAPSULE_LENGTH = (SetSize.Z * 1.15f) - CAPSULE_RADIUS * 2.0f;
+                    m_tainted_CAPSULE_LENGTH = (SetSize.Z*1.15f) - CAPSULE_RADIUS*2.0f;
                     //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
 
                     Velocity = new PhysicsVector(0f, 0f, 0f);
-               
-                _parent_scene.AddPhysicsActorTaint(this);
+
+                    _parent_scene.AddPhysicsActorTaint(this);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
+                }
             }
         }
         
@@ -616,8 +636,15 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
             set
             {
-                m_pidControllerActive = true;
-                _target_velocity = value;
+                if (PhysicsVector.isFinite(value))
+                {
+                    m_pidControllerActive = true;
+                    _target_velocity = value;
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
+                }
             }
         }
 
@@ -667,22 +694,29 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// <param name="force"></param>
         public override void AddForce(PhysicsVector force, bool pushforce)
         {
-            if (pushforce)
+            if (PhysicsVector.isFinite(force))
             {
-                m_pidControllerActive = false;
-                force *= 100f;
-                doForce(force);
-                //m_log.Debug("Push!");
-                //_target_velocity.X += force.X;
-               // _target_velocity.Y += force.Y;
-                //_target_velocity.Z += force.Z;
+                if (pushforce)
+                {
+                    m_pidControllerActive = false;
+                    force *= 100f;
+                    doForce(force);
+                    //m_log.Debug("Push!");
+                    //_target_velocity.X += force.X;
+                    // _target_velocity.Y += force.Y;
+                    //_target_velocity.Z += force.Z;
+                }
+                else
+                {
+                    m_pidControllerActive = true;
+                    _target_velocity.X += force.X;
+                    _target_velocity.Y += force.Y;
+                    _target_velocity.Z += force.Z;
+                }
             }
             else
             {
-                m_pidControllerActive = true;
-                _target_velocity.X += force.X;
-                _target_velocity.Y += force.Y;
-                _target_velocity.Z += force.Z;
+                m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
             }
             //m_lastUpdateSent = false;
         }
@@ -847,8 +881,14 @@ namespace OpenSim.Region.Physics.OdePlugin
                 // end add Kitto Flora
 
             }
-
-            doForce(vec);
+            if (PhysicsVector.isFinite(vec))
+            {
+                doForce(vec);
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
+            }
         }
 
         /// <summary>
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 56bd289..a55ae76 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -2325,7 +2325,17 @@ namespace OpenSim.Region.Physics.OdePlugin
         public override PhysicsVector Size
         {
             get { return _size; }
-            set { _size = value; }
+            set
+            {
+                if (PhysicsVector.isFinite(value))
+                {
+                    _size = value;
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got NaN Size on object");
+                }
+            }
         }
 
         public override float Mass
@@ -2337,7 +2347,17 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
             //get { return PhysicsVector.Zero; }
             get { return m_force; }
-            set { m_force = value; }
+            set
+            {
+                if (PhysicsVector.isFinite(value))
+                {
+                    m_force = value;
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object");
+                }
+            }
         }
 
         public override int VehicleType
@@ -2402,10 +2422,18 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
             set
             {
-                _velocity = value;
+                if (PhysicsVector.isFinite(value))
+                {
+                    _velocity = value;
+
+                    m_taintVelocity = value;
+                    _parent_scene.AddPhysicsActorTaint(this);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got NaN Velocity in Object");
+                }
 
-                m_taintVelocity = value;
-                _parent_scene.AddPhysicsActorTaint(this);
             }
         }
 
@@ -2421,8 +2449,15 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             set
             {
-                m_taintTorque = value;
-                _parent_scene.AddPhysicsActorTaint(this);
+                if (PhysicsVector.isFinite(value))
+                {
+                    m_taintTorque = value;
+                    _parent_scene.AddPhysicsActorTaint(this);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got NaN Torque in Object");
+                }
             }
         }
 
@@ -2441,7 +2476,27 @@ namespace OpenSim.Region.Physics.OdePlugin
         public override Quaternion Orientation
         {
             get { return _orientation; }
-            set { _orientation = value; }
+            set
+            {
+                if (QuaternionIsFinite(value))
+                    _orientation = value;
+                else
+                    m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
+
+            }
+        }
+
+        internal static bool QuaternionIsFinite(Quaternion q)
+        {
+            if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
+                return false;
+            if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
+                return false;
+            if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
+                return false;
+            if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
+                return false;
+            return true;
         }
 
         public override PhysicsVector Acceleration
@@ -2457,15 +2512,29 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public override void AddForce(PhysicsVector force, bool pushforce)
         {
-            m_forcelist.Add(force);
-            m_taintforce = true;
+            if (PhysicsVector.isFinite(force))
+            {
+                m_forcelist.Add(force);
+                m_taintforce = true;
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
+            }
             //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() +  " to prim at " + Position.ToString());
         }
 
         public override void AddAngularForce(PhysicsVector force, bool pushforce)
         {
-            m_angularforcelist.Add(force);
-            m_taintaddangularforce = true;
+            if (PhysicsVector.isFinite(force))
+            {
+                m_angularforcelist.Add(force);
+                m_taintaddangularforce = true;
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
+            }
         }
 
         public override PhysicsVector RotationalVelocity
@@ -2482,7 +2551,17 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                 return m_rotationalVelocity;
             }
-            set { m_rotationalVelocity = value; }
+            set
+            {
+                if (PhysicsVector.isFinite(value))
+                {
+                    m_rotationalVelocity = value;
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");
+                }
+            }
         }
 
         public override void CrossingFailure()
@@ -2518,12 +2597,18 @@ namespace OpenSim.Region.Physics.OdePlugin
         public override void LockAngularMotion(PhysicsVector axis)
         {
             // reverse the zero/non zero values for ODE.
-
-            axis.X = (axis.X > 0) ? 1f : 0f;
-            axis.Y = (axis.Y > 0) ? 1f : 0f;
-            axis.Z = (axis.Z > 0) ? 1f : 0f;
-            m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
-            m_taintAngularLock = new PhysicsVector(axis.X, axis.Y, axis.Z); ;
+            if (PhysicsVector.isFinite(axis))
+            {
+                axis.X = (axis.X > 0) ? 1f : 0f;
+                axis.Y = (axis.Y > 0) ? 1f : 0f;
+                axis.Z = (axis.Z > 0) ? 1f : 0f;
+                m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
+                m_taintAngularLock = new PhysicsVector(axis.X, axis.Y, axis.Z);
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
+            }
         }
 
         public void UpdatePositionAndVelocity()
@@ -2734,7 +2819,16 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
         }
 
-        public override PhysicsVector PIDTarget { set { m_PIDTarget = value; ; } }
+        public override PhysicsVector PIDTarget 
+        { 
+            set
+            {
+                if (PhysicsVector.isFinite(value))
+                    m_PIDTarget = value; 
+                else 
+                    m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
+            } 
+        }
         public override bool PIDActive { set { m_usePID = value; } }
         public override float PIDTau { set { m_PIDTau = value; } }