13 files changed, 521 insertions, 97 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
index f25b447..abbd22c 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
@@ -87,7 +87,7 @@ public enum FixedShapeKey : ulong
 [StructLayout(LayoutKind.Sequential)]
 public struct ShapeData
 {
-    public uint ID;
+    public UInt32 ID;
     public BSPhysicsShapeType Type;
     public Vector3 Position;
     public Quaternion Rotation;
@@ -111,7 +111,7 @@ public struct ShapeData
 [StructLayout(LayoutKind.Sequential)]
 public struct SweepHit
 {
-    public uint ID;
+    public UInt32 ID;
     public float Fraction;
     public Vector3 Normal;
     public Vector3 Point;
@@ -119,15 +119,15 @@ public struct SweepHit
 [StructLayout(LayoutKind.Sequential)]
 public struct RaycastHit
 {
-    public uint ID;
+    public UInt32 ID;
     public float Fraction;
     public Vector3 Normal;
 }
 [StructLayout(LayoutKind.Sequential)]
 public struct CollisionDesc
 {
-    public uint aID;
-    public uint bID;
+    public UInt32 aID;
+    public UInt32 bID;
     public Vector3 point;
     public Vector3 normal;
     public float penetration;
@@ -135,7 +135,7 @@ public struct CollisionDesc
 [StructLayout(LayoutKind.Sequential)]
 public struct EntityProperties
 {
-    public uint ID;
+    public UInt32 ID;
     public Vector3 Position;
     public Quaternion Rotation;
     public Vector3 Velocity;
@@ -325,7 +325,7 @@ public abstract BulletWorld Initialize(Vector3 maxPosition, ConfigurationParamet
 public abstract int PhysicsStep(BulletWorld world, float timeStep, int maxSubSteps, float fixedTimeStep,
                         out int updatedEntityCount, out int collidersCount);
 
-public abstract bool UpdateParameter(BulletWorld world, uint localID, String parm, float value);
+public abstract bool UpdateParameter(BulletWorld world, UInt32 localID, String parm, float value);
 
 public abstract void Shutdown(BulletWorld sim);
 
@@ -366,24 +366,24 @@ public abstract void UpdateChildTransform(BulletShape pShape, int childIndex, Ve
 
 public abstract void RecalculateCompoundShapeLocalAabb(BulletShape cShape);
 
-public abstract BulletShape DuplicateCollisionShape(BulletWorld sim, BulletShape srcShape, uint id);
+public abstract BulletShape DuplicateCollisionShape(BulletWorld sim, BulletShape srcShape, UInt32 id);
 
 public abstract bool DeleteCollisionShape(BulletWorld world, BulletShape shape);
 
 public abstract CollisionObjectTypes GetBodyType(BulletBody obj);
 
-public abstract BulletBody CreateBodyFromShape(BulletWorld sim, BulletShape shape, uint id, Vector3 pos, Quaternion rot);
+public abstract BulletBody CreateBodyFromShape(BulletWorld sim, BulletShape shape, UInt32 id, Vector3 pos, Quaternion rot);
 
-public abstract BulletBody CreateBodyWithDefaultMotionState(BulletShape shape, uint id, Vector3 pos, Quaternion rot);
+public abstract BulletBody CreateBodyWithDefaultMotionState(BulletShape shape, UInt32 id, Vector3 pos, Quaternion rot);
 
-public abstract BulletBody CreateGhostFromShape(BulletWorld sim, BulletShape shape, uint id, Vector3 pos, Quaternion rot);
+public abstract BulletBody CreateGhostFromShape(BulletWorld sim, BulletShape shape, UInt32 id, Vector3 pos, Quaternion rot);
 
 public abstract void DestroyObject(BulletWorld sim, BulletBody obj);
 
 // =====================================================================================
-public abstract BulletShape CreateGroundPlaneShape(uint id, float height, float collisionMargin);
+public abstract BulletShape CreateGroundPlaneShape(UInt32 id, float height, float collisionMargin);
 
-public abstract BulletShape CreateTerrainShape(uint id, Vector3 size, float minHeight, float maxHeight, float[] heightMap, 
+public abstract BulletShape CreateTerrainShape(UInt32 id, Vector3 size, float minHeight, float maxHeight, float[] heightMap, 
                                                                 float scaleFactor, float collisionMargin);
 
 // =====================================================================================
@@ -629,7 +629,7 @@ public abstract BulletConstraint GetConstraintRef(BulletBody obj, int index);
 
 public abstract int GetNumConstraintRefs(BulletBody obj);
 
-public abstract bool SetCollisionGroupMask(BulletBody body, uint filter, uint mask);
+public abstract bool SetCollisionGroupMask(BulletBody body, UInt32 filter, UInt32 mask);
 
 // =====================================================================================
 // btCollisionShape entries
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 7603254..3884a5d 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -56,7 +56,6 @@ public sealed class BSCharacter : BSPhysObject
     private int _physicsActorType;
     private bool _isPhysical;
     private bool _flying;
-    private bool _wasWalking;   // 'true' if the avatar was walking/moving last frame
     private bool _setAlwaysRun;
     private bool _throttleUpdates;
     private bool _floatOnWater;
@@ -84,7 +83,6 @@ public sealed class BSCharacter : BSPhysObject
         _position = pos;
 
         _flying = isFlying;
-        _wasWalking = true;     // causes first step to initialize standing
         _orientation = OMV.Quaternion.Identity;
         _velocity = OMV.Vector3.Zero;
         _buoyancy = ComputeBuoyancyFromFlying(isFlying);
@@ -220,7 +218,13 @@ public sealed class BSCharacter : BSPhysObject
             {
                 // The avatar shouldn't be moving
                 _velocityMotor.Zero();
-                ZeroMotion(true /* inTaintTime */);
+
+                // If we are colliding with a stationary object, presume we're standing and don't move around
+                if (!ColliderIsMoving)
+                {
+                    DetailLog("{0},BSCharacter.MoveMotor,collidingWithStationary,zeroingMotion", LocalID);
+                    ZeroMotion(true /* inTaintTime */);
+                }
 
                 // Standing has more friction on the ground
                 if (_currentFriction != BSParam.AvatarStandingFriction)
@@ -229,8 +233,6 @@ public sealed class BSCharacter : BSPhysObject
                     PhysicsScene.PE.SetFriction(PhysBody, _currentFriction);
                 }
                 DetailLog("{0},BSCharacter.MoveMotor,taint,stopping,target={1}", LocalID, _velocityMotor.TargetValue);
-
-                _wasWalking = false;
             }
             else
             {
@@ -260,7 +262,6 @@ public sealed class BSCharacter : BSPhysObject
 
                 DetailLog("{0},BSCharacter.MoveMotor,move,stepVel={1},vel={2},mass={3},moveForce={4}", LocalID, stepVelocity, _velocity, Mass, moveForce);
                 PhysicsScene.PE.ApplyCentralImpulse(PhysBody, moveForce);
-                _wasWalking = true;
             }
         });
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 7ad7c89..05ab180 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -125,9 +125,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         static readonly float PIOverTwo = ((float)Math.PI) / 2f;
 
         // For debugging, flags to turn on and off individual corrections.
-        private bool enableAngularVerticalAttraction;
-        private bool enableAngularDeflection;
-        private bool enableAngularBanking;
+        public bool enableAngularVerticalAttraction;
+        public bool enableAngularDeflection;
+        public bool enableAngularBanking;
 
         public BSDynamics(BSScene myScene, BSPrim myPrim)
         {
@@ -146,7 +146,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             enableAngularBanking = false;
             if (BSParam.VehicleDebuggingEnabled != ConfigurationParameters.numericFalse)
             {
-                enableAngularVerticalAttraction = false;
+                enableAngularVerticalAttraction = true;
                 enableAngularDeflection = false;
                 enableAngularBanking = false;
             }
@@ -165,7 +165,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         }
 
         #region Vehicle parameter setting
-        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
+        public void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
         {
             VDetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue);
             switch (pParam)
@@ -591,14 +591,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 m_vehicleMass = Prim.Linkset.LinksetMass;
 
                 // Friction affects are handled by this vehicle code
-                float friction = 0f;
-                PhysicsScene.PE.SetFriction(Prim.PhysBody, friction);
+                PhysicsScene.PE.SetFriction(Prim.PhysBody, BSParam.VehicleFriction);
+                PhysicsScene.PE.SetRestitution(Prim.PhysBody, BSParam.VehicleRestitution);
 
                 // Moderate angular movement introduced by Bullet.
                 // TODO: possibly set AngularFactor and LinearFactor for the type of vehicle.
                 //     Maybe compute linear and angular factor and damping from params.
-                float angularDamping = BSParam.VehicleAngularDamping;
-                PhysicsScene.PE.SetAngularDamping(Prim.PhysBody, angularDamping);
+                PhysicsScene.PE.SetAngularDamping(Prim.PhysBody, BSParam.VehicleAngularDamping);
+                PhysicsScene.PE.SetLinearFactor(Prim.PhysBody, BSParam.VehicleLinearFactorV);
+                PhysicsScene.PE.SetAngularFactorV(Prim.PhysBody, BSParam.VehicleAngularFactorV);
 
                 // Vehicles report collision events so we know when it's on the ground
                 PhysicsScene.PE.AddToCollisionFlags(Prim.PhysBody, CollisionFlags.BS_VEHICLE_COLLISIONS);
@@ -613,8 +614,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // The actual vehicle gravity is set to zero in Bullet so we can do all the application of same.
                 PhysicsScene.PE.SetGravity(Prim.PhysBody, Vector3.Zero);
 
-                VDetailLog("{0},BSDynamics.Refresh,mass={1},frict={2},inert={3},aDamp={4},grav={5}",
-                        Prim.LocalID, m_vehicleMass, friction, Prim.Inertia, angularDamping, m_VehicleGravity);
+                VDetailLog("{0},BSDynamics.Refresh,mass={1},inert={2},grav={3},aDamp={4},frict={5},rest={6},lFact={7},aFact={8}",
+                        Prim.LocalID, m_vehicleMass, Prim.Inertia, m_VehicleGravity,
+                        BSParam.VehicleAngularDamping, BSParam.VehicleFriction, BSParam.VehicleRestitution,
+                        BSParam.VehicleLinearFactor, BSParam.VehicleAngularFactor
+                        );
             }
             else
             {
@@ -673,13 +677,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private const int m_knownChangedWaterLevel         = 1 << 9;
         private const int m_knownChangedForwardVelocity    = 1 <<10;
 
-        private void ForgetKnownVehicleProperties()
+        public void ForgetKnownVehicleProperties()
         {
             m_knownHas = 0;
             m_knownChanged = 0;
         }
         // Push all the changed values back into the physics engine
-        private void PushKnownChanged()
+        public void PushKnownChanged()
         {
             if (m_knownChanged != 0)
             {
@@ -799,7 +803,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_knownVelocity = Prim.ForceVelocity;
                     m_knownHas |= m_knownChangedVelocity;
                 }
-                return (Vector3)m_knownVelocity;
+                return m_knownVelocity;
             }
             set
             {
@@ -898,9 +902,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             if (!IsActive) return;
 
-            if (PhysicsScene.VehiclePhysicalLoggingEnabled)
-                PhysicsScene.PE.DumpRigidBody(PhysicsScene.World, Prim.PhysBody);
-
             ForgetKnownVehicleProperties();
 
             MoveLinear(pTimestep);
@@ -922,6 +923,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     Prim.LocalID, VehiclePosition, m_knownForce, VehicleVelocity, VehicleRotationalVelocity);
         }
 
+        // Called after the simulation step
+        internal void PostStep(float pTimestep)
+        {
+            if (!IsActive) return;
+
+            if (PhysicsScene.VehiclePhysicalLoggingEnabled)
+                PhysicsScene.PE.DumpRigidBody(PhysicsScene.World, Prim.PhysBody);
+        }
+
         // Apply the effect of the linear motor and other linear motions (like hover and float).
         private void MoveLinear(float pTimestep)
         {
@@ -953,10 +963,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // ==================================================================
             // Clamp high or low velocities
             float newVelocityLengthSq = VehicleVelocity.LengthSquared();
-            if (newVelocityLengthSq > BSParam.VehicleMaxLinearVelocity)
+            if (newVelocityLengthSq > BSParam.VehicleMaxLinearVelocitySq)
             {
+                Vector3 origVelW = VehicleVelocity;         // DEBUG DEBUG
                 VehicleVelocity /= VehicleVelocity.Length();
                 VehicleVelocity *= BSParam.VehicleMaxLinearVelocity;
+                VDetailLog("{0},  MoveLinear,clampMax,origVelW={1},lenSq={2},maxVelSq={3},,newVelW={4}", 
+                            Prim.LocalID, origVelW, newVelocityLengthSq, BSParam.VehicleMaxLinearVelocitySq, VehicleVelocity);
             }
             else if (newVelocityLengthSq < 0.001f)
                 VehicleVelocity = Vector3.Zero;
@@ -968,8 +981,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         public void ComputeLinearVelocity(float pTimestep)
         {
             // Step the motor from the current value. Get the correction needed this step.
-            Vector3 currentVel = VehicleVelocity * Quaternion.Inverse(VehicleOrientation);
-            Vector3 linearMotorCorrectionV = m_linearMotor.Step(pTimestep, currentVel);
+            Vector3 origVelW = VehicleVelocity;             // DEBUG
+            Vector3 currentVelV = VehicleVelocity * Quaternion.Inverse(VehicleOrientation);
+            Vector3 linearMotorCorrectionV = m_linearMotor.Step(pTimestep, currentVelV);
 
             // Motor is vehicle coordinates. Rotate it to world coordinates
             Vector3 linearMotorVelocityW = linearMotorCorrectionV * VehicleOrientation;
@@ -984,8 +998,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // Add this correction to the velocity to make it faster/slower.
             VehicleVelocity += linearMotorVelocityW;
 
-            VDetailLog("{0},  MoveLinear,velocity,vehVel={1},correction={2},force={3}",
-                        Prim.LocalID, VehicleVelocity, linearMotorCorrectionV, linearMotorVelocityW);
+            VDetailLog("{0},  MoveLinear,velocity,origVelW={1},velV={2},correctV={3},correctW={4},newVelW={5}",
+                        Prim.LocalID, origVelW, currentVelV, linearMotorCorrectionV, linearMotorVelocityW, VehicleVelocity);
         }
 
         public void ComputeLinearTerrainHeightCorrection(float pTimestep)
@@ -1185,12 +1199,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
             // Hack to reduce downward force if the vehicle is probably sitting on the ground
             if (Prim.IsColliding && IsGroundVehicle)
-                appliedGravity *= 0.2f;
+                appliedGravity *= BSParam.VehicleGroundGravityFudge;
 
             VehicleAddForce(appliedGravity);
 
-            VDetailLog("{0},  MoveLinear,applyGravity,vehGrav={1},appliedForce-{2}", 
-                            Prim.LocalID, m_VehicleGravity, appliedGravity);
+            VDetailLog("{0},  MoveLinear,applyGravity,vehGrav={1},collid={2},appliedForce={3}", 
+                            Prim.LocalID, m_VehicleGravity, Prim.IsColliding, appliedGravity);
         }
 
         // =======================================================================
@@ -1292,6 +1306,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             if (enableAngularVerticalAttraction && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
             {
                 Vector3 vertContributionV = Vector3.Zero;
+                Vector3 origRotVelW = VehicleRotationalVelocity;        // DEBUG DEBUG
 
                 // Take a vector pointing up and convert it from world to vehicle relative coords.
                 Vector3 verticalError = Vector3.UnitZ * VehicleOrientation;
@@ -1319,13 +1334,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
                 // 'vertContrbution' is now the necessary angular correction to correct tilt in one second.
                 //     Correction happens over a number of seconds.
-                Vector3 unscaledContrib = vertContributionV;     // DEBUG DEBUG
+                Vector3 unscaledContribVerticalErrorV = vertContributionV;     // DEBUG DEBUG
                 vertContributionV /= m_verticalAttractionTimescale;
 
                 VehicleRotationalVelocity += vertContributionV * VehicleOrientation;
 
-                VDetailLog("{0},  MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},eff={3},ts={4},vertAttr={5}",
-                                Prim.LocalID, verticalError, unscaledContrib, m_verticalAttractionEfficiency, m_verticalAttractionTimescale, vertContributionV);
+                VDetailLog("{0},  MoveAngular,verticalAttraction,,origRotVW={1},vertError={2},unscaledV={3},eff={4},ts={5},vertContribV={6}",
+                                Prim.LocalID, origRotVelW, verticalError, unscaledContribVerticalErrorV, 
+                                m_verticalAttractionEfficiency, m_verticalAttractionTimescale, vertContributionV);
             }
         }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 2c8dd23..54dc458 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -232,7 +232,7 @@ public sealed class BSLinksetCompound : BSLinkset
                                                                         newLsi.OffsetFromCenterOfMass,
                                                                         newLsi.OffsetRot,
                                                                         true /* shouldRecalculateLocalAabb */);
-                            DetailLog("{0},BSLinksetCompound.UpdateProperties,changeChildPosRot,whichUpdated={1}newLsi={2}",
+                            DetailLog("{0},BSLinksetCompound.UpdateProperties,changeChildPosRot,whichUpdated={1},newLsi={2}",
                                                                         updated.LocalID, whichUpdated, newLsi);
                             updated.LinksetInfo = newLsi;
                             updatedChild = true;
@@ -377,7 +377,7 @@ public sealed class BSLinksetCompound : BSLinkset
     // Constraint linksets are rebuilt every time.
     // Note that this works for rebuilding just the root after a linkset is taken apart.
     // Called at taint time!!
-    private bool disableCOM = false;     // disable until we get this debugged
+    private bool disableCOM = true;     // disable until we get this debugged
     private void RecomputeLinksetCompound()
     {
         try
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
index 06186b0..8c098b2 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
@@ -82,9 +82,19 @@ public static class BSParam
         public static float AvatarStepApproachFactor { get; private set; }
         public static float AvatarStepForceFactor { get; private set; }
 
+    // Vehicle parameters
     public static float VehicleMaxLinearVelocity { get; private set; }
+    public static float VehicleMaxLinearVelocitySq { get; private set; }
     public static float VehicleMaxAngularVelocity { get; private set; }
+    public static float VehicleMaxAngularVelocitySq { get; private set; }
     public static float VehicleAngularDamping { get; private set; }
+    public static float VehicleFriction { get; private set; }
+    public static float VehicleRestitution { get; private set; }
+    public static float VehicleLinearFactor { get; private set; }
+    public static Vector3 VehicleLinearFactorV { get; private set; }
+    public static float VehicleAngularFactor { get; private set; }
+    public static Vector3 VehicleAngularFactorV { get; private set; }
+    public static float VehicleGroundGravityFudge { get; private set; }
     public static float VehicleDebuggingEnabled { get; private set; }
 
     public static float LinksetImplementation { get; private set; }
@@ -373,7 +383,7 @@ public static class BSParam
             (s) => { return TerrainRestitution; },
             (s,p,l,v) => { TerrainRestitution = v;  /* TODO: set on real terrain */ } ),
         new ParameterDefn("TerrainCollisionMargin", "Margin where collision checking starts" ,
-            0.04f,
+            0.08f,
             (s,cf,p,v) => { TerrainCollisionMargin = cf.GetFloat(p, v); },
             (s) => { return TerrainCollisionMargin; },
             (s,p,l,v) => { TerrainCollisionMargin = v;  /* TODO: set on real terrain */ } ),
@@ -443,17 +453,42 @@ public static class BSParam
             1000.0f,
             (s,cf,p,v) => { VehicleMaxLinearVelocity = cf.GetFloat(p, v); },
             (s) => { return (float)VehicleMaxLinearVelocity; },
-            (s,p,l,v) => { VehicleMaxLinearVelocity = v; } ),
+            (s,p,l,v) => { VehicleMaxLinearVelocity = v; VehicleMaxLinearVelocitySq = v * v; } ),
         new ParameterDefn("VehicleMaxAngularVelocity", "Maximum rotational velocity magnitude that can be assigned to a vehicle",
             12.0f,
             (s,cf,p,v) => { VehicleMaxAngularVelocity = cf.GetFloat(p, v); },
             (s) => { return (float)VehicleMaxAngularVelocity; },
-            (s,p,l,v) => { VehicleMaxAngularVelocity = v; } ),
+            (s,p,l,v) => { VehicleMaxAngularVelocity = v; VehicleMaxAngularVelocitySq = v * v; } ),
         new ParameterDefn("VehicleAngularDamping", "Factor to damp vehicle angular movement per second (0.0 - 1.0)",
             0.0f,
             (s,cf,p,v) => { VehicleAngularDamping = cf.GetFloat(p, v); },
             (s) => { return VehicleAngularDamping; },
             (s,p,l,v) => { VehicleAngularDamping = v; } ),
+        new ParameterDefn("VehicleLinearFactor", "Fraction of physical linear changes applied to vehicle (0.0 - 1.0)",
+            1.0f,
+            (s,cf,p,v) => { VehicleLinearFactor = cf.GetFloat(p, v); },
+            (s) => { return VehicleLinearFactor; },
+            (s,p,l,v) => { VehicleLinearFactor = v; VehicleLinearFactorV = new Vector3(v, v, v); } ),
+        new ParameterDefn("VehicleAngularFactor", "Fraction of physical angular changes applied to vehicle (0.0 - 1.0)",
+            1.0f,
+            (s,cf,p,v) => { VehicleAngularFactor = cf.GetFloat(p, v); },
+            (s) => { return VehicleAngularFactor; },
+            (s,p,l,v) => { VehicleAngularFactor = v; VehicleAngularFactorV = new Vector3(v, v, v); } ),
+        new ParameterDefn("VehicleFriction", "Friction of vehicle on the ground (0.0 - 1.0)",
+            0.0f,
+            (s,cf,p,v) => { VehicleFriction = cf.GetFloat(p, v); },
+            (s) => { return VehicleFriction; },
+            (s,p,l,v) => { VehicleFriction = v; } ),
+        new ParameterDefn("VehicleRestitution", "Bouncyness factor for vehicles (0.0 - 1.0)",
+            0.0f,
+            (s,cf,p,v) => { VehicleRestitution = cf.GetFloat(p, v); },
+            (s) => { return VehicleRestitution; },
+            (s,p,l,v) => { VehicleRestitution = v; } ),
+        new ParameterDefn("VehicleGroundGravityFudge", "Factor to multiple gravity if a ground vehicle is probably on the ground (0.0 - 1.0)",
+            0.2f,
+            (s,cf,p,v) => { VehicleGroundGravityFudge = cf.GetFloat(p, v); },
+            (s) => { return VehicleGroundGravityFudge; },
+            (s,p,l,v) => { VehicleGroundGravityFudge = v; } ),
         new ParameterDefn("VehicleDebuggingEnable", "Turn on/off vehicle debugging",
             ConfigurationParameters.numericFalse,
             (s,cf,p,v) => { VehicleDebuggingEnabled = BSParam.NumericBool(cf.GetBoolean(p, BSParam.BoolNumeric(v))); },
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index 027c786..a113530 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -95,12 +95,16 @@ public abstract class BSPhysObject : PhysicsActor
         SubscribedEventsMs = 0;
         CollidingStep = 0;
         CollidingGroundStep = 0;
+        CollisionAccumulation = 0;
+        ColliderIsMoving = false;
+        CollisionScore = 0;
     }
 
     // Tell the object to clean up.
     public virtual void Destroy()
     {
         UnRegisterAllPreStepActions();
+        UnRegisterAllPostStepActions();
     }
 
     public BSScene PhysicsScene { get; protected set; }
@@ -174,13 +178,14 @@ public abstract class BSPhysObject : PhysicsActor
     public abstract OMV.Vector3 RawPosition { get; set; }
     public abstract OMV.Vector3 ForcePosition { get; set; }
 
-    // Position is what the simulator thinks the positions of the prim is.
+    // 'Position' and 'Orientation' is what the simulator thinks the positions of the prim is.
     // Because Bullet needs the zero coordinate to be the center of mass of the linkset,
     //     sometimes it is necessary to displace the position the physics engine thinks
     //     the position is. PositionDisplacement must be added and removed from the
     //     position as the simulator position is stored and fetched from the physics
-    //     engine. 
+    //     engine. Similar to OrientationDisplacement.
     public virtual OMV.Vector3 PositionDisplacement { get; set; }
+    public virtual OMV.Quaternion OrientationDisplacement { get; set; }
 
     public abstract OMV.Quaternion RawOrientation { get; set; }
     public abstract OMV.Quaternion ForceOrientation { get; set; }
@@ -237,6 +242,12 @@ public abstract class BSPhysObject : PhysicsActor
     protected long CollidingObjectStep { get; set; }
     // The collision flags we think are set in Bullet
     protected CollisionFlags CurrentCollisionFlags { get; set; }
+    // On a collision, check the collider and remember if the last collider was moving
+    //    Used to modify the standing of avatars (avatars on stationary things stand still)
+    protected bool ColliderIsMoving;
+
+    // Count of collisions for this object
+    protected long CollisionAccumulation { get; set; }
 
     public override bool IsColliding {
         get { return (CollidingStep == PhysicsScene.SimulationStep); }
@@ -299,7 +310,12 @@ public abstract class BSPhysObject : PhysicsActor
             return ret;
         }
 
-        // if someone has subscribed for collision events....
+        CollisionAccumulation++;
+
+        // For movement tests, remember if we are colliding with an object that is moving.
+        ColliderIsMoving = collidee != null ? collidee.RawVelocity != OMV.Vector3.Zero : false;
+
+        // If someone has subscribed for collision events log the collision so it will be reported up
         if (SubscribedEvents()) {
             CollisionCollection.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth));
             DetailLog("{0},{1}.Collison.AddCollider,call,with={2},point={3},normal={4},depth={5}",
@@ -385,6 +401,17 @@ public abstract class BSPhysObject : PhysicsActor
     public override bool SubscribedEvents() {
         return (SubscribedEventsMs > 0);
     }
+    // Because 'CollisionScore' is called many times while sorting, it should not be recomputed
+    //    each time called. So this is built to be light weight for each collision and to do
+    //    all the processing when the user asks for the info.
+    public void ComputeCollisionScore()
+    {
+        // Scale the collision count by the time since the last collision.
+        // The "+1" prevents dividing by zero.
+        long timeAgo = PhysicsScene.SimulationStep - CollidingStep + 1;
+        CollisionScore = CollisionAccumulation / timeAgo;
+    }
+    public override float CollisionScore { get; set; }
 
     #endregion // Collisions
 
@@ -393,52 +420,103 @@ public abstract class BSPhysObject : PhysicsActor
     // These actions are optional so, rather than scanning all the physical objects and asking them
     //     if they have anything to do, a physical object registers for an event call before the step is performed.
     // This bookkeeping makes it easy to add, remove and clean up after all these registrations.
-    private Dictionary<string, BSScene.PreStepAction> RegisteredActions = new Dictionary<string, BSScene.PreStepAction>();
+    private Dictionary<string, BSScene.PreStepAction> RegisteredPrestepActions = new Dictionary<string, BSScene.PreStepAction>();
+    private Dictionary<string, BSScene.PostStepAction> RegisteredPoststepActions = new Dictionary<string, BSScene.PostStepAction>();
     protected void RegisterPreStepAction(string op, uint id, BSScene.PreStepAction actn)
     {
         string identifier = op + "-" + id.ToString();
 
-        lock (RegisteredActions)
+        lock (RegisteredPrestepActions)
         {
             // Clean out any existing action
             UnRegisterPreStepAction(op, id);
 
-            RegisteredActions[identifier] = actn;
+            RegisteredPrestepActions[identifier] = actn;
+
+            PhysicsScene.BeforeStep += actn;
         }
-        PhysicsScene.BeforeStep += actn;
         DetailLog("{0},BSPhysObject.RegisterPreStepAction,id={1}", LocalID, identifier);
     }
 
     // Unregister a pre step action. Safe to call if the action has not been registered.
-    protected void UnRegisterPreStepAction(string op, uint id)
+    // Returns 'true' if an action was actually removed
+    protected bool UnRegisterPreStepAction(string op, uint id)
     {
         string identifier = op + "-" + id.ToString();
         bool removed = false;
-        lock (RegisteredActions)
+        lock (RegisteredPrestepActions)
         {
-            if (RegisteredActions.ContainsKey(identifier))
+            if (RegisteredPrestepActions.ContainsKey(identifier))
             {
-                PhysicsScene.BeforeStep -= RegisteredActions[identifier];
-                RegisteredActions.Remove(identifier);
+                PhysicsScene.BeforeStep -= RegisteredPrestepActions[identifier];
+                RegisteredPrestepActions.Remove(identifier);
                 removed = true;
             }
         }
         DetailLog("{0},BSPhysObject.UnRegisterPreStepAction,id={1},removed={2}", LocalID, identifier, removed);
+        return removed;
     }
 
     protected void UnRegisterAllPreStepActions()
     {
-        lock (RegisteredActions)
+        lock (RegisteredPrestepActions)
         {
-            foreach (KeyValuePair<string, BSScene.PreStepAction> kvp in RegisteredActions)
+            foreach (KeyValuePair<string, BSScene.PreStepAction> kvp in RegisteredPrestepActions)
             {
                 PhysicsScene.BeforeStep -= kvp.Value;
             }
-            RegisteredActions.Clear();
+            RegisteredPrestepActions.Clear();
         }
         DetailLog("{0},BSPhysObject.UnRegisterAllPreStepActions,", LocalID);
     }
+    
+    protected void RegisterPostStepAction(string op, uint id, BSScene.PostStepAction actn)
+    {
+        string identifier = op + "-" + id.ToString();
 
+        lock (RegisteredPoststepActions)
+        {
+            // Clean out any existing action
+            UnRegisterPostStepAction(op, id);
+
+            RegisteredPoststepActions[identifier] = actn;
+
+            PhysicsScene.AfterStep += actn;
+        }
+        DetailLog("{0},BSPhysObject.RegisterPostStepAction,id={1}", LocalID, identifier);
+    }
+
+    // Unregister a pre step action. Safe to call if the action has not been registered.
+    // Returns 'true' if an action was actually removed.
+    protected bool UnRegisterPostStepAction(string op, uint id)
+    {
+        string identifier = op + "-" + id.ToString();
+        bool removed = false;
+        lock (RegisteredPoststepActions)
+        {
+            if (RegisteredPoststepActions.ContainsKey(identifier))
+            {
+                PhysicsScene.AfterStep -= RegisteredPoststepActions[identifier];
+                RegisteredPoststepActions.Remove(identifier);
+                removed = true;
+            }
+        }
+        DetailLog("{0},BSPhysObject.UnRegisterPostStepAction,id={1},removed={2}", LocalID, identifier, removed);
+        return removed;
+    }
+
+    protected void UnRegisterAllPostStepActions()
+    {
+        lock (RegisteredPoststepActions)
+        {
+            foreach (KeyValuePair<string, BSScene.PostStepAction> kvp in RegisteredPoststepActions)
+            {
+                PhysicsScene.AfterStep -= kvp.Value;
+            }
+            RegisteredPoststepActions.Clear();
+        }
+        DetailLog("{0},BSPhysObject.UnRegisterAllPostStepActions,", LocalID);
+    }
     
     #endregion // Per Simulation Step actions
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 8b00a33..2b0a539 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -59,7 +59,6 @@ public sealed class BSPrim : BSPhysObject
     private OMV.Vector3 _force;
     private OMV.Vector3 _velocity;
     private OMV.Vector3 _torque;
-    private float _collisionScore;
     private OMV.Vector3 _acceleration;
     private OMV.Quaternion _orientation;
     private int _physicsActorType;
@@ -74,7 +73,7 @@ public sealed class BSPrim : BSPhysObject
     private bool _kinematic;
     private float _buoyancy;
 
-    private BSDynamics _vehicle;
+    public BSDynamics VehicleController { get; private set; }
 
     private BSVMotor _targetMotor;
     private OMV.Vector3 _PIDTarget;
@@ -108,7 +107,7 @@ public sealed class BSPrim : BSPhysObject
         _friction = PhysicsScene.Params.defaultFriction;
         _restitution = PhysicsScene.Params.defaultRestitution;
 
-        _vehicle = new BSDynamics(PhysicsScene, this);            // add vehicleness
+        VehicleController = new BSDynamics(PhysicsScene, this);            // add vehicleness
 
         _mass = CalculateMass();
 
@@ -513,7 +512,7 @@ public sealed class BSPrim : BSPhysObject
 
     public override int VehicleType {
         get {
-            return (int)_vehicle.Type;   // if we are a vehicle, return that type
+            return (int)VehicleController.Type;   // if we are a vehicle, return that type
         }
         set {
             Vehicle type = (Vehicle)value;
@@ -522,14 +521,20 @@ public sealed class BSPrim : BSPhysObject
             {
                 // Done at taint time so we're sure the physics engine is not using the variables
                 // Vehicle code changes the parameters for this vehicle type.
-                _vehicle.ProcessTypeChange(type);
+                VehicleController.ProcessTypeChange(type);
                 ActivateIfPhysical(false);
 
                 // If an active vehicle, register the vehicle code to be called before each step
-                if (_vehicle.Type == Vehicle.TYPE_NONE)
+                if (VehicleController.Type == Vehicle.TYPE_NONE)
+                {
                     UnRegisterPreStepAction("BSPrim.Vehicle", LocalID);
+                    PhysicsScene.AfterStep -= VehicleController.PostStep;
+                }
                 else
-                    RegisterPreStepAction("BSPrim.Vehicle", LocalID, _vehicle.Step);
+                {
+                    RegisterPreStepAction("BSPrim.Vehicle", LocalID, VehicleController.Step);
+                    PhysicsScene.AfterStep += VehicleController.PostStep;
+                }
             });
         }
     }
@@ -537,7 +542,7 @@ public sealed class BSPrim : BSPhysObject
     {
         PhysicsScene.TaintedObject("BSPrim.VehicleFloatParam", delegate()
         {
-            _vehicle.ProcessFloatVehicleParam((Vehicle)param, value);
+            VehicleController.ProcessFloatVehicleParam((Vehicle)param, value);
             ActivateIfPhysical(false);
         });
     }
@@ -545,7 +550,7 @@ public sealed class BSPrim : BSPhysObject
     {
         PhysicsScene.TaintedObject("BSPrim.VehicleVectorParam", delegate()
         {
-            _vehicle.ProcessVectorVehicleParam((Vehicle)param, value);
+            VehicleController.ProcessVectorVehicleParam((Vehicle)param, value);
             ActivateIfPhysical(false);
         });
     }
@@ -553,7 +558,7 @@ public sealed class BSPrim : BSPhysObject
     {
         PhysicsScene.TaintedObject("BSPrim.VehicleRotationParam", delegate()
         {
-            _vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation);
+            VehicleController.ProcessRotationVehicleParam((Vehicle)param, rotation);
             ActivateIfPhysical(false);
         });
     }
@@ -561,7 +566,7 @@ public sealed class BSPrim : BSPhysObject
     {
         PhysicsScene.TaintedObject("BSPrim.VehicleFlags", delegate()
         {
-            _vehicle.ProcessVehicleFlags(param, remove);
+            VehicleController.ProcessVehicleFlags(param, remove);
         });
     }
 
@@ -638,11 +643,6 @@ public sealed class BSPrim : BSPhysObject
             // DetailLog("{0},BSPrim.SetTorque,call,torque={1}", LocalID, _torque);
         }
     }
-    public override float CollisionScore {
-        get { return _collisionScore; }
-        set { _collisionScore = value;
-        }
-    }
     public override OMV.Vector3 Acceleration {
         get { return _acceleration; }
         set { _acceleration = value; }
@@ -747,7 +747,7 @@ public sealed class BSPrim : BSPhysObject
     //     isSolid: other objects bounce off of this object
     //     isVolumeDetect: other objects pass through but can generate collisions
     //     collisionEvents: whether this object returns collision events
-    private void UpdatePhysicalParameters()
+    public void UpdatePhysicalParameters()
     {
         // DetailLog("{0},BSPrim.UpdatePhysicalParameters,entry,body={1},shape={2}", LocalID, BSBody, BSShape);
 
@@ -759,7 +759,7 @@ public sealed class BSPrim : BSPhysObject
         MakeDynamic(IsStatic);
 
         // Update vehicle specific parameters (after MakeDynamic() so can change physical parameters)
-        _vehicle.Refresh();
+        VehicleController.Refresh();
 
         // Arrange for collision events if the simulator wants them
         EnableCollisions(SubscribedEvents());
@@ -1601,7 +1601,7 @@ public sealed class BSPrim : BSPhysObject
             // Remove all the physical dependencies on the old body.
             // (Maybe someday make the changing of BSShape an event to be subscribed to by BSLinkset, ...)
             Linkset.RemoveBodyDependencies(this);
-            _vehicle.RemoveBodyDependencies(this);
+            VehicleController.RemoveBodyDependencies(this);
         });
 
         // Make sure the properties are set on the new object
@@ -1618,9 +1618,9 @@ public sealed class BSPrim : BSPhysObject
         {
             // A temporary kludge to suppress the rotational effects introduced on vehicles by Bullet
             // TODO: handle physics introduced by Bullet with computed vehicle physics.
-            if (_vehicle.IsActive)
+            if (VehicleController.IsActive)
             {
-                // entprop.RotationalVelocity = OMV.Vector3.Zero;
+                entprop.RotationalVelocity = OMV.Vector3.Zero;
             }
 
             // DetailLog("{0},BSPrim.UpdateProperties,entry,entprop={1}", LocalID, entprop);   // DEBUG DEBUG
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index cb304b6..a4690ba 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -26,6 +26,7 @@
  */
 using System;
 using System.Collections.Generic;
+using System.Linq;
 using System.Reflection;
 using System.Runtime.InteropServices;
 using System.Text;
@@ -87,7 +88,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     public delegate void PreStepAction(float timeStep);
     public delegate void PostStepAction(float timeStep);
     public event PreStepAction BeforeStep;
-    public event PreStepAction AfterStep;
+    public event PostStepAction AfterStep;
 
     // A value of the time now so all the collision and update routines do not have to get their own
     // Set to 'now' just before all the prims and actors are called for collisions and updates
@@ -697,7 +698,21 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
     public override Dictionary<uint, float> GetTopColliders()
     {
-        return new Dictionary<uint, float>();
+        Dictionary<uint, float> topColliders;
+
+        lock (PhysObjects)
+        {
+            foreach (KeyValuePair<uint, BSPhysObject> kvp in PhysObjects)
+            {
+                kvp.Value.ComputeCollisionScore();
+            }
+
+            List<BSPhysObject> orderedPrims = new List<BSPhysObject>(PhysObjects.Values);
+            orderedPrims.OrderByDescending(p => p.CollisionScore);
+            topColliders = orderedPrims.Take(25).ToDictionary(p => p.LocalID, p => p.CollisionScore);
+        }
+
+        return topColliders;
     }
 
     public override bool IsThreaded { get { return false;  } }
@@ -748,7 +763,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
     private void TriggerPostStepEvent(float timeStep)
     {
-        PreStepAction actions = AfterStep;
+        PostStepAction actions = AfterStep;
         if (actions != null)
             actions(timeStep);
 
@@ -840,7 +855,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         {
             DetailLog("{0},BSScene.AssertInTaintTime,NOT IN TAINT TIME,Region={1},Where={2}", DetailLogZero, RegionName, whereFrom);
             m_log.ErrorFormat("{0} NOT IN TAINT TIME!! Region={1}, Where={2}", LogHeader, RegionName, whereFrom);
-            Util.PrintCallStack(DetailLog);
+            // Util.PrintCallStack(DetailLog);
         }
         return InTaintTime;
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
index 801f690..a95e169 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
@@ -1,8 +1,11 @@
 CURRENT PRIORITIES
 =================================================
+One sided meshes? Should terrain be built into a closed shape?
+        When meshes get partially wedged into the terrain, they cannot push themselves out.
+        It is possible that Bullet processes collisions whether entering or leaving a mesh.
+        Ref: http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4869
 Deleting a linkset while standing on the root will leave the physical shape of the root behind.
         Not sure if it is because standing on it. Done with large prim linksets.
-Child movement in linkset (don't rebuild linkset)
 Vehicle angular vertical attraction
 vehicle angular banking
 Center-of-gravity
@@ -12,6 +15,7 @@ when should angular and linear motor targets be zeroed? when selected?
         Need a vehicle.clear()? Or an 'else' in prestep if not physical.
 Teravus llMoveToTarget script debug
         Mixing of hover, buoyancy/gravity, moveToTarget, into one force
+        Setting hover height to zero disables hover even if hover flags are on (from SL wiki)
 Nebadon vehicles turning funny in arena
 limitMotorUp calibration (more down?)
 llRotLookAt
@@ -72,7 +76,11 @@ Incorporate inter-relationship of angular corrections. For instance, angularDefl
 
 GENERAL TODO LIST:
 =================================================
+Avatar standing on a moving object should start to move with the object.
 llMoveToTarget objects are not effected by gravity until target is removed.
+Compute CCD parameters based on body size
+Can solver iterations be changed per body/shape? Can be for constraints but what
+        about regular vehicles?
 Implement llSetPhysicalMaterial.
         extend it with Center-of-mass, rolling friction, density
 Implement llSetForceAndTorque.
@@ -321,4 +329,5 @@ Mantis 6040 script http://opensimulator.org/mantis/view.php?id=6040 (DONE)
 Boats float low in the water (DONE)
 Boats floating at proper level (DONE)
 When is force introduced by SetForce removed? The prestep action could go forever. (DONE)
-        (Resolution: setForce registers a prestep action which keeps applying the force)
\ No newline at end of file
+        (Resolution: setForce registers a prestep action which keeps applying the force)
+Child movement in linkset (don't rebuild linkset) (DONE 20130122))
\ No newline at end of file
diff --git a/OpenSim/Region/Physics/BulletSPlugin/Tests/BasicVehicles.cs b/OpenSim/Region/Physics/BulletSPlugin/Tests/BasicVehicles.cs
new file mode 100755
index 0000000..5900103
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/Tests/BasicVehicles.cs
@@ -0,0 +1,127 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+
+using NUnit.Framework;
+using log4net;
+
+using OpenSim.Framework;
+using OpenSim.Region.Physics.BulletSPlugin;
+using OpenSim.Region.Physics.Manager;
+using OpenSim.Tests.Common;
+
+using OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin.Tests
+{
+[TestFixture]
+public class BasicVehicles : OpenSimTestCase
+{
+    // Documentation on attributes: http://www.nunit.org/index.php?p=attributes&r=2.6.1
+    // Documentation on assertions: http://www.nunit.org/index.php?p=assertions&r=2.6.1
+
+    BSScene PhysicsScene { get; set; }
+    BSPrim TestVehicle { get; set; }
+    Vector3 TestVehicleInitPosition { get; set; }
+    float timeStep = 0.089f;
+
+    [TestFixtureSetUp]
+    public void Init()
+    {
+        Dictionary<string, string> engineParams = new Dictionary<string, string>();
+        PhysicsScene = BulletSimTestsUtil.CreateBasicPhysicsEngine(engineParams);
+
+        PrimitiveBaseShape pbs = PrimitiveBaseShape.CreateSphere();
+        Vector3 pos = new Vector3(100.0f, 100.0f, 0f);
+        pos.Z = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos) + 2f;
+        TestVehicleInitPosition = pos;
+        Vector3 size = new Vector3(1f, 1f, 1f);
+        pbs.Scale = size;
+        Quaternion rot = Quaternion.Identity;
+        bool isPhys = false;
+        uint localID = 123;
+
+        PhysicsScene.AddPrimShape("testPrim", pbs, pos, size, rot, isPhys, localID);
+        TestVehicle = (BSPrim)PhysicsScene.PhysObjects[localID];
+        // The actual prim shape creation happens at taint time
+        PhysicsScene.ProcessTaints();
+
+    }
+
+    [TestFixtureTearDown]
+    public void TearDown()
+    {
+        if (PhysicsScene != null)
+        {
+            // The Dispose() will also free any physical objects in the scene
+            PhysicsScene.Dispose();
+            PhysicsScene = null;
+        }
+    }
+
+    [TestCase(25,  0.25f,  0.25f,  0.25f)]
+    [TestCase(25, -0.25f,  0.25f,  0.25f)]
+    [TestCase(25,  0.25f, -0.25f,  0.25f)]
+    [TestCase(25, -0.25f, -0.25f,  0.25f)]
+    public void VerticalAttraction(int simSteps, float initRoll, float initPitch, float initYaw)
+    {
+        Quaternion initOrientation = Quaternion.CreateFromEulers(initRoll, initPitch, initYaw);
+        TestVehicle.Orientation = initOrientation;
+
+        TestVehicle.Position = TestVehicleInitPosition;
+
+        // The vehicle controller is not enabled directly (set a vehicle type).
+        //    Instead the appropriate values are set and calls are made just the parts of the
+        //    controller we want to exercise. Stepping the physics engine then applies
+        //    the actions of that one feature.
+        TestVehicle.VehicleController.ProcessFloatVehicleParam(Vehicle.VERTICAL_ATTRACTION_EFFICIENCY, 0.2f);
+        TestVehicle.VehicleController.ProcessFloatVehicleParam(Vehicle.VERTICAL_ATTRACTION_TIMESCALE, 2f);
+        TestVehicle.VehicleController.enableAngularVerticalAttraction = true;
+
+        TestVehicle.IsPhysical = true;
+        PhysicsScene.ProcessTaints();
+
+        // Step the simulator a bunch of times and and vertical attraction should orient the vehicle up
+        for (int ii = 0; ii < simSteps; ii++)
+        {
+            TestVehicle.VehicleController.ForgetKnownVehicleProperties();
+            TestVehicle.VehicleController.ComputeAngularVerticalAttraction();
+            TestVehicle.VehicleController.PushKnownChanged();
+
+            PhysicsScene.Simulate(timeStep);
+        }
+
+        // After these steps, the vehicle should be upright
+        Vector3 upPointer = Vector3.UnitZ * TestVehicle.Orientation;
+        Assert.That(upPointer.Z, Is.GreaterThan(0.99f));
+    }
+}
+}
\ No newline at end of file
diff --git a/OpenSim/Region/Physics/BulletSPlugin/Tests/BulletSimTests.cs b/OpenSim/Region/Physics/BulletSPlugin/Tests/BulletSimTests.cs
new file mode 100755
index 0000000..35cbc1d
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/Tests/BulletSimTests.cs
@@ -0,0 +1,56 @@
+/*

+ * Copyright (c) Contributors, http://opensimulator.org/

+ * See CONTRIBUTORS.TXT for a full list of copyright holders.

+ *

+ * Redistribution and use in source and binary forms, with or without

+ * modification, are permitted provided that the following conditions are met:

+ *     * Redistributions of source code must retain the above copyright

+ *       notice, this list of conditions and the following disclaimer.

+ *     * Redistributions in binary form must reproduce the above copyright

+ *       notice, this list of conditions and the following disclaimer in the

+ *       documentation and/or other materials provided with the distribution.

+ *     * Neither the name of the OpenSimulator Project nor the

+ *       names of its contributors may be used to endorse or promote products

+ *       derived from this software without specific prior written permission.

+ *

+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY

+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED

+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE

+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY

+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES

+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND

+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT

+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS

+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

+ */

+

+using System;

+using System.Collections.Generic;

+using System.Linq;

+using System.Text;

+

+using NUnit.Framework;

+using log4net;

+

+using OpenSim.Tests.Common;

+

+namespace OpenSim.Region.Physics.BulletSPlugin.Tests

+{

+[TestFixture]

+public class BulletSimTests : OpenSimTestCase

+{

+    // Documentation on attributes: http://www.nunit.org/index.php?p=attributes&r=2.6.1

+    // Documentation on assertions: http://www.nunit.org/index.php?p=assertions&r=2.6.1

+

+    [TestFixtureSetUp]

+    public void Init()

+    {

+    }

+

+    [TestFixtureTearDown]

+    public void TearDown()

+    {

+    }

+}

+}

diff --git a/OpenSim/Region/Physics/BulletSPlugin/Tests/BulletSimTestsUtil.cs b/OpenSim/Region/Physics/BulletSPlugin/Tests/BulletSimTestsUtil.cs
new file mode 100755
index 0000000..215e92f
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/Tests/BulletSimTestsUtil.cs
@@ -0,0 +1,87 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+
+using Nini.Config;
+
+using OpenSim.Framework;
+using OpenSim.Region.Physics.BulletSPlugin;
+using OpenSim.Region.Physics.Meshing;
+
+namespace OpenSim.Region.Physics.BulletSPlugin.Tests
+{
+// Utility functions for building up and tearing down the sample physics environments
+public static class BulletSimTestsUtil
+{
+    // 'engineName' is the Bullet engine to use. Either null (for unmanaged), "BulletUnmanaged" or "BulletXNA"
+    // 'params' is a set of keyValue pairs to set in the engine's configuration file (override defaults)
+    //      May be 'null' if there are no overrides.
+    public static BSScene CreateBasicPhysicsEngine(Dictionary<string,string> paramOverrides)
+    {
+        IConfigSource openSimINI = new IniConfigSource();
+        IConfig startupConfig = openSimINI.AddConfig("Startup");
+        startupConfig.Set("physics", "BulletSim");
+        startupConfig.Set("meshing", "Meshmerizer");
+        startupConfig.Set("cacheSculptMaps", "false");  // meshmerizer shouldn't save maps
+
+        IConfig bulletSimConfig = openSimINI.AddConfig("BulletSim");
+        // If the caller cares, specify the bullet engine otherwise it will default to "BulletUnmanaged".
+        // bulletSimConfig.Set("BulletEngine", "BulletUnmanaged");
+        // bulletSimConfig.Set("BulletEngine", "BulletXNA");
+        bulletSimConfig.Set("MeshSculptedPrim", "false");
+        bulletSimConfig.Set("ForceSimplePrimMeshing", "true");
+        if (paramOverrides != null)
+        {
+            foreach (KeyValuePair<string, string> kvp in paramOverrides)
+            {
+                bulletSimConfig.Set(kvp.Key, kvp.Value);
+            }
+        }
+        // bulletSimConfig.Set("PhysicsLoggingEnabled","True");
+        // bulletSimConfig.Set("PhysicsLoggingDoFlush","True");
+        // bulletSimConfig.Set("VehicleLoggingEnabled","True");
+
+        BSPlugin bsPlugin = new BSPlugin();
+
+        BSScene bsScene = (BSScene)bsPlugin.GetScene("BSTestRegion");
+
+        // Since the asset requestor is not initialized, any mesh or sculptie will be a cube.
+        // In the future, add a fake asset fetcher to get meshes and sculpts.
+        // bsScene.RequestAssetMethod = ???;
+
+        Meshing.Meshmerizer mesher = new Meshmerizer(openSimINI);
+        bsScene.Initialise(mesher, openSimINI);
+
+        return bsScene;
+    }
+
+}
+}
diff --git a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
index 02a0b15..6d7f079 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
@@ -4096,8 +4096,8 @@ namespace OpenSim.Region.Physics.OdePlugin
             lock (_prims)
             {
                 List<OdePrim> orderedPrims = new List<OdePrim>(_prims);
-                orderedPrims.OrderByDescending(p => p.CollisionScore).Take(25);
-                topColliders = orderedPrims.ToDictionary(p => p.LocalID, p => p.CollisionScore);
+                orderedPrims.OrderByDescending(p => p.CollisionScore);
+                topColliders = orderedPrims.Take(25).ToDictionary(p => p.LocalID, p => p.CollisionScore);
 
                 foreach (OdePrim p in _prims)
                     p.CollisionScore = 0;