5 files changed, 262 insertions, 267 deletions

diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index 71ace16..c86bc62 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -68,15 +68,15 @@ namespace OpenSim.Region.Physics.OdePlugin
     {
         private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
 
-        private PhysicsVector _position;
+        private Vector3 _position;
         private d.Vector3 _zeroPosition;
         // private d.Matrix3 m_StandUpRotation;
         private bool _zeroFlag = false;
         private bool m_lastUpdateSent = false;
-        private PhysicsVector _velocity;
-        private PhysicsVector _target_velocity;
-        private PhysicsVector _acceleration;
-        private PhysicsVector m_rotationalVelocity;
+        private Vector3 _velocity;
+        private Vector3 _target_velocity;
+        private Vector3 _acceleration;
+        private Vector3 m_rotationalVelocity;
         private float m_mass = 80f;
         public float m_density = 60f;
         private bool m_pidControllerActive = true;
@@ -99,7 +99,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private bool m_hackSentFall = false;
         private bool m_hackSentFly = false;
         private int m_requestedUpdateFrequency = 0;
-        private PhysicsVector m_taintPosition = new PhysicsVector(0, 0, 0);
+        private Vector3 m_taintPosition = Vector3.Zero;
         public uint m_localID = 0;
         public bool m_returnCollisions = false;
         // taints and their non-tainted counterparts
@@ -143,22 +143,17 @@ namespace OpenSim.Region.Physics.OdePlugin
         public UUID m_uuid;
         public bool bad = false;
 
-        public OdeCharacter(String avName, OdeScene parent_scene, PhysicsVector pos, CollisionLocker dode, PhysicsVector size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor)
+        public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, CollisionLocker dode, Vector3 size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor)
         {
             m_uuid = UUID.Random();
 
-            // ode = dode;
-            _velocity = new PhysicsVector();
-            _target_velocity = new PhysicsVector();
-
-
-            if (PhysicsVector.isFinite(pos))
+            if (pos.IsFinite())
             {
-                if (pos.Z > 9999999)
+                if (pos.Z > 9999999f)
                 {
                     pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
                 }
-                if (pos.Z < -90000)
+                if (pos.Z < -90000f)
                 {
                     pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
                 }
@@ -169,15 +164,13 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
             else
             {
-                _position = new PhysicsVector(((int)_parent_scene.WorldExtents.X * 0.5f), ((int)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128, 128) + 10);
+                _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
                 m_taintPosition.X = _position.X;
                 m_taintPosition.Y = _position.Y;
                 m_taintPosition.Z = _position.Z;
                 m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
             }
 
-
-            _acceleration = new PhysicsVector();
             _parent_scene = parent_scene;
 
             PID_D = pid_d;
@@ -189,7 +182,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             walkDivisor = walk_divisor;
             runDivisor = rundivisor;
 
-
             // m_StandUpRotation =
             //     new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
             //                   0.5f);
@@ -205,7 +197,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             m_isPhysical = false; // current status: no ODE information exists
             m_tainted_isPhysical = true; // new tainted status: need to create ODE information
 
-            
             _parent_scene.AddPhysicsActorTaint(this);
             
             m_name = avName;
@@ -412,20 +403,20 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// Not really a good choice unless you 'know' it's a good
         /// spot otherwise you're likely to orbit the avatar.
         /// </summary>
-        public override PhysicsVector Position
+        public override Vector3 Position
         {
             get { return _position; }
             set
             {
                 if (Body == IntPtr.Zero || Shell == IntPtr.Zero)
                 {
-                    if (PhysicsVector.isFinite(value))
+                    if (value.IsFinite())
                     {
-                        if (value.Z > 9999999)
+                        if (value.Z > 9999999f)
                         {
                             value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
                         }
-                        if (value.Z < -90000)
+                        if (value.Z < -90000f)
                         {
                             value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
                         }
@@ -447,7 +438,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
-        public override PhysicsVector RotationalVelocity
+        public override Vector3 RotationalVelocity
         {
             get { return m_rotationalVelocity; }
             set { m_rotationalVelocity = value; }
@@ -457,20 +448,20 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// This property sets the height of the avatar only.  We use the height to make sure the avatar stands up straight
         /// and use it to offset landings properly
         /// </summary>
-        public override PhysicsVector Size
+        public override Vector3 Size
         {
-            get { return new PhysicsVector(CAPSULE_RADIUS*2, CAPSULE_RADIUS*2, CAPSULE_LENGTH); }
+            get { return new Vector3(CAPSULE_RADIUS * 2, CAPSULE_RADIUS * 2, CAPSULE_LENGTH); }
             set
             {
-                if (PhysicsVector.isFinite(value))
+                if (value.IsFinite())
                 {
                     m_pidControllerActive = true;
 
-                    PhysicsVector SetSize = value;
+                    Vector3 SetSize = value;
                     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);
+                    Velocity = Vector3.Zero;
 
                     _parent_scene.AddPhysicsActorTaint(this);
                 }
@@ -481,7 +472,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
-        private void AlignAvatarTiltWithCurrentDirectionOfMovement(PhysicsVector movementVector)
+        private void AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3 movementVector)
         {
             movementVector.Z = 0f;
             float magnitude = (float)Math.Sqrt((double)(movementVector.X * movementVector.X + movementVector.Y * movementVector.Y));
@@ -643,7 +634,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 // (with -0..0 motor stops) falls into the terrain for reasons yet
                 // to be comprehended in their entirety.
                 #endregion
-                AlignAvatarTiltWithCurrentDirectionOfMovement(new PhysicsVector(0,0,0));
+                AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3.Zero);
                 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0.08f);
                 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
                 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0.08f);
@@ -688,7 +679,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         }
 
-        public override void LockAngularMotion(PhysicsVector axis)
+        public override void LockAngularMotion(Vector3 axis)
         {
 
         }
@@ -716,9 +707,9 @@ namespace OpenSim.Region.Physics.OdePlugin
 //             //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
 //         }
 
-        public override PhysicsVector Force
+        public override Vector3 Force
         {
-            get { return new PhysicsVector(_target_velocity.X, _target_velocity.Y, _target_velocity.Z); }
+            get { return _target_velocity; }
             set { return; }
         }
 
@@ -733,7 +724,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         }
 
-        public override void VehicleVectorParam(int param, PhysicsVector value)
+        public override void VehicleVectorParam(int param, Vector3 value)
         {
 
         }
@@ -748,14 +739,14 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         }
 
-        public override PhysicsVector CenterOfMass
+        public override Vector3 CenterOfMass
         {
-            get { return PhysicsVector.Zero; }
+            get { return Vector3.Zero; }
         }
 
-        public override PhysicsVector GeometricCenter
+        public override Vector3 GeometricCenter
         {
-            get { return PhysicsVector.Zero; }
+            get { return Vector3.Zero; }
         }
 
         public override PrimitiveBaseShape Shape
@@ -763,18 +754,18 @@ namespace OpenSim.Region.Physics.OdePlugin
             set { return; }
         }
 
-        public override PhysicsVector Velocity
+        public override Vector3 Velocity
         {
             get {
-                // There's a problem with PhysicsVector.Zero! Don't Use it Here!
+                // There's a problem with Vector3.Zero! Don't Use it Here!
                 if (_zeroFlag)
-                    return new PhysicsVector(0f, 0f, 0f);
+                    return Vector3.Zero;
                 m_lastUpdateSent = false;
                 return _velocity;
             }
             set
             {
-                if (PhysicsVector.isFinite(value))
+                if (value.IsFinite())
                 {
                     m_pidControllerActive = true;
                     _target_velocity = value;
@@ -786,9 +777,9 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
-        public override PhysicsVector Torque
+        public override Vector3 Torque
         {
-            get { return PhysicsVector.Zero; }
+            get { return Vector3.Zero; }
             set { return; }
         }
 
@@ -814,12 +805,12 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
-        public override PhysicsVector Acceleration
+        public override Vector3 Acceleration
         {
             get { return _acceleration; }
         }
 
-        public void SetAcceleration(PhysicsVector accel)
+        public void SetAcceleration(Vector3 accel)
         {
             m_pidControllerActive = true;
             _acceleration = accel;
@@ -830,9 +821,9 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// The PID controller takes this target velocity and tries to make it a reality
         /// </summary>
         /// <param name="force"></param>
-        public override void AddForce(PhysicsVector force, bool pushforce)
+        public override void AddForce(Vector3 force, bool pushforce)
         {
-            if (PhysicsVector.isFinite(force))
+            if (force.IsFinite())
             {
                 if (pushforce)
                 {
@@ -861,7 +852,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             //m_lastUpdateSent = false;
         }
 
-        public override void AddAngularForce(PhysicsVector force, bool pushforce)
+        public override void AddAngularForce(Vector3 force, bool pushforce)
         {
 
         }
@@ -870,7 +861,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// After all of the forces add up with 'add force' we apply them with doForce
         /// </summary>
         /// <param name="force"></param>
-        public void doForce(PhysicsVector force)
+        public void doForce(Vector3 force)
         {
             if (!collidelock)
             {
@@ -881,7 +872,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
-        public override void SetMomentum(PhysicsVector momentum)
+        public override void SetMomentum(Vector3 momentum)
         {
         }
 
@@ -908,9 +899,9 @@ namespace OpenSim.Region.Physics.OdePlugin
             //PidStatus = true;
 
             d.Vector3 localpos = d.BodyGetPosition(Body);
-            PhysicsVector localPos = new PhysicsVector(localpos.X, localpos.Y, localpos.Z);
+            Vector3 localPos = new Vector3(localpos.X, localpos.Y, localpos.Z);
             
-            if (!PhysicsVector.isFinite(localPos))
+            if (!localPos.IsFinite())
             {
 
                 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
@@ -946,7 +937,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 return;
             }
 
-            PhysicsVector vec = new PhysicsVector();
+            Vector3 vec = Vector3.Zero;
             d.Vector3 vel = d.BodyGetLinearVel(Body);
 
             float movementdivisor = 1f;
@@ -1059,12 +1050,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
                 // end add Kitto Flora
             }
-            if (PhysicsVector.isFinite(vec))
+            if (vec.IsFinite())
             {
                 doForce(vec);
                 if (!_zeroFlag)
                 {
-                  AlignAvatarTiltWithCurrentDirectionOfMovement(new PhysicsVector(vec.X, vec.Y, vec.Z));
+                  AlignAvatarTiltWithCurrentDirectionOfMovement(vec);
                 }
             }
             else
@@ -1197,7 +1188,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
         }
 
-        public override PhysicsVector PIDTarget { set { return; } }
+        public override Vector3 PIDTarget { set { return; } }
         public override bool PIDActive { set { return; } }
         public override float PIDTau { set { return; } }
 
@@ -1311,7 +1302,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     d.GeomDestroy(Shell);
                     AvatarGeomAndBodyCreation(_position.X, _position.Y,
                                       _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor);
-                    Velocity = new PhysicsVector(0f, 0f, 0f);
+                    Velocity = Vector3.Zero;
 
                     _parent_scene.geom_name_map[Shell] = m_name;
                     _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
@@ -1325,7 +1316,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
             }
 
-            if (!m_taintPosition.IsIdentical(_position, 0.05f))
+            if (!m_taintPosition.ApproxEquals(_position, 0.05f))
             {
                 if (Body != IntPtr.Zero)
                 {
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs
index 467eba0..4a802cd 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs
@@ -1,16 +1,4 @@
 /*
- * Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
- * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
- * ODEPrim.cs contains methods dealing with Prim editing, Prim
- * characteristics and Kinetic motion.
- * ODEDynamics.cs contains methods dealing with Prim Physical motion
- * (dynamics) and the associated settings. Old Linear and angular
- * motors for dynamic motion have been replace with  MoveLinear()
- * and MoveAngular(); 'Physical' is used only to switch ODE dynamic 
- * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
- * switch between 'VEHICLE' parameter use and general dynamics
- * settings use.
- * 
  * Copyright (c) Contributors, http://opensimulator.org/
  * See CONTRIBUTORS.TXT for a full list of copyright holders.
  *
@@ -37,6 +25,19 @@
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
+/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
+ * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
+ * ODEPrim.cs contains methods dealing with Prim editing, Prim
+ * characteristics and Kinetic motion.
+ * ODEDynamics.cs contains methods dealing with Prim Physical motion
+ * (dynamics) and the associated settings. Old Linear and angular
+ * motors for dynamic motion have been replace with  MoveLinear()
+ * and MoveAngular(); 'Physical' is used only to switch ODE dynamic 
+ * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
+ * switch between 'VEHICLE' parameter use and general dynamics
+ * settings use.
+ */ 
+
 using System;
 using System.Collections.Generic;
 using System.Reflection;
@@ -231,7 +232,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             
         }//end ProcessFloatVehicleParam
 
-        internal void ProcessVectorVehicleParam(Vehicle pParam, PhysicsVector pValue)
+        internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
         {
             switch (pParam)
             {
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index f59f0ae..5ff9d32 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -1,18 +1,5 @@
-/*
- * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
- * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
- * ODEPrim.cs contains methods dealing with Prim editing, Prim
- * characteristics and Kinetic motion.
- * ODEDynamics.cs contains methods dealing with Prim Physical motion
- * (dynamics) and the associated settings. Old Linear and angular
- * motors for dynamic motion have been replace with  MoveLinear()
- * and MoveAngular(); 'Physical' is used only to switch ODE dynamic 
- * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
- * switch between 'VEHICLE' parameter use and general dynamics
- * settings use.
- * Copyright (c) Contributors, http://opensimulator.org/
+/* Copyright (c) Contributors, http://opensimulator.org/
  * See CONTRIBUTORS.TXT for a full list of copyright holders.
- *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
  *     * Redistributions of source code must retain the above copyright
@@ -35,6 +22,20 @@
  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
+
+/*
+ * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
+ * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
+ * ODEPrim.cs contains methods dealing with Prim editing, Prim
+ * characteristics and Kinetic motion.
+ * ODEDynamics.cs contains methods dealing with Prim Physical motion
+ * (dynamics) and the associated settings. Old Linear and angular
+ * motors for dynamic motion have been replace with  MoveLinear()
+ * and MoveAngular(); 'Physical' is used only to switch ODE dynamic 
+ * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
+ * switch between 'VEHICLE' parameter use and general dynamics
+ * settings use.
+ */
 using System;
 using System.Collections.Generic;
 using System.Reflection;
@@ -56,44 +57,43 @@ namespace OpenSim.Region.Physics.OdePlugin
     {
         private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
 
-        private PhysicsVector _position;
-        private PhysicsVector _velocity;
-        private PhysicsVector _torque = new PhysicsVector(0,0,0);
-        private PhysicsVector m_lastVelocity = new PhysicsVector(0.0f, 0.0f, 0.0f);
-        private PhysicsVector m_lastposition = new PhysicsVector(0.0f, 0.0f, 0.0f);
+        private Vector3 _position;
+        private Vector3 _velocity;
+        private Vector3 _torque;
+        private Vector3 m_lastVelocity;
+        private Vector3 m_lastposition;
         private Quaternion m_lastorientation = new Quaternion();
-        private PhysicsVector m_rotationalVelocity;
-        private PhysicsVector _size;
-        private PhysicsVector _acceleration;
+        private Vector3 m_rotationalVelocity;
+        private Vector3 _size;
+        private Vector3 _acceleration;
         // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
         private Quaternion _orientation;
-        private PhysicsVector m_taintposition;
-        private PhysicsVector m_taintsize;
-        private PhysicsVector m_taintVelocity = new PhysicsVector(0, 0, 0);
-        private PhysicsVector m_taintTorque = new PhysicsVector(0, 0, 0);
+        private Vector3 m_taintposition;
+        private Vector3 m_taintsize;
+        private Vector3 m_taintVelocity;
+        private Vector3 m_taintTorque;
         private Quaternion m_taintrot;
-        private PhysicsVector m_angularlock = new PhysicsVector(1f, 1f, 1f);
-        private PhysicsVector m_taintAngularLock = new PhysicsVector(1f, 1f, 1f);
+        private Vector3 m_angularlock = Vector3.One;
+        private Vector3 m_taintAngularLock = Vector3.One;
         private IntPtr Amotor = IntPtr.Zero;
 
-        private PhysicsVector m_PIDTarget = new PhysicsVector(0, 0, 0);
-        // private PhysicsVector m_taintPIDTarget = new PhysicsVector(0, 0, 0);
-        private float m_PIDTau = 0f;
+        private Vector3 m_PIDTarget;
+        private float m_PIDTau;
         private float PID_D = 35f;
         private float PID_G = 25f;
-        private bool m_usePID = false;
+        private bool m_usePID;
 
         // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
         // and are for non-VEHICLES only.
-         
-        private float m_PIDHoverHeight = 0f;
-        private float m_PIDHoverTau = 0f;
-        private bool m_useHoverPID = false;
+
+        private float m_PIDHoverHeight;
+        private float m_PIDHoverTau;
+        private bool m_useHoverPID;
         private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
-        private float m_targetHoverHeight = 0f;
-        private float m_groundHeight = 0f;
-        private float m_waterHeight = 0f;
-        private float m_buoyancy = 0f;                          //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 
+        private float m_targetHoverHeight;
+        private float m_groundHeight;
+        private float m_waterHeight;
+        private float m_buoyancy;                               //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 
 
         // private float m_tensor = 5f;
         private int body_autodisable_frames = 20;
@@ -104,11 +104,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                                                         | CollisionCategories.Body
                                                         | CollisionCategories.Character
                                                         );
-        private bool m_taintshape = false;
-        private bool m_taintPhysics = false;
+        private bool m_taintshape;
+        private bool m_taintPhysics;
         private bool m_collidesLand = true;
-        private bool m_collidesWater = false;
-        public bool m_returnCollisions = false;
+        private bool m_collidesWater;
+        public bool m_returnCollisions;
 
         // Default we're a Geometry
         private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
@@ -116,85 +116,83 @@ namespace OpenSim.Region.Physics.OdePlugin
         // Default, Collide with Other Geometries, spaces and Bodies
         private CollisionCategories m_collisionFlags = m_default_collisionFlags;
 
-        public bool m_taintremove = false;
-        public bool m_taintdisable = false;
-        public bool m_disabled = false;
-        public bool m_taintadd = false;
-        public bool m_taintselected = false;
-        public bool m_taintCollidesWater = false;
+        public bool m_taintremove;
+        public bool m_taintdisable;
+        public bool m_disabled;
+        public bool m_taintadd;
+        public bool m_taintselected;
+        public bool m_taintCollidesWater;
 
-        public uint m_localID = 0;
+        public uint m_localID;
 
         //public GCHandle gc;
         private CollisionLocker ode;
 
         private bool m_taintforce = false;
         private bool m_taintaddangularforce = false;
-        private PhysicsVector m_force = new PhysicsVector(0.0f, 0.0f, 0.0f);
-        private List<PhysicsVector> m_forcelist = new List<PhysicsVector>();
-        private List<PhysicsVector> m_angularforcelist = new List<PhysicsVector>();
+        private Vector3 m_force;
+        private List<Vector3> m_forcelist = new List<Vector3>();
+        private List<Vector3> m_angularforcelist = new List<Vector3>();
 
         private IMesh _mesh;
         private PrimitiveBaseShape _pbs;
         private OdeScene _parent_scene;
-        public IntPtr m_targetSpace = (IntPtr) 0;
+        public IntPtr m_targetSpace = IntPtr.Zero;
         public IntPtr prim_geom;
         public IntPtr prev_geom;
         public IntPtr _triMeshData;
 
-        private IntPtr _linkJointGroup = (IntPtr)0;
-        private PhysicsActor _parent = null;
-        private PhysicsActor m_taintparent = null;
+        private IntPtr _linkJointGroup = IntPtr.Zero;
+        private PhysicsActor _parent;
+        private PhysicsActor m_taintparent;
 
         private List<OdePrim> childrenPrim = new List<OdePrim>();
 
-        private bool iscolliding = false;
-        private bool m_isphysical = false;
-        private bool m_isSelected = false;
+        private bool iscolliding;
+        private bool m_isphysical;
+        private bool m_isSelected;
 
-        internal bool m_isVolumeDetect = false; // If true, this prim only detects collisions but doesn't collide actively
+        internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
 
-        private bool m_throttleUpdates = false;
-        private int throttleCounter = 0;
-        public int m_interpenetrationcount = 0;
-        public float m_collisionscore = 0;
-        public int m_roundsUnderMotionThreshold = 0;
-        private int m_crossingfailures = 0;
+        private bool m_throttleUpdates;
+        private int throttleCounter;
+        public int m_interpenetrationcount;
+        public float m_collisionscore;
+        public int m_roundsUnderMotionThreshold;
+        private int m_crossingfailures;
 
-        public bool outofBounds = false;
+        public bool outofBounds;
         private float m_density = 10.000006836f; // Aluminum g/cm3;
 
-        public bool _zeroFlag = false;
-        private bool m_lastUpdateSent = false;
+        public bool _zeroFlag;
+        private bool m_lastUpdateSent;
 
-        public IntPtr Body = (IntPtr) 0;
+        public IntPtr Body = IntPtr.Zero;
         public String m_primName;
-//        private String m_primName;
-        private PhysicsVector _target_velocity;
+        private Vector3 _target_velocity;
         public d.Mass pMass;
 
-        public int m_eventsubscription = 0;
-        private CollisionEventUpdate CollisionEventsThisFrame = null;
+        public int m_eventsubscription;
+        private CollisionEventUpdate CollisionEventsThisFrame;
 
-        private IntPtr m_linkJoint = (IntPtr)0;
+        private IntPtr m_linkJoint = IntPtr.Zero;
 
-        public volatile bool childPrim = false;
+        public volatile bool childPrim;
 
         private ODEDynamics m_vehicle;
 
         internal int m_material = (int)Material.Wood;
 
-        public OdePrim(String primName, OdeScene parent_scene, PhysicsVector pos, PhysicsVector size,
+        public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
                        Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
         {
-            _target_velocity = new PhysicsVector(0, 0, 0);
             m_vehicle = new ODEDynamics();
             //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
             ode = dode;
-            _velocity = new PhysicsVector();
-            if (!PhysicsVector.isFinite(pos))
+            if (!pos.IsFinite())
             {
-                pos = new PhysicsVector(((int)Constants.RegionSize * 0.5f), ((int)Constants.RegionSize * 0.5f), parent_scene.GetTerrainHeightAtXY(((int)Constants.RegionSize * 0.5f), ((int)Constants.RegionSize * 0.5f)) + 0.5f);
+                pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
+                    parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
                 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
             }
             _position = pos;
@@ -209,9 +207,9 @@ namespace OpenSim.Region.Physics.OdePlugin
             prim_geom = IntPtr.Zero;
             prev_geom = IntPtr.Zero;
 
-            if (!PhysicsVector.isFinite(pos))
+            if (!pos.IsFinite())
             {
-                size = new PhysicsVector(0.5f, 0.5f, 0.5f);
+                size = new Vector3(0.5f, 0.5f, 0.5f);
                 m_log.Warn("[PHYSICS]: Got nonFinite Object create Size");
             }
 
@@ -221,8 +219,6 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             _size = size;
             m_taintsize = _size;
-            _acceleration = new PhysicsVector();
-            m_rotationalVelocity = PhysicsVector.Zero;
 
             if (!QuaternionIsFinite(rotation))
             {
@@ -387,7 +383,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 m_disabled = false;
 
                 // The body doesn't already have a finite rotation mode set here
-                if ((!m_angularlock.IsIdentical(PhysicsVector.Zero, 0)) && _parent == null)
+                if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null)
                 {
                     createAMotor(m_angularlock);
                 }
@@ -800,6 +796,8 @@ namespace OpenSim.Region.Physics.OdePlugin
             m_collisionscore = 0;
         }
 
+        private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
+
         public void setMesh(OdeScene parent_scene, IMesh mesh)
         {
             // This sleeper is there to moderate how long it takes between
@@ -831,19 +829,24 @@ namespace OpenSim.Region.Physics.OdePlugin
             mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
 
             mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
+            if (m_MeshToTriMeshMap.ContainsKey(mesh))
+            {
+                _triMeshData = m_MeshToTriMeshMap[mesh];
+            }
+            else
+            {
+                _triMeshData = d.GeomTriMeshDataCreate();
 
-            _triMeshData = d.GeomTriMeshDataCreate();
-
-            d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
-            d.GeomTriMeshDataPreprocess(_triMeshData);
+                d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
+                d.GeomTriMeshDataPreprocess(_triMeshData);
+                m_MeshToTriMeshMap[mesh] = _triMeshData;
+            }
 
             _parent_scene.waitForSpaceUnlock(m_targetSpace);
-
             try
             {
                 if (prim_geom == IntPtr.Zero)
                 {
-//Console.WriteLine(" setMesh 1");               
                     SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
                 }
             }
@@ -853,6 +856,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 return;
             }
 
+
            // if (IsPhysical && Body == (IntPtr) 0)
            // {
                 // Recreate the body
@@ -873,7 +877,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             
             if (prim_geom != IntPtr.Zero)
             {
-                         if (!_position.IsIdentical(m_taintposition,0f))
+                         if (!_position.ApproxEquals(m_taintposition, 0f))
                             changemove(timestep);
 
                      if (m_taintrot != _orientation)
@@ -898,7 +902,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     changePhysicsStatus(timestep);
                 //
 
-                if (!_size.IsIdentical(m_taintsize,0))
+                if (!_size.ApproxEquals(m_taintsize,0f))
                     changesize(timestep);
                 //
 
@@ -912,7 +916,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if (m_taintaddangularforce)
                     changeAddAngularForce(timestep);
 
-                if (!m_taintTorque.IsIdentical(PhysicsVector.Zero, 0.001f))
+                if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
                     changeSetTorque(timestep);
 
                 if (m_taintdisable)
@@ -921,7 +925,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if (m_taintselected != m_isSelected)
                     changeSelectedStatus(timestep);
 
-                if (!m_taintVelocity.IsIdentical(PhysicsVector.Zero, 0.001f))
+                if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
                     changevelocity(timestep);
 
                 if (m_taintparent != _parent)
@@ -930,7 +934,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if (m_taintCollidesWater != m_collidesWater)
                     changefloatonwater(timestep);
 
-                if (!m_angularlock.IsIdentical(m_taintAngularLock,0))
+                if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f))
                     changeAngularLock(timestep);
  
             }
@@ -950,7 +954,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 //If we have a parent then we're not authorative here
                 if (_parent == null)
                 {
-                    if (!m_taintAngularLock.IsIdentical(new PhysicsVector(1f,1f,1f), 0))
+                    if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f))
                     {
                         //d.BodySetFiniteRotationMode(Body, 0);
                         //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
@@ -967,7 +971,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
             }
             // Store this for later in case we get turned into a separate body
-            m_angularlock = new PhysicsVector(m_taintAngularLock.X, m_taintAngularLock.Y, m_taintAngularLock.Z);
+            m_angularlock = m_taintAngularLock;
             
         }
 
@@ -1111,7 +1115,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 prm.m_disabled = false;
 
                                 // The body doesn't already have a finite rotation mode set here
-                                if ((!m_angularlock.IsIdentical(PhysicsVector.Zero, 0)) && _parent == null)
+                                if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
                                 {
                                     prm.createAMotor(m_angularlock);
                                 }
@@ -1154,7 +1158,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                             m_disabled = false;
 
                             // The body doesn't already have a finite rotation mode set here
-                            if ((!m_angularlock.IsIdentical(PhysicsVector.Zero, 0)) && _parent == null)
+                            if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
                             {
                                 createAMotor(m_angularlock);
                             }
@@ -1338,7 +1342,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             m_taintshape = false;
             m_taintforce = false;
             m_taintdisable = false;
-            m_taintVelocity = PhysicsVector.Zero;
+            m_taintVelocity = Vector3.Zero;
         }
 
         public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
@@ -1567,7 +1571,7 @@ Console.WriteLine(" JointCreateFixed");
 //Console.WriteLine("Move " +  m_primName);             
                         if(!d.BodyIsEnabled (Body))  d.BodyEnable (Body); // KF add 161009
                         // NON-'VEHICLES' are dealt with here
-                        if (d.BodyIsEnabled(Body) && !m_angularlock.IsIdentical(PhysicsVector.Zero, 0.003f))
+                        if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
                         {
                             d.Vector3 avel2 = d.BodyGetAngularVel(Body);
                             if (m_angularlock.X == 1)
@@ -1624,7 +1628,7 @@ Console.WriteLine(" JointCreateFixed");
 
                             d.Vector3 pos = d.BodyGetPosition(Body);
                             _target_velocity =
-                                new PhysicsVector(
+                            new Vector3(
                                     (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
                                     (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
                                     (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
@@ -1632,7 +1636,7 @@ Console.WriteLine(" JointCreateFixed");
 
                             //  if velocity is zero, use position control; otherwise, velocity control
 
-                            if (_target_velocity.IsIdentical(PhysicsVector.Zero,0.1f))
+                            if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
                             {
                                 //  keep track of where we stopped.  No more slippin' & slidin'
         
@@ -1717,13 +1721,13 @@ Console.WriteLine(" JointCreateFixed");
 
 
                             _target_velocity =
-                            new PhysicsVector(0.0f, 0.0f,
+                            new Vector3(0.0f, 0.0f,
                                 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
                                 );
 
                         //  if velocity is zero, use position control; otherwise, velocity control
 
-                        if (_target_velocity.IsIdentical(PhysicsVector.Zero, 0.1f))
+                        if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
                         {
                             //  keep track of where we stopped.  No more slippin' & slidin'
         
@@ -1812,7 +1816,7 @@ Console.WriteLine(" JointCreateFixed");
                 d.BodySetQuaternion(Body, ref myrot);
                     if (m_isphysical)
                     {
-                        if (!m_angularlock.IsIdentical(new PhysicsVector(1, 1, 1), 0))
+                    if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
                             createAMotor(m_angularlock);
                     }
                 }
@@ -2121,7 +2125,7 @@ Console.WriteLine(" JointCreateFixed");
                     //m_log.Info("[PHYSICS]: dequeing forcelist");
                     if (IsPhysical)
                     {
-                        PhysicsVector iforce = new PhysicsVector();
+                        Vector3 iforce = Vector3.Zero;
                         for (int i = 0; i < m_forcelist.Count; i++)
                         {
                             iforce = iforce + (m_forcelist[i] * 100);
@@ -2151,8 +2155,8 @@ Console.WriteLine(" JointCreateFixed");
                     d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
                 }
             }
-            
-            m_taintTorque = new PhysicsVector(0, 0, 0);
+
+            m_taintTorque = Vector3.Zero;
         }
 
         public void changeAddAngularForce(float timestamp)
@@ -2164,7 +2168,7 @@ Console.WriteLine(" JointCreateFixed");
                     //m_log.Info("[PHYSICS]: dequeing forcelist");
                     if (IsPhysical)
                     {
-                        PhysicsVector iforce = new PhysicsVector();
+                        Vector3 iforce = Vector3.Zero;
                         for (int i = 0; i < m_angularforcelist.Count; i++)
                         {
                             iforce = iforce + (m_angularforcelist[i] * 100);
@@ -2198,7 +2202,7 @@ Console.WriteLine(" JointCreateFixed");
                 
                 //resetCollisionAccounting();
             }
-            m_taintVelocity = PhysicsVector.Zero;
+            m_taintVelocity = Vector3.Zero;
         }
 
         public override bool IsPhysical
@@ -2207,7 +2211,7 @@ Console.WriteLine(" JointCreateFixed");
             set { 
                   m_isphysical = value;
                   if (!m_isphysical) // Zero the remembered last velocity
-                      m_lastVelocity = new PhysicsVector(0.0f, 0.0f, 0.0f);
+                      m_lastVelocity = Vector3.Zero;
                 }
         }
 
@@ -2252,7 +2256,7 @@ Console.WriteLine(" JointCreateFixed");
             get { return _zeroFlag; }
         }
 
-        public override PhysicsVector Position
+        public override Vector3 Position
         {
             get { return _position; }
 
@@ -2261,12 +2265,12 @@ Console.WriteLine(" JointCreateFixed");
             }
         }
 
-        public override PhysicsVector Size
+        public override Vector3 Size
         {
             get { return _size; }
             set
             {
-                if (PhysicsVector.isFinite(value))
+                if (value.IsFinite())
                 {
                     _size = value;
                 }
@@ -2282,13 +2286,13 @@ Console.WriteLine(" JointCreateFixed");
             get { return CalculateMass(); }
         }
 
-        public override PhysicsVector Force
+        public override Vector3 Force
         {
-            //get { return PhysicsVector.Zero; }
+            //get { return Vector3.Zero; }
             get { return m_force; }
             set
             {
-                if (PhysicsVector.isFinite(value))
+                if (value.IsFinite())
                 {
                     m_force = value;
                 }
@@ -2310,7 +2314,7 @@ Console.WriteLine(" JointCreateFixed");
             m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value);
         }
 
-        public override void VehicleVectorParam(int param, PhysicsVector value)
+        public override void VehicleVectorParam(int param, Vector3 value)
         {
             m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value);
         }
@@ -2328,14 +2332,14 @@ Console.WriteLine(" JointCreateFixed");
             }
         }
 
-        public override PhysicsVector CenterOfMass
+        public override Vector3 CenterOfMass
         {
-            get { return PhysicsVector.Zero; }
+            get { return Vector3.Zero; }
         }
 
-        public override PhysicsVector GeometricCenter
+        public override Vector3 GeometricCenter
         {
-            get { return PhysicsVector.Zero; }
+            get { return Vector3.Zero; }
         }
 
         public override PrimitiveBaseShape Shape
@@ -2347,13 +2351,13 @@ Console.WriteLine(" JointCreateFixed");
             }
         }
 
-        public override PhysicsVector Velocity
+        public override Vector3 Velocity
         {
             get
             {
                 // Averate previous velocity with the new one so
                 // client object interpolation works a 'little' better
-                PhysicsVector returnVelocity = new PhysicsVector();
+                Vector3 returnVelocity = Vector3.Zero;
                 returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
                 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
                 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
@@ -2361,7 +2365,7 @@ Console.WriteLine(" JointCreateFixed");
             }
             set
             {
-                if (PhysicsVector.isFinite(value))
+                if (value.IsFinite())
                 {
                     _velocity = value;
 
@@ -2376,19 +2380,19 @@ Console.WriteLine(" JointCreateFixed");
             }
         }
 
-        public override PhysicsVector Torque
+        public override Vector3 Torque
         {
             get
             {
                 if (!m_isphysical || Body == IntPtr.Zero)
-                    return new PhysicsVector(0,0,0);
+                    return Vector3.Zero;
 
                 return _torque;
             }
 
             set
             {
-                if (PhysicsVector.isFinite(value))
+                if (value.IsFinite())
                 {
                     m_taintTorque = value;
                     _parent_scene.AddPhysicsActorTaint(this);
@@ -2440,20 +2444,20 @@ Console.WriteLine(" JointCreateFixed");
             return true;
         }
 
-        public override PhysicsVector Acceleration
+        public override Vector3 Acceleration
         {
             get { return _acceleration; }
         }
 
 
-        public void SetAcceleration(PhysicsVector accel)
+        public void SetAcceleration(Vector3 accel)
         {
             _acceleration = accel;
         }
 
-        public override void AddForce(PhysicsVector force, bool pushforce)
+        public override void AddForce(Vector3 force, bool pushforce)
         {
-            if (PhysicsVector.isFinite(force))
+            if (force.IsFinite())
             {
                 m_forcelist.Add(force);
                 m_taintforce = true;
@@ -2465,9 +2469,9 @@ Console.WriteLine(" JointCreateFixed");
             //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() +  " to prim at " + Position.ToString());
         }
 
-        public override void AddAngularForce(PhysicsVector force, bool pushforce)
+        public override void AddAngularForce(Vector3 force, bool pushforce)
         {
-            if (PhysicsVector.isFinite(force))
+            if (force.IsFinite())
             {
                 m_angularforcelist.Add(force);
                 m_taintaddangularforce = true;
@@ -2478,23 +2482,23 @@ Console.WriteLine(" JointCreateFixed");
             }
         }
 
-        public override PhysicsVector RotationalVelocity
+        public override Vector3 RotationalVelocity
         {
             get
             {
-                PhysicsVector pv = new PhysicsVector(0, 0, 0);
+                Vector3 pv = Vector3.Zero;
                 if (_zeroFlag)
                     return pv;
                 m_lastUpdateSent = false;
 
-                if (m_rotationalVelocity.IsIdentical(pv, 0.2f))
+                if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
                     return pv;
 
                 return m_rotationalVelocity;
             }
             set
             {
-                if (PhysicsVector.isFinite(value))
+                if (value.IsFinite())
                 {
                     m_rotationalVelocity = value;
                 }
@@ -2535,16 +2539,16 @@ Console.WriteLine(" JointCreateFixed");
             m_taintparent = null;
         }
 
-        public override void LockAngularMotion(PhysicsVector axis)
+        public override void LockAngularMotion(Vector3 axis)
         {
             // reverse the zero/non zero values for ODE.
-            if (PhysicsVector.isFinite(axis))
+            if (axis.IsFinite())
             {
                 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);
+                m_taintAngularLock = axis;
             }
             else
             {
@@ -2557,7 +2561,7 @@ Console.WriteLine(" JointCreateFixed");
             //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
             if (_parent == null)
             {
-                PhysicsVector pv = new PhysicsVector(0, 0, 0);
+                Vector3 pv = Vector3.Zero;
                 bool lastZeroFlag = _zeroFlag;
                 if (Body != (IntPtr)0) // FIXME -> or if it is a joint
                 {
@@ -2566,9 +2570,9 @@ Console.WriteLine(" JointCreateFixed");
                     d.Vector3 vel = d.BodyGetLinearVel(Body);
                     d.Vector3 rotvel = d.BodyGetAngularVel(Body);
                     d.Vector3 torque = d.BodyGetTorque(Body);
-                    _torque.setValues(torque.X, torque.Y, torque.Z);
-                    PhysicsVector l_position = new PhysicsVector();
-                    Quaternion l_orientation = new Quaternion();
+                    _torque = new Vector3(torque.X, torque.Y, torque.Z);
+                    Vector3 l_position = Vector3.Zero;
+                    Quaternion l_orientation = Quaternion.Identity;
 
                     //  kluge to keep things in bounds.  ODE lets dead avatars drift away (they should be removed!)
                     //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
@@ -2703,16 +2707,16 @@ Console.WriteLine(" JointCreateFixed");
                         _velocity.Z = vel.Z;
 
                         _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
-                        _acceleration = new PhysicsVector(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
+                        _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
                         //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
 
-                        if (_velocity.IsIdentical(pv, 0.5f))
+                        if (_velocity.ApproxEquals(pv, 0.5f))
                         {
                             m_rotationalVelocity = pv;
                         }
                         else
                         {
-                            m_rotationalVelocity.setValues(rotvel.X, rotvel.Y, rotvel.Z);
+                            m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z);
                         }
 
                         //m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
@@ -2760,15 +2764,15 @@ Console.WriteLine(" JointCreateFixed");
             }
         }
 
-        public override void SetMomentum(PhysicsVector momentum)
+        public override void SetMomentum(Vector3 momentum)
         {
         }
 
-        public override PhysicsVector PIDTarget 
+        public override Vector3 PIDTarget 
         { 
             set
             {
-                if (PhysicsVector.isFinite(value))
+                if (value.IsFinite())
                 {
                     m_PIDTarget = value;
                 }
@@ -2784,7 +2788,7 @@ Console.WriteLine(" JointCreateFixed");
         public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
         public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
 
-        private void createAMotor(PhysicsVector axis)
+        private void createAMotor(Vector3 axis)
         {
             if (Body == IntPtr.Zero)
                 return;
diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
index 0a065be..2f42646 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
@@ -239,6 +239,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
         private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
         private readonly HashSet<OdePrim> _taintedPrimH = new HashSet<OdePrim>();
+        private readonly Object _taintedPrimLock = new Object();
         private readonly List<OdePrim> _taintedPrimL = new List<OdePrim>();
         private readonly HashSet<OdeCharacter> _taintedActors = new HashSet<OdeCharacter>();
         private readonly List<d.ContactGeom> _perloopContact = new List<d.ContactGeom>();
@@ -683,7 +684,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// </summary>
         /// <param name="pos"></param>
         /// <returns>Returns which split up space the given position is in.</returns>
-        public string whichspaceamIin(PhysicsVector pos)
+        public string whichspaceamIin(Vector3 pos)
         {
             return calculateSpaceForGeom(pos).ToString();
         }
@@ -962,7 +963,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                                     //p2.CollidingObj = true;
                                     contacts[i].depth = 0.00000003f;
-                                    p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 0.5f);
+                                    p2.Velocity = p2.Velocity + new Vector3(0f, 0f, 0.5f);
                                     contacts[i].pos =
                                         new d.Vector3(contacts[i].pos.X + (p1.Size.X/2),
                                                       contacts[i].pos.Y + (p1.Size.Y/2),
@@ -980,7 +981,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                                     //p2.CollidingObj = true;
                                     contacts[i].depth = 0.00000003f;
-                                    p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 0.5f);
+                                    p1.Velocity = p1.Velocity + new Vector3(0f, 0f, 0.5f);
                                     contacts[i].pos =
                                         new d.Vector3(contacts[i].pos.X + (p1.Size.X/2),
                                                       contacts[i].pos.Y + (p1.Size.Y/2),
@@ -1645,9 +1646,9 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         #region Add/Remove Entities
 
-        public override PhysicsActor AddAvatar(string avName, PhysicsVector position, PhysicsVector size, bool isFlying)
+        public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
         {
-            PhysicsVector pos = new PhysicsVector();
+            Vector3 pos;
             pos.X = position.X;
             pos.Y = position.Y;
             pos.Z = position.Z;
@@ -1697,18 +1698,12 @@ namespace OpenSim.Region.Physics.OdePlugin
             
         }
 
-        private PhysicsActor AddPrim(String name, PhysicsVector position, PhysicsVector size, Quaternion rotation,
+        private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
                                      IMesh mesh, PrimitiveBaseShape pbs, bool isphysical)
         {
-            
-            PhysicsVector pos = new PhysicsVector(position.X, position.Y, position.Z);
-            //pos.X = position.X;
-            //pos.Y = position.Y;
-            //pos.Z = position.Z;
-            PhysicsVector siz = new PhysicsVector();
-            siz.X = size.X;
-            siz.Y = size.Y;
-            siz.Z = size.Z;
+
+            Vector3 pos = position;
+            Vector3 siz = size;
             Quaternion rot = rotation;
 
             OdePrim newPrim;
@@ -1735,14 +1730,14 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
-        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position,
-                                                  PhysicsVector size, Quaternion rotation) //To be removed
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation) //To be removed
         {
             return AddPrimShape(primName, pbs, position, size, rotation, false);
         }
 
-        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position,
-                                                  PhysicsVector size, Quaternion rotation, bool isPhysical)
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation, bool isPhysical)
         {
             PhysicsActor result;
             IMesh mesh = null;
@@ -1975,7 +1970,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         // this joint will just be added to a waiting list that is NOT processed during the main
         // Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests.
 
-        public override PhysicsJoint RequestJointCreation(string objectNameInScene, PhysicsJointType jointType, PhysicsVector position,
+        public override PhysicsJoint RequestJointCreation(string objectNameInScene, PhysicsJointType jointType, Vector3 position,
                                             Quaternion rotation, string parms, List<string> bodyNames, string trackedBodyName, Quaternion localRotation)
 
         {
@@ -1983,7 +1978,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             OdePhysicsJoint joint = new OdePhysicsJoint();
             joint.ObjectNameInScene = objectNameInScene;
             joint.Type = jointType;
-            joint.Position = new PhysicsVector(position.X, position.Y, position.Z);
+            joint.Position = position;
             joint.Rotation = rotation;
             joint.RawParams = parms;
             joint.BodyNames = new List<string>(bodyNames);
@@ -2035,7 +2030,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         }
 
         // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
-        public override PhysicsVector GetJointAnchor(PhysicsJoint joint)
+        public override Vector3 GetJointAnchor(PhysicsJoint joint)
         {
             Debug.Assert(joint.IsInPhysicsEngine);
             d.Vector3 pos = new d.Vector3();
@@ -2057,14 +2052,14 @@ namespace OpenSim.Region.Physics.OdePlugin
                         break;
                 }
             }
-            return new PhysicsVector(pos.X, pos.Y, pos.Z);
+            return new Vector3(pos.X, pos.Y, pos.Z);
         }
 
         // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
         // WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function
         // appears to be unreliable. Fortunately we can compute the joint axis ourselves by
         // keeping track of the joint's original orientation relative to one of the involved bodies.
-        public override PhysicsVector GetJointAxis(PhysicsJoint joint)
+        public override Vector3 GetJointAxis(PhysicsJoint joint)
         {
             Debug.Assert(joint.IsInPhysicsEngine);
             d.Vector3 axis = new d.Vector3();
@@ -2086,7 +2081,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         break;
                 }
             }
-            return new PhysicsVector(axis.X, axis.Y, axis.Z);
+            return new Vector3(axis.X, axis.Y, axis.Z);
         }
 
 
@@ -2254,7 +2249,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// <param name="pos">the position that the geom moved to</param>
         /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
         /// <returns>a pointer to the new space it's in</returns>
-        public IntPtr recalculateSpaceForGeom(IntPtr geom, PhysicsVector pos, IntPtr currentspace)
+        public IntPtr recalculateSpaceForGeom(IntPtr geom, Vector3 pos, IntPtr currentspace)
         {
             // Called from setting the Position and Size of an ODEPrim so
             // it's already in locked space.
@@ -2401,7 +2396,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// </summary>
         /// <param name="pos"></param>
         /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
-        public IntPtr calculateSpaceForGeom(PhysicsVector pos)
+        public IntPtr calculateSpaceForGeom(Vector3 pos)
         {
             int[] xyspace = calculateSpaceArrayItemFromPos(pos);
             //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString());
@@ -2413,7 +2408,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// </summary>
         /// <param name="pos"></param>
         /// <returns>an array item based on the position</returns>
-        public int[] calculateSpaceArrayItemFromPos(PhysicsVector pos)
+        public int[] calculateSpaceArrayItemFromPos(Vector3 pos)
         {
             int[] returnint = new int[2];
 
@@ -2572,7 +2567,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (prim is OdePrim)
             {
                 OdePrim taintedprim = ((OdePrim) prim);
-                lock (_taintedPrimH)
+                lock (_taintedPrimLock)
                 {
                     if (!(_taintedPrimH.Contains(taintedprim))) 
                                 {
@@ -2700,24 +2695,28 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 // Modify other objects in the scene.
                                 processedtaints = false;
 
-                                lock (_taintedPrimL)
+                                lock (_taintedPrimLock)
                                 {
                                     foreach (OdePrim prim in _taintedPrimL)
                                     {
-       
-                                    
                                         if (prim.m_taintremove)
                                         {
-//Console.WriteLine("Simulate calls RemovePrimThreadLocked");                                          
+                                            //Console.WriteLine("Simulate calls RemovePrimThreadLocked");                                          
                                             RemovePrimThreadLocked(prim);
                                         }
                                         else
                                         {
-//Console.WriteLine("Simulate calls ProcessTaints");                                        
+                                            //Console.WriteLine("Simulate calls ProcessTaints");                                        
                                             prim.ProcessTaints(timeStep);
                                         }
                                         processedtaints = true;
                                         prim.m_collisionscore = 0;
+
+                                        // This loop can block up the Heartbeat for a very long time on large regions.
+                                        // We need to let the Watchdog know that the Heartbeat is not dead
+                                        // NOTE: This is currently commented out, but if things like OAR loading are 
+                                        // timing the heartbeat out we will need to uncomment it
+                                        //Watchdog.UpdateThread();
                                     }
 
                                     if (SupportsNINJAJoints)
diff --git a/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs b/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs
index cdd38c4..69e2d03 100644
--- a/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs
+++ b/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs
@@ -76,8 +76,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         public void CreateAndDropPhysicalCube()
         {
             PrimitiveBaseShape newcube = PrimitiveBaseShape.CreateBox();
-            PhysicsVector position = new PhysicsVector(((int)Constants.RegionSize * 0.5f), ((int)Constants.RegionSize * 0.5f), 128);
-            PhysicsVector size = new PhysicsVector(0.5f, 0.5f, 0.5f);
+            Vector3 position = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), 128f);
+            Vector3 size = new Vector3(0.5f, 0.5f, 0.5f);
             Quaternion rot = Quaternion.Identity;
             PhysicsActor prim = ps.AddPrimShape("CoolShape", newcube, position, size, rot, true);
             OdePrim oprim = (OdePrim)prim;