1 files changed, 272 insertions, 448 deletions

diff --git a/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs
index 0b9c45f..8934330 100644
--- a/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs
+++ b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs
@@ -33,7 +33,7 @@
  * 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 
+ * 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.
@@ -48,7 +48,6 @@ using System.Runtime.InteropServices;
 using System.Threading;
 using log4net;
 using OpenMetaverse;
-using Ode.NET;
 using OpenSim.Framework;
 using OpenSim.Region.PhysicsModules.SharedBase;
 
@@ -83,8 +82,15 @@ namespace OpenSim.Region.PhysicsModule.ODE
             set
             {
                 m_isphysical = value;
-                if (!m_isphysical) // Zero the remembered last velocity
+                if (!m_isphysical)
+                {
+                    _zeroFlag = true; // Zero the remembered last velocity
                     m_lastVelocity = Vector3.Zero;
+                    _acceleration = Vector3.Zero;
+                    _velocity = Vector3.Zero;
+                    m_taintVelocity = Vector3.Zero;
+                    m_rotationalVelocity = Vector3.Zero;
+                }
             }
         }
 
@@ -104,10 +110,12 @@ namespace OpenSim.Region.PhysicsModule.ODE
         private Vector3 m_taintVelocity;
         private Vector3 m_taintTorque;
         private Quaternion m_taintrot;
-        private Vector3 m_angularlock = Vector3.One;
-        private Vector3 m_taintAngularLock = Vector3.One;
+
         private IntPtr Amotor = IntPtr.Zero;
 
+        private byte m_taintAngularLock = 0;
+        private byte m_angularlock = 0;
+
         private bool m_assetFailed = false;
 
         private Vector3 m_PIDTarget;
@@ -125,7 +133,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
         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_buoyancy;                //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
 
         // private float m_tensor = 5f;
         private int body_autodisable_frames = 20;
@@ -162,7 +170,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
         private PrimitiveBaseShape _pbs;
         private OdeScene _parent_scene;
-        
+
         /// <summary>
         /// The physics space which contains prim geometries
         /// </summary>
@@ -195,7 +203,6 @@ namespace OpenSim.Region.PhysicsModule.ODE
         public int m_interpenetrationcount { get; private set; }
         internal float m_collisionscore;
         public int m_roundsUnderMotionThreshold { get; private set; }
-        private int m_crossingfailures;
 
         public bool outofBounds { get; private set; }
         private float m_density = 10.000006836f; // Aluminum g/cm3;
@@ -350,12 +357,12 @@ namespace OpenSim.Region.PhysicsModule.ODE
             if (m_assetFailed)
             {
                 d.GeomSetCategoryBits(prim_geom, 0);
-                d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
+                d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits);
             }
             else
             {
-                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
             }
 
             _parent_scene.geom_name_map[prim_geom] = Name;
@@ -423,7 +430,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 if (m_assetFailed)
                 {
                     d.GeomSetCategoryBits(prim_geom, 0);
-                    d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
+                    d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits);
                 }
                 else
                 {
@@ -431,12 +438,12 @@ namespace OpenSim.Region.PhysicsModule.ODE
                     m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
                 }
 
-                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
 
                 d.BodySetAutoDisableFlag(Body, true);
                 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
-                
+
                 // disconnect from world gravity so we can apply buoyancy
                 d.BodySetGravityMode (Body, false);
 
@@ -445,7 +452,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 m_disabled = false;
 
                 // The body doesn't already have a finite rotation mode set here
-                if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
+                if (m_angularlock != 0 && _parent == null)
                 {
                     createAMotor(m_angularlock);
                 }
@@ -467,8 +474,8 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
             float returnMass = 0;
             float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
-            float hollowVolume = hollowAmount * hollowAmount; 
-            
+            float hollowVolume = hollowAmount * hollowAmount;
+
             switch (_pbs.ProfileShape)
             {
                 case ProfileShape.Square:
@@ -504,16 +511,16 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
                     else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
                         {
-                        //a tube 
+                        //a tube
 
                         volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
                         tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY);
                         volume -= volume*tmp*tmp;
-                        
+
                         if (hollowAmount > 0.0)
                             {
                             hollowVolume *= hollowAmount;
-                            
+
                             switch (_pbs.HollowShape)
                                 {
                                 case HollowShape.Square:
@@ -572,7 +579,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                         volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
                         tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
                         volume *= (1.0f - tmp * tmp);
-                        
+
                         if (hollowAmount > 0.0)
                             {
 
@@ -816,8 +823,8 @@ namespace OpenSim.Region.PhysicsModule.ODE
                         }
                         else
                         {
-                            d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                            d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                            d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                            d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
                         }
 
                         d.BodyDestroy(Body);
@@ -838,7 +845,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 else
                 {
                     _parent_scene.DeactivatePrim(this);
-                    
+
                     m_collisionCategories &= ~CollisionCategories.Body;
                     m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
 
@@ -850,8 +857,8 @@ namespace OpenSim.Region.PhysicsModule.ODE
                     else
                     {
 
-                        d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                        d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                        d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                        d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
                     }
 
                     Body = IntPtr.Zero;
@@ -909,7 +916,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 else
                 {
                     _triMeshData = d.GeomTriMeshDataCreate();
-    
+
                     d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
                     d.GeomTriMeshDataPreprocess(_triMeshData);
                     m_MeshToTriMeshMap[mesh] = _triMeshData;
@@ -919,7 +926,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
 //            _parent_scene.waitForSpaceUnlock(m_targetSpace);
             try
             {
-                SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
+                SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null));
             }
             catch (AccessViolationException)
             {
@@ -967,7 +974,7 @@ Console.WriteLine("ZProcessTaints for " + Name);
                     rotate();
                 }
             }
-        
+
             if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent))
                 changePhysicsStatus();
 
@@ -1001,7 +1008,7 @@ Console.WriteLine("ZProcessTaints for " + Name);
             if (m_taintCollidesWater != m_collidesWater)
                 changefloatonwater();
 
-            if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f))
+            if (m_taintAngularLock != m_angularlock)
                 changeAngularLock();
         }
 
@@ -1017,10 +1024,8 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 //If we have a parent then we're not authorative here
                 if (_parent == null)
                 {
-                    if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f))
+                    if (m_taintAngularLock != 0)
                     {
-                        //d.BodySetFiniteRotationMode(Body, 0);
-                        //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
                         createAMotor(m_taintAngularLock);
                     }
                     else
@@ -1034,7 +1039,6 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 }
             }
 
-            // Store this for later in case we get turned into a separate body
             m_angularlock = m_taintAngularLock;
         }
 
@@ -1070,7 +1074,7 @@ Console.WriteLine("ZProcessTaints for " + Name);
             else if (_parent != null && m_taintparent == null)
             {
 //Console.WriteLine("  changelink B");
-            
+
                 if (_parent is OdePrim)
                 {
                     OdePrim obj = (OdePrim)_parent;
@@ -1078,16 +1082,16 @@ Console.WriteLine("ZProcessTaints for " + Name);
                     childPrim = false;
                     //_parent = null;
                 }
-                
+
                 /*
                     if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
                     d.JointGroupDestroy(_linkJointGroup);
-                        
+
                     _linkJointGroup = (IntPtr)0;
                     m_linkJoint = (IntPtr)0;
                 */
             }
- 
+
             _parent = m_taintparent;
             m_taintPhysics = IsPhysical;
         }
@@ -1145,12 +1149,12 @@ Console.WriteLine("ZProcessTaints for " + Name);
                     if (prm.m_assetFailed)
                     {
                         d.GeomSetCategoryBits(prm.prim_geom, 0);
-                        d.GeomSetCollideBits(prm.prim_geom, prm.BadMeshAssetCollideBits);
+                        d.GeomSetCollideBits(prm.prim_geom, (uint)prm.BadMeshAssetCollideBits);
                     }
                     else
                     {
-                        d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
-                        d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
+                        d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
+                        d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
                     }
 
                     d.Quaternion quat = new d.Quaternion();
@@ -1194,14 +1198,14 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 if (m_assetFailed)
                 {
                     d.GeomSetCategoryBits(prim_geom, 0);
-                    d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
+                    d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits);
                 }
                 else
                 {
                     //Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name);
-                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                    d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
                     //Console.WriteLine(" Post GeomSetCategoryBits 2");
-                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                    d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
                 }
 
                 d.Quaternion quat2 = new d.Quaternion();
@@ -1230,7 +1234,8 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 m_disabled = false;
 
                 // The body doesn't already have a finite rotation mode set here
-                if ((!m_angularlock.ApproxEquals(Vector3.One, 0f)) && _parent == null)
+                // or remove
+                if (_parent == null)
                 {
                     createAMotor(m_angularlock);
                 }
@@ -1340,7 +1345,7 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 // in between the disabling and the collision properties setting
                 // which would wake the physical body up from a soft disabling and potentially cause it to fall
                 // through the ground.
-                
+
                 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
                 // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
                 // so that causes the selected part to wake up and continue moving.
@@ -1354,7 +1359,7 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
                 // selected and disabled. then, due to a thread switch, the selection processing is
                 // interrupted and the physics engine continues to simulate, so the last 50 items, whose
-                // selection was not yet processed, continues to simulate. this wakes up ALL of the 
+                // selection was not yet processed, continues to simulate. this wakes up ALL of the
                 // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
                 // up, start simulating again, which in turn wakes up the last 50.
 
@@ -1370,8 +1375,8 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 }
                 else
                 {
-                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                    d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
                 }
 
                 if (IsPhysical)
@@ -1396,12 +1401,12 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 if (m_assetFailed)
                 {
                     d.GeomSetCategoryBits(prim_geom, 0);
-                    d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
+                    d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits);
                 }
                 else
                 {
-                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                    d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
                 }
 
                 if (IsPhysical)
@@ -1560,7 +1565,7 @@ Console.WriteLine("CreateGeom:");
         private void changeadd()
         {
 //            m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name);
-            
+
             int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
             IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
 
@@ -1686,6 +1691,9 @@ Console.WriteLine(" JointCreateFixed");
             float fy = 0;
             float fz = 0;
 
+            if (outofBounds)
+                return;
+
             if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim)        // KF: Only move root prims.
             {
                 if (m_vehicle.Type != Vehicle.TYPE_NONE)
@@ -1697,31 +1705,16 @@ Console.WriteLine(" JointCreateFixed");
                 {
 //Console.WriteLine("Move " +  Name);
                     if (!d.BodyIsEnabled (Body))  d.BodyEnable (Body); // KF add 161009
-                    // NON-'VEHICLES' are dealt with here
-//                    if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
-//                    {
-//                        d.Vector3 avel2 = d.BodyGetAngularVel(Body);
-//                        /*
-//                        if (m_angularlock.X == 1)
-//                            avel2.X = 0;
-//                        if (m_angularlock.Y == 1)
-//                            avel2.Y = 0;
-//                        if (m_angularlock.Z == 1)
-//                            avel2.Z = 0;
-//                        d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
-//                         */
-//                    }
-                    //float PID_P = 900.0f;
 
                     float m_mass = CalculateMass();
 
 //                    fz = 0f;
                     //m_log.Info(m_collisionFlags.ToString());
 
-                    
+
                     //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
                     // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
-                    // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up 
+                    // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
                     // gravityz multiplier = 1 - m_buoyancy
                     fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
 
@@ -1736,7 +1729,7 @@ Console.WriteLine(" JointCreateFixed");
                         fz = 0f;
 
                         //  no lock; for now it's only called from within Simulate()
-    
+
                         // If the PID Controller isn't active then we set our force
                         // calculating base velocity to the current position
 
@@ -1745,7 +1738,7 @@ Console.WriteLine(" JointCreateFixed");
                             //PID_G = PID_G / m_PIDTau;
                             m_PIDTau = 1;
                         }
-    
+
                         if ((PID_G - m_PIDTau) <= 0)
                         {
                             PID_G = m_PIDTau + 1;
@@ -1768,7 +1761,7 @@ Console.WriteLine(" JointCreateFixed");
                         if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
                         {
                             //  keep track of where we stopped.  No more slippin' & slidin'
-    
+
                             // We only want to deactivate the PID Controller if we think we want to have our surrogate
                             // react to the physics scene by moving it's position.
                             // Avatar to Avatar collisions
@@ -1789,7 +1782,7 @@ Console.WriteLine(" JointCreateFixed");
                             // We're flying and colliding with something
                             fx = ((_target_velocity.X) - vel.X) * (PID_D);
                             fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
-    
+
                             // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
 
                             fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
@@ -1800,7 +1793,7 @@ Console.WriteLine(" JointCreateFixed");
                     if (m_useHoverPID && !PIDActive)
                     {
 //Console.WriteLine("Hover " +  Name);
-                    
+
                         // If we're using the PID controller, then we have no gravity
                         fz = (-1 * _parent_scene.gravityz) * m_mass;
 
@@ -1857,7 +1850,7 @@ Console.WriteLine(" JointCreateFixed");
                         if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
                         {
                             //  keep track of where we stopped.  No more slippin' & slidin'
-    
+
                             // We only want to deactivate the PID Controller if we think we want to have our surrogate
                             // react to the physics scene by moving it's position.
                             // Avatar to Avatar collisions
@@ -1895,7 +1888,7 @@ Console.WriteLine(" JointCreateFixed");
                         {
                             // A physical body at rest on a surface will auto-disable after a while,
                             // this appears to re-enable it incase the surface it is upon vanishes,
-                            // and the body should fall again. 
+                            // and the body should fall again.
                             d.BodySetLinearVel(Body, 0f, 0f, 0f);
                             d.BodySetForce(Body, 0, 0, 0);
                             enableBodySoft();
@@ -1904,7 +1897,7 @@ Console.WriteLine(" JointCreateFixed");
                         // 35x10 = 350n times the mass per second applied maximum.
                         float nmax = 35f * m_mass;
                         float nmin = -35f * m_mass;
-                    
+
                         if (fx > nmax)
                             fx = nmax;
                         if (fx < nmin)
@@ -1923,7 +1916,7 @@ Console.WriteLine(" JointCreateFixed");
               //  _zeroPosition = d.BodyGetPosition(Body);
                 return;
 //Console.WriteLine("Nothing " +  Name);
-               
+
             }
         }
 
@@ -1940,8 +1933,8 @@ Console.WriteLine(" JointCreateFixed");
                 d.BodySetQuaternion(Body, ref myrot);
                 if (IsPhysical)
                 {
-                    if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
-                        createAMotor(m_angularlock);
+                    // create or remove locks
+                    createAMotor(m_angularlock);
                 }
             }
             else
@@ -1949,7 +1942,7 @@ Console.WriteLine(" JointCreateFixed");
                 // daughter prim, do Geom set
                 d.GeomSetQuaternion(prim_geom, ref myrot);
             }
-            
+
             resetCollisionAccounting();
             m_taintrot = _orientation;
         }
@@ -2087,7 +2080,7 @@ Console.WriteLine(" JointCreateFixed");
                     else
                         m_assetFailed = false;
                 }
-                    
+
             }
 
             CreateGeom(m_targetSpace, mesh);
@@ -2140,10 +2133,10 @@ Console.WriteLine(" JointCreateFixed");
             }
 
             if (m_assetFailed)
-                d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
+                d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits);
             else
 
-                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
         }
         /// <summary>
         /// Change prim in response to a shape taint.
@@ -2319,7 +2312,7 @@ Console.WriteLine(" JointCreateFixed");
                         }
                         d.BodyEnable(Body);
                         d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
-                        
+
                     }
                     m_angularforcelist.Clear();
                 }
@@ -2349,7 +2342,7 @@ Console.WriteLine(" JointCreateFixed");
                         d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
                     }
                 }
-                
+
                 //resetCollisionAccounting();
             }
 
@@ -2659,16 +2652,46 @@ Console.WriteLine(" JointCreateFixed");
 
         public override void CrossingFailure()
         {
-            m_crossingfailures++;
-            if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
-            {
-                base.RaiseOutOfBounds(_position);
-                return;
-            }
-            else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+            /*
+                        m_crossingfailures++;
+                        if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+                        {
+                            base.RaiseOutOfBounds(_position);
+                            return;
+                        }
+                        else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+                        {
+                            m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name);
+                        }
+            */
+
+            d.AllocateODEDataForThread(0U);
+
+            _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
+            _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
+            _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
+
+            m_lastposition = _position;
+            _velocity.X = 0;
+            _velocity.Y = 0;
+            _velocity.Z = 0;
+
+            m_lastVelocity = _velocity;
+
+            if (Body != IntPtr.Zero)
             {
-                m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name);
+                d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+                d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
             }
+
+            if(m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
+                m_vehicle.Stop(); // this also updates vehicle last position from the body position
+
+            enableBodySoft();
+
+            outofBounds = false;
+            base.RequestPhysicsterseUpdate();
+
         }
 
         public override float Buoyancy
@@ -2687,32 +2710,23 @@ Console.WriteLine(" JointCreateFixed");
             m_taintparent = null;
         }
 
-        public override void LockAngularMotion(Vector3 axis)
+        public override void LockAngularMotion(byte axislocks)
         {
-            // reverse the zero/non zero values for ODE.
-            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 = axis;
-            }
-            else
-            {
-                m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
-            }
+            // m_log.DebugFormat("[axislocks]: {0}", axislocks);
+            m_taintAngularLock = axislocks;
         }
 
         internal void UpdatePositionAndVelocity()
         {
             //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
+            if (outofBounds)
+                return;
             if (_parent == null)
             {
                 Vector3 pv = Vector3.Zero;
                 bool lastZeroFlag = _zeroFlag;
                 float m_minvelocity = 0;
-                if (Body != (IntPtr)0) // FIXME -> or if it is a joint
+                if (Body != IntPtr.Zero) // FIXME -> or if it is a joint
                 {
                     d.Vector3 vec = d.BodyGetPosition(Body);
                     d.Quaternion ori = d.BodyGetQuaternion(Body);
@@ -2723,12 +2737,6 @@ Console.WriteLine(" JointCreateFixed");
                     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); }
-                    //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
-                    //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
-                    //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
-
                     m_lastposition = _position;
                     m_lastorientation = _orientation;
 
@@ -2740,26 +2748,6 @@ Console.WriteLine(" JointCreateFixed");
                     l_orientation.Z = ori.Z;
                     l_orientation.W = ori.W;
 
-                    if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f)
-                    {
-                        //base.RaiseOutOfBounds(l_position);
-
-                        if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
-                        {
-                            _position = l_position;
-                            //_parent_scene.remActivePrim(this);
-                            if (_parent == null)
-                                base.RequestPhysicsterseUpdate();
-                            return;
-                        }
-                        else
-                        {
-                            if (_parent == null)
-                                base.RaiseOutOfBounds(l_position);
-                            return;
-                        }
-                    }
-
                     if (l_position.Z < 0)
                     {
                         // This is so prim that get lost underground don't fall forever and suck up
@@ -2769,8 +2757,6 @@ Console.WriteLine(" JointCreateFixed");
                         // It's a hack and will generate a console message if it fails.
 
                         //IsPhysical = false;
-                        if (_parent == null)
-                            base.RaiseOutOfBounds(_position);
 
                         _acceleration.X = 0;
                         _acceleration.Y = 0;
@@ -2784,16 +2770,65 @@ Console.WriteLine(" JointCreateFixed");
                         m_rotationalVelocity.Z = 0;
 
                         if (_parent == null)
+                            base.RaiseOutOfBounds(_position);
+
+                        if (_parent == null)
                             base.RequestPhysicsterseUpdate();
 
                         m_throttleUpdates = false;
                         throttleCounter = 0;
                         _zeroFlag = true;
                         //outofBounds = true;
+                        return;
+                    }
+
+                    if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f)
+                    {
+                        //base.RaiseOutOfBounds(l_position);
+                        /*
+                                                if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+                                                {
+                                                    _position = l_position;
+                                                    //_parent_scene.remActivePrim(this);
+                                                    if (_parent == null)
+                                                        base.RequestPhysicsterseUpdate();
+                                                    return;
+                                                }
+                                                else
+                                                {
+                                                    if (_parent == null)
+                                                        base.RaiseOutOfBounds(l_position);
+                                                    return;
+                                                }
+                        */
+                        outofBounds = true;
+                        // part near the border on outside
+                        if (l_position.X < 0)
+                            Util.Clamp(l_position.X, -0.1f, -2f);
+                        else
+                            Util.Clamp(l_position.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
+                        if (l_position.Y < 0)
+                            Util.Clamp(l_position.Y, -0.1f, -2f);
+                        else
+                            Util.Clamp(l_position.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
+
+                        d.BodySetPosition(Body, l_position.X, l_position.Y, l_position.Z);
+
+                        // stop it
+                        d.BodySetAngularVel(Body, 0, 0, 0);
+                        d.BodySetLinearVel(Body, 0, 0, 0);
+                        disableBodySoft();
+
+                        _position = l_position;
+                        // tell framework to fix it
+                        if (_parent == null)
+                            base.RequestPhysicsterseUpdate();
+                        return;
                     }
 
+
                     //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
-//Console.WriteLine("Adiff " + Name + " = " + Adiff);
+                    //Console.WriteLine("Adiff " + Name + " = " + Adiff);
                     if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
                         && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
                         && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
@@ -2864,12 +2899,12 @@ Console.WriteLine(" JointCreateFixed");
                         _acceleration = ((_velocity - m_lastVelocity) / 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());
-                       
-                        // Note here that linearvelocity is affecting angular velocity...  so I'm guessing this is a vehicle specific thing... 
-                        // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large. 
+
+                        // Note here that linearvelocity is affecting angular velocity...  so I'm guessing this is a vehicle specific thing...
+                        // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large.
                         // reducing this to 0.02m/frame seems to help the angular rubberbanding quite a bit, however, to make sure it doesn't affect elevators and vehicles
                         // adding these logical exclusion situations to maintain this where I think it was intended to be.
-                        if (m_throttleUpdates || PIDActive || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) 
+                        if (m_throttleUpdates || PIDActive || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero))
                         {
                             m_minvelocity = 0.5f;
                         }
@@ -2938,8 +2973,8 @@ Console.WriteLine(" JointCreateFixed");
         {
         }
 
-        public override Vector3 PIDTarget 
-        { 
+        public override Vector3 PIDTarget
+        {
             set
             {
                 if (value.IsFinite())
@@ -2948,16 +2983,17 @@ Console.WriteLine(" JointCreateFixed");
                 }
                 else
                     m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
-            } 
+            }
         }
+
         public override bool PIDActive { get; set; }
         public override float PIDTau { set { m_PIDTau = value; } }
 
         public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
-        public override bool PIDHoverActive { set { m_useHoverPID = value; } }
+        public override bool PIDHoverActive { get { return m_useHoverPID;} set { m_useHoverPID = value; } }
         public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
         public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
-        
+
         public override Quaternion APIDTarget{ set { return; } }
 
         public override bool APIDActive{ set { return; } }
@@ -2966,7 +3002,7 @@ Console.WriteLine(" JointCreateFixed");
 
         public override float APIDDamping{ set { return; } }
 
-        private void createAMotor(Vector3 axis)
+        private void createAMotor(byte axislock)
         {
             if (Body == IntPtr.Zero)
                 return;
@@ -2977,150 +3013,105 @@ Console.WriteLine(" JointCreateFixed");
                 Amotor = IntPtr.Zero;
             }
 
-            float axisnum = 3;
-
-            axisnum = (axisnum - (axis.X + axis.Y + axis.Z));
-
-            // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z);
-
-            
-            // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again.
-            d.Mass objMass;
-            d.MassSetZero(out objMass);
-            DMassCopy(ref pMass, ref objMass);
-
-            //m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
-
-            Matrix4 dMassMat = FromDMass(objMass);
-
-            Matrix4 mathmat = Inverse(dMassMat);
-
-            /*
-            //m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]);
-
-            mathmat = Inverse(mathmat);
-
-
-            objMass = FromMatrix4(mathmat, ref objMass);
-            //m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
-
-            mathmat = Inverse(mathmat);
-            */
-            if (axis.X == 0)
-            {
-                mathmat.M33 = 50.0000001f;
-                //objMass.I.M22 = 0;
-            }
-            if (axis.Y == 0)
-            {
-                mathmat.M22 = 50.0000001f;
-                //objMass.I.M11 = 0;
-            }
-            if (axis.Z == 0)
-            {
-                mathmat.M11 = 50.0000001f;
-                //objMass.I.M00 = 0;
-            }
-            
-            
+            if(axislock == 0)
+                return;
 
-            mathmat = Inverse(mathmat);
-            objMass = FromMatrix4(mathmat, ref objMass);
-            //m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
-           
-            //return;
-            if (d.MassCheck(ref objMass))
-            {
-                d.BodySetMass(Body, ref objMass);
-            }
-            else
-            {
-                //m_log.Debug("[PHYSICS]: Mass invalid, ignoring");
-            }
+            int axisnum = 0;
+            bool axisX = false;
+            bool axisY = false;
+            bool axisZ = false;
+            if((axislock & 0x02) != 0)
+                {
+                axisnum++;
+                axisX = true;
+                }
+            if((axislock & 0x04) != 0)
+                {
+                axisnum++;
+                axisY = true;
+                }
+            if((axislock & 0x08) != 0)
+                {
+                axisnum++;
+                axisZ = true;
+                }
 
-            if (axisnum <= 0)
+            if(axisnum == 0)
                 return;
-            // int dAMotorEuler = 1;
+            // stop it
+            d.BodySetTorque(Body, 0, 0, 0);
+            d.BodySetAngularVel(Body, 0, 0, 0);
 
             Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
             d.JointAttach(Amotor, Body, IntPtr.Zero);
+
             d.JointSetAMotorMode(Amotor, 0);
 
-            d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
-            int i = 0;
+            d.JointSetAMotorNumAxes(Amotor, axisnum);
 
-            if (axis.X == 0)
-            {
-                d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0);
-                i++;
-            }
+            // get current orientation to lock
 
-            if (axis.Y == 0)
-            {
-                d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0);
-                i++;
-            }
+            d.Quaternion dcur = d.BodyGetQuaternion(Body);
+            Quaternion curr; // crap convertion between identical things
+            curr.X = dcur.X;
+            curr.Y = dcur.Y;
+            curr.Z = dcur.Z;
+            curr.W = dcur.W;
+            Vector3 ax;
 
-            if (axis.Z == 0)
-            {
-                d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1);
+            int i = 0;
+            int j = 0;
+            if (axisX)
+            {
+                ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X
+                d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
+                d.JointSetAMotorAngle(Amotor, 0, 0);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.CFM, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);
+                i++;
+                j = 256; // move to next axis set
+            }
+
+            if (axisY)
+            {
+                ax = (new Vector3(0, 1, 0)) * curr;
+                d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
+                d.JointSetAMotorAngle(Amotor, i, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
                 i++;
+                j += 256;
             }
 
-            for (int j = 0; j < (int)axisnum; j++)
+            if (axisZ)
             {
-                //d.JointSetAMotorAngle(Amotor, j, 0);
+                ax = (new Vector3(0, 0, 1)) * curr;
+                d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
+                d.JointSetAMotorAngle(Amotor, i, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
             }
-
-            //d.JointSetAMotorAngle(Amotor, 1, 0);
-            //d.JointSetAMotorAngle(Amotor, 2, 0);
-
-            // These lowstops and high stops are effectively (no wiggle room)
-            d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
-            //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);//
-        }
-
-        private Matrix4 FromDMass(d.Mass pMass)
-        {
-            Matrix4 obj;
-            obj.M11 = pMass.I.M00;
-            obj.M12 = pMass.I.M01;
-            obj.M13 = pMass.I.M02;
-            obj.M14 = 0;
-            obj.M21 = pMass.I.M10;
-            obj.M22 = pMass.I.M11;
-            obj.M23 = pMass.I.M12;
-            obj.M24 = 0;
-            obj.M31 = pMass.I.M20;
-            obj.M32 = pMass.I.M21;
-            obj.M33 = pMass.I.M22;
-            obj.M34 = 0;
-            obj.M41 = 0;
-            obj.M42 = 0;
-            obj.M43 = 0;
-            obj.M44 = 1;
-            return obj;
-        }
-
-        private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
-        {
-            obj.I.M00 = pMat[0, 0];
-            obj.I.M01 = pMat[0, 1];
-            obj.I.M02 = pMat[0, 2];
-            obj.I.M10 = pMat[1, 0];
-            obj.I.M11 = pMat[1, 1];
-            obj.I.M12 = pMat[1, 2];
-            obj.I.M20 = pMat[2, 0];
-            obj.I.M21 = pMat[2, 1];
-            obj.I.M22 = pMat[2, 2];
-            return obj;
         }
 
         public override void SubscribeEvents(int ms)
@@ -3135,7 +3126,7 @@ Console.WriteLine(" JointCreateFixed");
             m_eventsubscription = 0;
         }
 
-        public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
+        public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
         {
             CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
         }
@@ -3165,173 +3156,6 @@ Console.WriteLine(" JointCreateFixed");
             return false;
         }
 
-        public static Matrix4 Inverse(Matrix4 pMat)
-        {
-            if (determinant3x3(pMat) == 0)
-            {
-                return Matrix4.Identity; // should probably throw an error.  singluar matrix inverse not possible
-            }
-
-            return (Adjoint(pMat) / determinant3x3(pMat));
-        }
-
-        public static Matrix4 Adjoint(Matrix4 pMat)
-        {
-            Matrix4 adjointMatrix = new Matrix4();
-            for (int i=0; i<4; i++)
-            {
-                for (int j=0; j<4; j++)
-                {
-                    Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
-                }
-            }
-
-            adjointMatrix = Transpose(adjointMatrix);
-            return adjointMatrix;
-        }
-
-        public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
-        {
-            Matrix4 minor = new Matrix4();
-            int m = 0, n = 0;
-            for (int i = 0; i < 4; i++)
-            {
-                if (i == iRow)
-                    continue;
-                n = 0;
-                for (int j = 0; j < 4; j++)
-                {
-                    if (j == iCol)
-                        continue;
-                    Matrix4SetValue(ref minor, m,n, matrix[i, j]);
-                    n++;
-                }
-                m++;
-            }
-
-            return minor;
-        }
-
-        public static Matrix4 Transpose(Matrix4 pMat)
-        {
-            Matrix4 transposeMatrix = new Matrix4();
-            for (int i = 0; i < 4; i++)
-                for (int j = 0; j < 4; j++)
-                    Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
-            return transposeMatrix;
-        }
-
-        public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
-        {
-            switch (r)
-            {
-                case 0:
-                    switch (c)
-                    {
-                        case 0:
-                            pMat.M11 = val;
-                            break;
-                        case 1:
-                            pMat.M12 = val;
-                            break;
-                        case 2:
-                            pMat.M13 = val;
-                            break;
-                        case 3:
-                            pMat.M14 = val;
-                            break;
-                    }
-
-                    break;
-                case 1:
-                    switch (c)
-                    {
-                        case 0:
-                            pMat.M21 = val;
-                            break;
-                        case 1:
-                            pMat.M22 = val;
-                            break;
-                        case 2:
-                            pMat.M23 = val;
-                            break;
-                        case 3:
-                            pMat.M24 = val;
-                            break;
-                    }
-
-                    break;
-                case 2:
-                    switch (c)
-                    {
-                        case 0:
-                            pMat.M31 = val;
-                            break;
-                        case 1:
-                            pMat.M32 = val;
-                            break;
-                        case 2:
-                            pMat.M33 = val;
-                            break;
-                        case 3:
-                            pMat.M34 = val;
-                            break;
-                    }
-
-                    break;
-                case 3:
-                    switch (c)
-                    {
-                        case 0:
-                            pMat.M41 = val;
-                            break;
-                        case 1:
-                            pMat.M42 = val;
-                            break;
-                        case 2:
-                            pMat.M43 = val;
-                            break;
-                        case 3:
-                            pMat.M44 = val;
-                            break;
-                    }
-
-                    break;
-            }
-        }
-
-        private static float determinant3x3(Matrix4 pMat)
-        {
-            float det = 0;
-            float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2];
-            float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0];
-            float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1];
-            float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2];
-            float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0];
-            float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1];
-
-            det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
-            return det;
-        }
-        
-        private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
-        {
-            dst.c.W = src.c.W;
-            dst.c.X = src.c.X;
-            dst.c.Y = src.c.Y;
-            dst.c.Z = src.c.Z;
-            dst.mass = src.mass;
-            dst.I.M00 = src.I.M00;
-            dst.I.M01 = src.I.M01;
-            dst.I.M02 = src.I.M02;
-            dst.I.M10 = src.I.M10;
-            dst.I.M11 = src.I.M11;
-            dst.I.M12 = src.I.M12;
-            dst.I.M20 = src.I.M20;
-            dst.I.M21 = src.I.M21;
-            dst.I.M22 = src.I.M22;
-        }
-
         public override void SetMaterial(int pMaterial)
         {
             m_material = pMaterial;
@@ -3365,7 +3189,7 @@ Console.WriteLine(" JointCreateFixed");
 //                m_assetFailed = false;
 
 //                m_log.DebugFormat(
-//                    "[ODE PRIM]: Received mesh/sculpt data asset {0} with {1} bytes for {2} at {3} in {4}", 
+//                    "[ODE PRIM]: Received mesh/sculpt data asset {0} with {1} bytes for {2} at {3} in {4}",
 //                    _pbs.SculptTexture, _pbs.SculptData.Length, Name, _position, _parent_scene.Name);
 
                 m_taintshape = true;
@@ -3377,6 +3201,6 @@ Console.WriteLine(" JointCreateFixed");
                     "[ODE PRIM]: Could not get mesh/sculpt asset {0} for {1} at {2} in {3}",
                     _pbs.SculptTexture, Name, _position, _parent_scene.PhysicsSceneName);
             }
-        }          
+        }
     }
 }
\ No newline at end of file