2 files changed, 369 insertions, 98 deletions

diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index 8e92171..b824cbe 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -34,6 +34,26 @@ using OpenSim.Region.Physics.Manager;
 
 namespace OpenSim.Region.Physics.OdePlugin
 {
+    /// <summary>
+    /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
+    /// </summary>
+ 
+    public enum dParam : int
+    {
+        LowStop = 0,
+        HiStop = 1,
+        Vel = 2,
+        FMax = 3,
+        FudgeFactor = 4,
+        Bounce = 5,
+        CFM = 6,
+        ERP = 7,
+        StopCFM = 8,
+        LoStop2 = 256,
+        HiStop2 = 257,
+        LoStop3 = 512,
+        HiStop3 = 513
+    }
     public class OdeCharacter : PhysicsActor
     {
         private PhysicsVector _position;
@@ -45,7 +65,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         private PhysicsVector _target_velocity;
         private PhysicsVector _acceleration;
         private PhysicsVector m_rotationalVelocity;
-        private float m_density = 50f;
+        private float m_mass = 80f;
+        private float m_density = 60f;
         private bool m_pidControllerActive = true;
         private static float PID_D = 3020.0f;
         private static float PID_P = 7000.0f;
@@ -96,34 +117,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             lock (OdeScene.OdeLock)
             {
-                int dAMotorEuler = 1;
-                Shell = d.CreateCapsule(parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH);
-                d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH);
-                Body = d.BodyCreate(parent_scene.world);
-                d.BodySetMass(Body, ref ShellMass);
-                d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
-                d.GeomSetBody(Shell, Body);
-
-
-                d.BodySetRotation(Body, ref m_StandUpRotation);
-
-
-                //Amotor = d.JointCreateAMotor(parent_scene.world, IntPtr.Zero); 
-                //d.JointAttach(Amotor, Body, IntPtr.Zero);
-                //d.JointSetAMotorMode(Amotor, dAMotorEuler);
-                //d.JointSetAMotorNumAxes(Amotor, 3);
-                //d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
-                //d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
-                ///d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
-                //d.JointSetAMotorAngle(Amotor, 0, 0);
-                //d.JointSetAMotorAngle(Amotor, 1, 0);
-                //d.JointSetAMotorAngle(Amotor, 2, 0);
-                //d.JointSetAMotorParam(Amotor, 0, -0);
-                //d.JointSetAMotorParam(Amotor, 0x200, -0);
-                //d.JointSetAMotorParam(Amotor, 0x100, -0);
-                // d.JointSetAMotorParam(Amotor, 0, 0);
-                // d.JointSetAMotorParam(Amotor, 3, 0);
-                // d.JointSetAMotorParam(Amotor, 2, 0);
+                AvatarGeomAndBodyCreation(pos.X, pos.Y, pos.Z);
             }
             m_name = avName;
             parent_scene.geom_name_map[Shell] = avName;
@@ -136,6 +130,9 @@ namespace OpenSim.Region.Physics.OdePlugin
             set { return; }
         }
 
+        /// <summary>
+        /// If this is set, the avatar will move faster
+        /// </summary>
         public override bool SetAlwaysRun
         {
             get { return m_alwaysRun; }
@@ -160,6 +157,10 @@ namespace OpenSim.Region.Physics.OdePlugin
             set { flying = value; }
         }
 
+        /// <summary>
+        /// Returns if the avatar is colliding in general.
+        /// This includes the ground and objects and avatar.
+        /// </summary>
         public override bool IsColliding
         {
             get { return m_iscolliding; }
@@ -208,11 +209,17 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+        /// <summary>
+        /// Returns if an avatar is colliding with the ground
+        /// </summary>
         public override bool CollidingGround
         {
             get { return m_iscollidingGround; }
             set
             {
+                // Collisions against the ground are not really reliable
+                // So, to get a consistant value we have to average the current result over time
+                // Currently we use 1 second = 10 calls to this.
                 int i;
                 int truecount = 0;
                 int falsecount = 0;
@@ -256,6 +263,9 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+        /// <summary>
+        /// Returns if the avatar is colliding with an object
+        /// </summary>
         public override bool CollidingObj
         {
             get { return m_iscollidingObj; }
@@ -269,11 +279,21 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+        /// <summary>
+        /// turn the PID controller on or off.   
+        /// The PID Controller will turn on all by itself in many situations
+        /// </summary>
+        /// <param name="status"></param>
         public void SetPidStatus(bool status)
         {
             m_pidControllerActive = status;
         }
 
+        /// <summary>
+        /// This 'puts' an avatar somewhere in the physics space.
+        /// 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
         {
             get { return _position; }
@@ -293,6 +313,10 @@ namespace OpenSim.Region.Physics.OdePlugin
             set { m_rotationalVelocity = value; }
         }
 
+        /// <summary>
+        /// 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
         {
             get { return new PhysicsVector(CAPSULE_RADIUS*2, CAPSULE_RADIUS*2, CAPSULE_LENGTH); }
@@ -301,6 +325,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 m_pidControllerActive = true;
                 lock (OdeScene.OdeLock)
                 {
+                    d.JointDestroy(Amotor);
                     PhysicsVector SetSize = value;
                     float prevCapsule = CAPSULE_LENGTH;
                     float capsuleradius = CAPSULE_RADIUS;
@@ -309,20 +334,91 @@ namespace OpenSim.Region.Physics.OdePlugin
                     CAPSULE_LENGTH = (SetSize.Z - ((SetSize.Z*0.43f))); // subtract 43% of the size
                     d.BodyDestroy(Body);
                     d.GeomDestroy(Shell);
-                    //MainLog.Instance.Verbose("PHYSICS", "Set Avatar Height To: " + (CAPSULE_RADIUS + CAPSULE_LENGTH));
-                    Shell = d.CreateCapsule(_parent_scene.space, capsuleradius, CAPSULE_LENGTH);
-                    d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH);
-                    Body = d.BodyCreate(_parent_scene.world);
-                    d.BodySetMass(Body, ref ShellMass);
-                    d.BodySetPosition(Body, _position.X, _position.Y,
-                                      _position.Z + Math.Abs(CAPSULE_LENGTH - prevCapsule));
-                    d.GeomSetBody(Shell, Body);
+                    AvatarGeomAndBodyCreation(_position.X, _position.Y,
+                                      _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule)*2));
+                    Velocity = new PhysicsVector(0f, 0f, 0f);
+                    
                 }
                 _parent_scene.geom_name_map[Shell] = m_name;
                 _parent_scene.actor_name_map[Shell] = (PhysicsActor) this;
             }
         }
+        /// <summary>
+        /// This creates the Avatar's physical Surrogate at the position supplied
+        /// </summary>
+        /// <param name="npositionX"></param>
+        /// <param name="npositionY"></param>
+        /// <param name="npositionZ"></param>
+        private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
+        {
+            int dAMotorEuler = 1;
+            Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH);
+            d.MassSetCapsuleTotal(out ShellMass, m_mass, 2, CAPSULE_RADIUS, CAPSULE_LENGTH);
+            Body = d.BodyCreate(_parent_scene.world);
+            d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
+
+            d.BodySetMass(Body, ref ShellMass);
+
+            // 90 Stand up on the cap of the capped cyllinder
+            d.RFromAxisAndAngle(out m_StandUpRotation, 1, 0, 0, (float)(Math.PI / 2));
+
+
+            d.GeomSetRotation(Shell, ref m_StandUpRotation);
+            d.BodySetRotation(Body, ref m_StandUpRotation);
+
+            d.GeomSetBody(Shell, Body);
+
+
+            // The purpose of the AMotor here is to keep the avatar's physical 
+            // surrogate from rotating while moving
+            Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
+            d.JointAttach(Amotor, Body, IntPtr.Zero);
+            d.JointSetAMotorMode(Amotor, dAMotorEuler);
+            d.JointSetAMotorNumAxes(Amotor, 3);
+            d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
+            d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
+            d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
+            d.JointSetAMotorAngle(Amotor, 0, 0);
+            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, -0.000000000001f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.000000000001f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.000000000001f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.000000000001f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.000000000001f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.000000000001f);
+
+            // Fudge factor is 1f by default, we're setting it to 0.  We don't want it to Fudge or the 
+            // capped cyllinder will fall over
+            d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 3800000f);
+
+
+            // The purpose of this routine here is to quickly stabilize the Body while it's popped up in the air.
+            // The amotor needs a few seconds to stabilize so without it, the avatar shoots up sky high when you 
+            // change appearance and when you enter the simulator
+            // After this routine is done, the amotor stabilizes much quicker
+            d.Vector3 feet;
+            d.Vector3 head;
+            d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet);
+            d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head);
+            float posture = head.Z - feet.Z;
+
+            // restoring force proportional to lack of posture:
+            float servo = (2.5f - posture) * POSTURE_SERVO;
+            d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f);
+            d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f);
+            
+            
+        }
 
+        // 
+        /// <summary>
+        /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
+        /// This may be used in calculations in the scene/scenepresence
+        /// </summary>
         public override float Mass
         {
             get
@@ -385,6 +481,11 @@ namespace OpenSim.Region.Physics.OdePlugin
             _acceleration = accel;
         }
 
+        /// <summary>
+        /// Adds the force supplied to the Target Velocity 
+        /// 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)
         {
             m_pidControllerActive = true;
@@ -395,29 +496,15 @@ namespace OpenSim.Region.Physics.OdePlugin
             //m_lastUpdateSent = false;
         }
 
+        /// <summary>
+        /// 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)
         {
             if (!collidelock)
             {
                 d.BodyAddForce(Body, force.X, force.Y, force.Z);
-
-                //  ok -- let's stand up straight!
-                //d.Matrix3 StandUpRotationalMatrix = new d.Matrix3(0.8184158f, -0.5744568f, -0.0139677f, 0.5744615f, 0.8185215f, -0.004074608f, 0.01377355f, -0.004689182f, 0.9998941f);
-                //d.BodySetRotation(Body, ref StandUpRotationalMatrix);
-                //d.BodySetRotation(Body, ref m_StandUpRotation);
-                // The above matrix was generated with the amazing standup routine below by danX0r *cheer*
-                d.Vector3 feet;
-                d.Vector3 head;
-                d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet);
-                d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head);
-                float posture = head.Z - feet.Z;
-
-                // restoring force proportional to lack of posture:
-                float servo = (2.5f - posture) * POSTURE_SERVO;
-                d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f);
-                d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f);
-
-                //m_lastUpdateSent = false;
             }
         }
 
@@ -425,9 +512,19 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
         }
 
+
+        /// <summary>
+        /// Called from Simulate
+        /// This is the avatar's movement control + PID Controller
+        /// </summary>
+        /// <param name="timeStep"></param>
         public void Move(float timeStep)
         {
             //  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
+
             if (m_pidControllerActive == false)
             {
                 _zeroPosition = d.BodyGetPosition(Body);
@@ -458,6 +555,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
                 if (m_pidControllerActive)
                 {
+                    // 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
+                    // Prim to avatar collisions
+
                     d.Vector3 pos = d.BodyGetPosition(Body);
                     vec.X = (_target_velocity.X - vel.X)*PID_D + (_zeroPosition.X - pos.X)*PID_P;
                     vec.Y = (_target_velocity.Y - vel.Y)*PID_D + (_zeroPosition.Y - pos.Y)*PID_P;
@@ -474,11 +576,14 @@ namespace OpenSim.Region.Physics.OdePlugin
                 _zeroFlag = false;
                 if (m_iscolliding || flying)
                 {
+                    // We're flying and colliding with something
                     vec.X = ((_target_velocity.X/movementdivisor) - vel.X)*PID_D;
                     vec.Y = ((_target_velocity.Y/movementdivisor) - vel.Y)*PID_D;
                 }
                 if (m_iscolliding && !flying && _target_velocity.Z > 0.0f)
                 {
+                    // We're colliding with something and we're not flying but we're moving
+                    // This means we're walking or running.
                     d.Vector3 pos = d.BodyGetPosition(Body);
                     vec.Z = (_target_velocity.Z - vel.Z)*PID_D + (_zeroPosition.Z - pos.Z)*PID_P;
                     if (_target_velocity.X > 0)
@@ -492,6 +597,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
                 else if (!m_iscolliding && !flying)
                 {
+                    // we're not colliding and we're not flying so that means we're falling!
+                    // m_iscolliding includes collisions with the ground.
                     d.Vector3 pos = d.BodyGetPosition(Body);
                     if (_target_velocity.X > 0)
                     {
@@ -518,6 +625,9 @@ namespace OpenSim.Region.Physics.OdePlugin
             doForce(vec);
         }
 
+        /// <summary>
+        /// Updates the reported position and velocity.  This essentially sends the data up to ScenePresence.
+        /// </summary>
         public void UpdatePositionAndVelocity()
         {
             //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
@@ -533,25 +643,21 @@ namespace OpenSim.Region.Physics.OdePlugin
             _position.Y = vec.Y;
             _position.Z = vec.Z;
 
+            // Did we move last? = zeroflag
+            // This helps keep us from sliding all over
+
             if (_zeroFlag)
             {
                 _velocity.X = 0.0f;
                 _velocity.Y = 0.0f;
                 _velocity.Z = 0.0f;
+            
+                // Did we send out the 'stopped' message?
                 if (!m_lastUpdateSent)
                 {
                     m_lastUpdateSent = true;
                     base.RequestPhysicsterseUpdate();
-                    //string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
-                    //int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
-                    //if (primScenAvatarIn == "0")
-                    //{
-                    //MainLog.Instance.Verbose("Physics", "Avatar " + m_name + " in space with no prim. Arr:':" + arrayitem[0].ToString() + "," + arrayitem[1].ToString());
-                    //}
-                    //else
-                    //{
-                    //    MainLog.Instance.Verbose("Physics", "Avatar " + m_name + " in Prim space':" + primScenAvatarIn + ". Arr:" + arrayitem[0].ToString() + "," + arrayitem[1].ToString());
-                    //}
+                    
                 }
             }
             else
@@ -564,6 +670,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 _velocity.Z = (vec.Z);
                 if (_velocity.Z < -6 && !m_hackSentFall)
                 {
+                    // Collisionupdates will be used in the future, right now the're not being used.
                     m_hackSentFall = true;
                     //base.SendCollisionUpdate(new CollisionEventUpdate());
                     m_pidControllerActive = false;
@@ -581,13 +688,21 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+        /// <summary>
+        /// Cleanup the things we use in the scene.
+        /// </summary>
         public void Destroy()
         {
             lock (OdeScene.OdeLock)
             {
-                // d.JointDestroy(Amotor);
+                // Kill the Amotor
+                d.JointDestroy(Amotor);
+
+                //kill the Geometry
                 d.GeomDestroy(Shell);
                 _parent_scene.geom_name_map.Remove(Shell);
+                
+                //kill the body
                 d.BodyDestroy(Body);
             }
         }
diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
index 17dbd0a..366f30b 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
@@ -86,6 +86,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private IntPtr contactgroup;
         private IntPtr LandGeom = (IntPtr) 0;
         private double[] _heightmap;
+        private float[] _origheightmap;
         private d.NearCallback nearCallback;
         public d.TriCallback triCallback;
         public d.TriArrayCallback triArrayCallback;
@@ -115,6 +116,12 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public IMesher mesher;
 
+
+        /// <summary>
+        /// Initiailizes the scene
+        /// Sets many properties that ODE requires to be stable
+        /// These settings need to be tweaked 'exactly' right or weird stuff happens.
+        /// </summary>
         public OdeScene()
         {
             nearCallback = near;
@@ -128,17 +135,27 @@ namespace OpenSim.Region.Physics.OdePlugin
             contact.surface.soft_cfm = 0.00003f;
             */
 
+            // Centeral contact friction and bounce
             contact.surface.mu = 250.0f;
             contact.surface.bounce = 0.2f;
 
+            // Terrain contact friction and Bounce 
+            // This is the *non* moving version.   Use this when an avatar 
+            // isn't moving to keep it in place better
             TerrainContact.surface.mode |= d.ContactFlags.SoftERP;
             TerrainContact.surface.mu = 550.0f;
             TerrainContact.surface.bounce = 0.1f;
             TerrainContact.surface.soft_erp = 0.1025f;
 
+            // Prim contact friction and bounce
+            // THis is the *non* moving version of friction and bounce 
+            // Use this when an avatar comes in contact with a prim
+            // and is moving
             AvatarMovementprimContact.surface.mu = 150.0f;
             AvatarMovementprimContact.surface.bounce = 0.1f;
 
+            // Terrain contact friction bounce and various error correcting calculations
+            // Use this when an avatar is in contact with the terrain and moving.
             AvatarMovementTerrainContact.surface.mode |= d.ContactFlags.SoftERP;
             AvatarMovementTerrainContact.surface.mu = 150.0f;
             AvatarMovementTerrainContact.surface.bounce = 0.1f;
@@ -146,6 +163,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             lock (OdeLock)
             {
+                // Creat the world and the first space
                 world = d.WorldCreate();
                 space = d.HashSpaceCreate(IntPtr.Zero);
                 d.HashSpaceSetLevels(space, -4, 128);
@@ -153,15 +171,25 @@ namespace OpenSim.Region.Physics.OdePlugin
                 //contactgroup
 
 
+                // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
+
                 d.WorldSetGravity(world, 0.0f, 0.0f, -10.0f);
                 d.WorldSetAutoDisableFlag(world, false);
                 d.WorldSetContactSurfaceLayer(world, 0.001f);
+                
+                // Set how many steps we go without running collision testing
+                // This is in addition to the step size.
+                // Essentially Steps * m_physicsiterations
                 d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
-                d.WorldSetContactMaxCorrectingVel(world, 1000.0f);
+                ///d.WorldSetContactMaxCorrectingVel(world, 1000.0f);
             }
 
+            // zero out a heightmap array float array (single dimention [flattened]))
             _heightmap = new double[514*514];
 
+
+            // Zero out the prim spaces array (we split our space into smaller spaces so 
+            // we can hit test less.
             for (int i = 0; i < staticPrimspace.GetLength(0); i++)
             {
                 for (int j = 0; j < staticPrimspace.GetLength(1); j++)
@@ -171,19 +199,35 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+        // Initialize the mesh plugin
         public override void Initialise(IMesher meshmerizer)
         {
             mesher = meshmerizer;
         }
 
+        /// <summary>
+        /// Debug space message for printing the space that a prim/avatar is in.
+        /// </summary>
+        /// <param name="pos"></param>
+        /// <returns>Returns which split up space the given position is in.</returns>
         public string whichspaceamIin(PhysicsVector pos)
         {
             return calculateSpaceForGeom(pos).ToString();
         }
 
+        /// <summary>
+        /// This is our near callback.  A geometry is near a body
+        /// </summary>
+        /// <param name="space">The space that contains the geoms.  Remember, spaces are also geoms</param>
+        /// <param name="g1">a geometry or space</param>
+        /// <param name="g2">another geometry or space</param>
         private void near(IntPtr space, IntPtr g1, IntPtr g2)
         {
             //  no lock here!  It's invoked from within Simulate(), which is thread-locked
+            
+            // Test if we're collidng a geom with a space.  
+            // If so we have to drill down into the space recursively
+
             if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
             {
                 // Separating static prim geometry spaces.   
@@ -192,7 +236,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 // contact points in the space
 
                 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
-                //Colliding a space or a geom with a space or a geom.
+                //Colliding a space or a geom with a space or a geom. so drill down
 
                 //Collide all geoms in each space..   
                 //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
@@ -226,12 +270,13 @@ namespace OpenSim.Region.Physics.OdePlugin
                     name2 = "null";
                 }
 
-                if (id == d.GeomClassID.TriMeshClass)
-                {
+                //if (id == d.GeomClassID.TriMeshClass)
+                //{
                     //               MainLog.Instance.Verbose("near: A collision was detected between {1} and {2}", 0, name1, name2);
                     //System.Console.WriteLine("near: A collision was detected between {1} and {2}", 0, name1, name2);
-                }
+                //}
 
+                // Figure out how many contact points we have
                 int count = 0;
                 try
                 {
@@ -244,13 +289,15 @@ namespace OpenSim.Region.Physics.OdePlugin
                     base.TriggerPhysicsBasedRestart();
                 }
 
+                PhysicsActor p1;
+                PhysicsActor p2;
+
                 for (int i = 0; i < count; i++)
                 {
                     IntPtr joint;
                     // If we're colliding with terrain, use 'TerrainContact' instead of contact.
                     // allows us to have different settings
-                    PhysicsActor p1;
-                    PhysicsActor p2;
+                    
 
                     if (!actor_name_map.TryGetValue(g1, out p1))
                     {
@@ -267,14 +314,14 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                     switch (p1.PhysicsActorType)
                     {
-                        case (int) ActorTypes.Agent:
+                        case (int)ActorTypes.Agent:
                             p2.CollidingObj = true;
                             break;
-                        case (int) ActorTypes.Prim:
+                        case (int)ActorTypes.Prim:
                             if (p2.Velocity.X > 0 || p2.Velocity.Y > 0 || p2.Velocity.Z > 0)
                                 p2.CollidingObj = true;
                             break;
-                        case (int) ActorTypes.Unknown:
+                        case (int)ActorTypes.Unknown:
                             p2.CollidingGround = true;
                             break;
                         default:
@@ -288,11 +335,15 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                     if (contacts[i].depth >= 0.08f)
                     {
+                        /*  This is disabled at the moment only because it needs more tweaking
+                            It will eventually be uncommented
+                         
                         if (contacts[i].depth >= 1.00f)
                         {
                             //MainLog.Instance.Debug("PHYSICS",contacts[i].depth.ToString());
                         }
-                        // If you interpenetrate a prim with an agent
+                         
+                        //If you interpenetrate a prim with an agent
                         if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
                              p1.PhysicsActorType == (int) ActorTypes.Prim) ||
                             (p1.PhysicsActorType == (int) ActorTypes.Agent &&
@@ -300,35 +351,37 @@ namespace OpenSim.Region.Physics.OdePlugin
                         {
                             if (p2.PhysicsActorType == (int) ActorTypes.Agent)
                             {   
-                                //p2.CollidingObj = true;
-                                //contacts[i].depth = 0.003f;
-                                //p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f);
-                                //OdeCharacter character = (OdeCharacter) p2;
-                                //character.SetPidStatus(true);
-                                //contacts[i].pos = new d.Vector3(contacts[i].pos.X + (p1.Size.X / 2), contacts[i].pos.Y + (p1.Size.Y / 2), contacts[i].pos.Z + (p1.Size.Z / 2));
+                                p2.CollidingObj = true;
+                                contacts[i].depth = 0.003f;
+                                p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f);
+                                OdeCharacter character = (OdeCharacter) p2;
+                                character.SetPidStatus(true);
+                                contacts[i].pos = new d.Vector3(contacts[i].pos.X + (p1.Size.X / 2), contacts[i].pos.Y + (p1.Size.Y / 2), contacts[i].pos.Z + (p1.Size.Z / 2));
                                 
                             }
                             else
                             {
                                 
-                                //contacts[i].depth = 0.0000000f;
+                                contacts[i].depth = 0.0000000f;
                             }
                             if (p1.PhysicsActorType == (int) ActorTypes.Agent)
                             {
                                 
-                                //p1.CollidingObj = true;
-                                //contacts[i].depth = 0.003f;
-                                //p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f);
-                                //contacts[i].pos = new d.Vector3(contacts[i].pos.X + (p2.Size.X / 2), contacts[i].pos.Y + (p2.Size.Y / 2), contacts[i].pos.Z + (p2.Size.Z / 2));
-                                //OdeCharacter character = (OdeCharacter)p1;
-                                //character.SetPidStatus(true);
+                                p1.CollidingObj = true;
+                                contacts[i].depth = 0.003f;
+                                p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f);
+                                contacts[i].pos = new d.Vector3(contacts[i].pos.X + (p2.Size.X / 2), contacts[i].pos.Y + (p2.Size.Y / 2), contacts[i].pos.Z + (p2.Size.Z / 2));
+                                OdeCharacter character = (OdeCharacter)p1;
+                                character.SetPidStatus(true);
                             }
                             else
                             {
                                 
-                                //contacts[i].depth = 0.0000000f;
+                                contacts[i].depth = 0.0000000f;
                             }
                         }
+                         */
+
                         // If you interpenetrate a prim with another prim
                         if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim)
                         {
@@ -337,6 +390,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         }
                         if (contacts[i].depth >= 1.00f)
                         {
+                            OpenSim.Framework.Console.MainLog.Instance.Verbose("P", contacts[i].depth.ToString());
                             if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
                                  p1.PhysicsActorType == (int) ActorTypes.Unknown) ||
                                 (p1.PhysicsActorType == (int) ActorTypes.Agent &&
@@ -347,7 +401,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                     OdeCharacter character = (OdeCharacter) p2;
 
                                     //p2.CollidingObj = true;
-                                    contacts[i].depth = 0.003f;
+                                    contacts[i].depth = 0.00000003f;
                                     p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 0.5f);
                                     contacts[i].pos =
                                         new d.Vector3(contacts[i].pos.X + (p1.Size.X/2),
@@ -363,7 +417,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                     OdeCharacter character = (OdeCharacter)p1;
 
                                     //p2.CollidingObj = true;
-                                    contacts[i].depth = 0.003f;
+                                    contacts[i].depth = 0.00000003f;
                                     p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 0.5f);
                                     contacts[i].pos =
                                         new d.Vector3(contacts[i].pos.X + (p1.Size.X/2),
@@ -383,30 +437,39 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                     if (contacts[i].depth >= 0f)
                     {
+                        // If we're collidng against terrain
                         if (name1 == "Terrain" || name2 == "Terrain")
                         {
+                            // If we're moving
                             if ((p2.PhysicsActorType == (int) ActorTypes.Agent) &&
                                 (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
                             {
+                                // Use the movement terrain contact
                                 AvatarMovementTerrainContact.geom = contacts[i];
                                 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact);
                             }
                             else
                             {
+                                // Use the non moving terrain contact
                                 TerrainContact.geom = contacts[i];
                                 joint = d.JointCreateContact(world, contactgroup, ref TerrainContact);
                             }
                         }
                         else
                         {
+                            // we're colliding with prim or avatar
+
+                            // check if we're moving
                             if ((p2.PhysicsActorType == (int) ActorTypes.Agent) &&
                                 (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
                             {
+                                // Use the Movement prim contact
                                 AvatarMovementprimContact.geom = contacts[i];
                                 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact);
                             }
                             else
                             {
+                                // Use the non movement contact
                                 contact.geom = contacts[i];
                                 joint = d.JointCreateContact(world, contactgroup, ref contact);
                             }
@@ -416,6 +479,11 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                     if (count > 3)
                     {
+                        // If there are more then 3 contact points, it's likely
+                        // that we've got a pile of objects
+                        // 
+                        // We don't want to send out hundreds of terse updates over and over again
+                        // so lets throttle them and send them again after it's somewhat sorted out.
                         p2.ThrottleUpdates = true;
                     }
                     //System.Console.WriteLine(count.ToString());
@@ -423,16 +491,43 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
             }
         }
+        private float GetTerrainHeightAtXY(float x, float y) 
+        {
+            return (float)_origheightmap[(int) y*256 + (int) x];
+            
 
+        }
+
+        /// <summary>
+        /// This is our collision testing routine in ODE
+        /// </summary>
+        /// <param name="timeStep"></param>
         private void collision_optimized(float timeStep)
         {
             foreach (OdeCharacter chr in _characters)
             {
+                // Reset the collision values to false
+                // since we don't know if we're colliding yet
+
                 chr.IsColliding = false;
                 chr.CollidingGround = false;
                 chr.CollidingObj = false;
+                
+                // test the avatar's geometry for collision with the space
+                // This will return near and the space that they are the closest to
+                // And we'll run this again against the avatar and the space segment
+                // This will return with a bunch of possible objects in the space segment
+                // and we'll run it again on all of them.
+
                 d.SpaceCollide2(space, chr.Shell, IntPtr.Zero, nearCallback);
+                //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y);
+                //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10)
+                //{
+                    //chr.Position.Z = terrainheight + 10.0f;
+                    //forcedZ = true;
+                //}
             }
+
             // If the sim is running slow this frame, 
             // don't process collision for prim!
             if (timeStep < (m_SkipFramesAtms/3))
@@ -469,6 +564,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                     // This if may not need to be there..    it might be skipped anyway.
                     if (d.BodyIsEnabled(chr.Body))
                     {
+                        // Collide test the prims with the terrain..   since if you don't do this, 
+                        // next frame, all of the physical prim in the scene will awaken and explode upwards
                         d.SpaceCollide2(LandGeom, chr.prim_geom, IntPtr.Zero, nearCallback);
                     }
                 }
@@ -509,6 +606,15 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+        /// <summary>
+        /// This is called from within simulate but outside the locked portion
+        /// We need to do our own locking here
+        /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
+        /// 
+        /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory 
+        /// that the space was using.
+        /// </summary>
+        /// <param name="prim"></param>
         public void RemovePrimThreadLocked(OdePrim prim)
         {
             lock (OdeLock)
@@ -567,7 +673,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                 _prims.Remove(prim);
             }
         }
-
+        /// <summary>
+        /// Takes a space pointer and zeros out the array we're using to hold the spaces
+        /// </summary>
+        /// <param name="space"></param>
         public void resetSpaceArrayItemToZero(IntPtr space)
         {
             for (int x = 0; x < staticPrimspace.GetLength(0); x++)
@@ -585,15 +694,25 @@ namespace OpenSim.Region.Physics.OdePlugin
             staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero;
         }
 
+        /// <summary>
+        /// Called when a static prim moves.  Allocates a space for the prim based on it's position
+        /// </summary>
+        /// <param name="geom">the pointer to the geom that moved</param>
+        /// <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)
         {
-            //Todo recalculate space the prim is in.
+            
             // Called from setting the Position and Size of an ODEPrim so 
             // it's already in locked space.
 
             // we don't want to remove the main space
             // we don't need to test physical here because this function should 
             // never be called if the prim is physical(active)
+
+            // All physical prim end up in the root space
+
             if (currentspace != space)
             {
                 if (d.SpaceQuery(currentspace, geom) && currentspace != (IntPtr) 0)
@@ -707,6 +826,12 @@ namespace OpenSim.Region.Physics.OdePlugin
             return newspace;
         }
 
+        /// <summary>
+        /// Creates a new space at X Y
+        /// </summary>
+        /// <param name="iprimspaceArrItemX"></param>
+        /// <param name="iprimspaceArrItemY"></param>
+        /// <returns>A pointer to the created space</returns>
         public IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY)
         {
             // creating a new space for prim and inserting it into main space.
@@ -715,6 +840,11 @@ namespace OpenSim.Region.Physics.OdePlugin
             return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY];
         }
 
+        /// <summary>
+        /// Calculates the space the prim should be in by it's position
+        /// </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)
         {
             int[] xyspace = calculateSpaceArrayItemFromPos(pos);
@@ -725,6 +855,11 @@ namespace OpenSim.Region.Physics.OdePlugin
             return locationbasedspace;
         }
 
+        /// <summary>
+        /// Holds the space allocation logic
+        /// </summary>
+        /// <param name="pos"></param>
+        /// <returns>an array item based on the position</returns>
         public int[] calculateSpaceArrayItemFromPos(PhysicsVector pos)
         {
             int[] returnint = new int[2];
@@ -837,7 +972,11 @@ namespace OpenSim.Region.Physics.OdePlugin
             return 1;
         }
 
-
+        /// <summary>
+        /// Routine to figure out if we need to mesh this prim with our mesher
+        /// </summary>
+        /// <param name="pbs"></param>
+        /// <returns></returns>
         public bool needsMeshing(PrimitiveBaseShape pbs)
         {
             if (pbs.ProfileHollow != 0)
@@ -879,6 +1018,12 @@ namespace OpenSim.Region.Physics.OdePlugin
             return result;
         }
 
+        /// <summary>
+        /// Called after our prim properties are set Scale, position etc.
+        /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
+        /// This assures us that we have no race conditions
+        /// </summary>
+        /// <param name="prim"></param>
         public override void AddPhysicsActorTaint(PhysicsActor prim)
         {
             if (prim is OdePrim)
@@ -889,6 +1034,15 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+        /// <summary>
+        /// This is our main simulate loop
+        /// It's thread locked by a Mutex in the scene. 
+        /// It holds Collisions, it instructs ODE to step through the physical reactions
+        /// It moves the objects around in memory
+        /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
+        /// </summary>
+        /// <param name="timeStep"></param>
+        /// <returns></returns>
         public override float Simulate(float timeStep)
         {
             float fps = 0;
@@ -904,7 +1058,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             if (step_time >= m_SkipFramesAtms)
             {
-                // Instead of trying to catch up, it'll do one physics frame only
+                // Instead of trying to catch up, it'll do 5 physics frames only
                 step_time = ODE_STEPSIZE;
                 m_physicsiterations = 5;
             }
@@ -960,6 +1114,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 {
                     actor.UpdatePositionAndVelocity();
                 }
+
                 bool processedtaints = false;
                 foreach (OdePrim prim in _taintedPrim)
                 {
@@ -970,6 +1125,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     }
                     processedtaints = true;
                 }
+
                 if (processedtaints)
                     _taintedPrim = new List<OdePrim>();
 
@@ -1152,7 +1308,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             // this._heightmap[i] = (double)heightMap[i];
             // dbm (danx0r) -- heightmap x,y must be swapped for Ode (should fix ODE, but for now...)
             // also, creating a buffer zone of one extra sample all around
-
+            _origheightmap = heightMap;
             const uint heightmapWidth = m_regionWidth + 2;
             const uint heightmapHeight = m_regionHeight + 2;
             const uint heightmapWidthSamples = 2*m_regionWidth + 2;