1 files changed, 485 insertions, 90 deletions

diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
index 45723e3..6d8e1a8 100644
--- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
+++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
@@ -1,11 +1,10 @@
 #region Copyright
 /*
-* Copyright (c) Contributors, http://opensimulator.org/
+* Copyright (c) Contributors, http://www.openmetaverse.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:
+* 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
@@ -13,23 +12,19 @@ met:
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the OpenSim Project nor the
 *       names of its contributors may be used to endorse or promote products
-*       derived from this software without specific prior written 
-permission.
+*       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
+* (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
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*
+* 
 */
 #endregion
 #region References
@@ -47,9 +42,10 @@ using XnaDevRu.BulletX.Dynamics;
 namespace OpenSim.Region.Physics.BulletXPlugin
 {
     /// <summary>
-    /// This Class converts objects and types for BulletX
+    /// BulletXConversions are called now BulletXMaths
+    /// This Class converts objects and types for BulletX and give some operations
     /// </summary>
-    public class BulletXConversions
+    public class BulletXMaths 
     {
         //Vector3
         public static MonoXnaCompactMaths.Vector3 PhysicsVectorToXnaVector3(PhysicsVector physicsVector)
@@ -69,6 +65,114 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         {
             return new AxiomQuaternion(xnaQuaternion.W, xnaQuaternion.X, xnaQuaternion.Y, xnaQuaternion.Z);
         }
+
+        //Next methods are extracted from XnaDevRu.BulletX(See 3rd party license):
+        //- SetRotation (class MatrixOperations)
+        //- GetRotation (class MatrixOperations)
+        //- GetElement (class MathHelper)
+        //- SetElement (class MathHelper)
+        internal static void SetRotation(ref Matrix m, MonoXnaCompactMaths.Quaternion q)
+        {
+            float d = q.LengthSquared();
+            float s = 2f / d;
+            float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;
+            float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;
+            float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;
+            float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;
+            m = new Matrix(1 - (yy + zz), xy - wz, xz + wy, 0,
+                            xy + wz, 1 - (xx + zz), yz - wx, 0,
+                            xz - wy, yz + wx, 1 - (xx + yy), 0,
+                            m.M41, m.M42, m.M43, 1);
+        }
+        internal static MonoXnaCompactMaths.Quaternion GetRotation(Matrix m)
+        {
+            MonoXnaCompactMaths.Quaternion q = new MonoXnaCompactMaths.Quaternion();
+
+            float trace = m.M11 + m.M22 + m.M33;
+
+            if (trace > 0)
+            {
+                float s = (float)Math.Sqrt(trace + 1);
+                q.W = s * 0.5f;
+                s = 0.5f / s;
+
+                q.X = (m.M32 - m.M23) * s;
+                q.Y = (m.M13 - m.M31) * s;
+                q.Z = (m.M21 - m.M12) * s;
+            }
+            else
+            {
+                int i = m.M11 < m.M22 ?
+                    (m.M22 < m.M33 ? 2 : 1) :
+                    (m.M11 < m.M33 ? 2 : 0);
+                int j = (i + 1) % 3;
+                int k = (i + 2) % 3;
+
+                float s = (float)Math.Sqrt(GetElement(m, i, i) - GetElement(m, j, j) - GetElement(m, k, k) + 1);
+                SetElement(ref q, i, s * 0.5f);
+                s = 0.5f / s;
+
+                q.W = (GetElement(m, k, j) - GetElement(m, j, k)) * s;
+                SetElement(ref q, j, (GetElement(m, j, i) + GetElement(m, i, j)) * s);
+                SetElement(ref q, k, (GetElement(m, k, i) + GetElement(m, i, k)) * s);
+            }
+
+            return q;
+        }
+        internal static float SetElement(ref MonoXnaCompactMaths.Quaternion q, int index, float value)
+        {
+            switch (index)
+            {
+                case 0:
+                    q.X = value; break;
+                case 1:
+                    q.Y = value; break;
+                case 2:
+                    q.Z = value; break;
+                case 3:
+                    q.W = value; break;
+            }
+
+            return 0;
+        }
+        internal static float GetElement(Matrix mat, int row, int col)
+        {
+            switch (row)
+            {
+                case 0:
+                    switch (col)
+                    {
+                        case 0:
+                            return mat.M11;
+                        case 1:
+                            return mat.M12;
+                        case 2:
+                            return mat.M13;
+                    } break;
+                case 1:
+                    switch (col)
+                    {
+                        case 0:
+                            return mat.M21;
+                        case 1:
+                            return mat.M22;
+                        case 2:
+                            return mat.M23;
+                    } break;
+                case 2:
+                    switch (col)
+                    {
+                        case 0:
+                            return mat.M31;
+                        case 1:
+                            return mat.M32;
+                        case 2:
+                            return mat.M33;
+                    } break;
+            }
+
+            return 0;
+        }
     }
     /// <summary>
     /// PhysicsPlugin Class for BulletX
@@ -94,7 +198,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         }
         public string GetName()
         {
-            return ("BulletXEngine");
+            return ("modified_BulletX");//Changed!! "BulletXEngine" To "modified_BulletX"
         }
         public void Dispose()
         {
@@ -105,10 +209,12 @@ namespace OpenSim.Region.Physics.BulletXPlugin
     /// </summary>
     public class BulletXScene : PhysicsScene
     {
+        #region BulletXScene Fields
         public DiscreteDynamicsWorld ddWorld;
-                private CollisionDispatcher cDispatcher;
-                private OverlappingPairCache opCache;
-                private SequentialImpulseConstraintSolver sicSolver;
+        private CollisionDispatcher cDispatcher;
+        private OverlappingPairCache opCache;
+        private SequentialImpulseConstraintSolver sicSolver;
+        public static Object BulletXLock = new Object();
 
         private const int minXY = 0;
         private const int minZ = 0;
@@ -128,6 +234,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         public static float HeightLevel0 { get { return heightLevel0; } }
         public static float HeightLevel1 { get { return heightLevel1; } }
         public static float LowGravityFactor { get { return lowGravityFactor; } }
+        #endregion
 
         public BulletXScene()
         {
@@ -137,8 +244,11 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             opCache = new AxisSweep3(worldMinDim, worldMaxDim, maxHandles);
             sicSolver = new SequentialImpulseConstraintSolver();
 
-            ddWorld = new DiscreteDynamicsWorld(cDispatcher, opCache, sicSolver);
-            ddWorld.Gravity = new MonoXnaCompactMaths.Vector3(0, 0, -gravity);
+            lock (BulletXLock)
+            {
+                ddWorld = new DiscreteDynamicsWorld(cDispatcher, opCache, sicSolver);
+                ddWorld.Gravity = new MonoXnaCompactMaths.Vector3(0, 0, -gravity);
+            }
 
             this._heightmap = new float[65536];
         }
@@ -148,47 +258,110 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             pos.X = position.X;
             pos.Y = position.Y;
             pos.Z = position.Z + 20;
-            BulletXCharacter newAv = new BulletXCharacter(this, pos);
-            _characters.Add(newAv);
+            BulletXCharacter newAv = null;
+            lock (BulletXLock)
+            {
+                newAv = new BulletXCharacter(this, pos);
+                _characters.Add(newAv);
+            }
             return newAv;
         }
         public override void RemoveAvatar(PhysicsActor actor)
         {
             if (actor is BulletXCharacter)
             {
-                _characters.Remove((BulletXCharacter)actor);
+                lock (BulletXLock)
+                {
+                    _characters.Remove((BulletXCharacter)actor);
+                }
             }
         }
         public override PhysicsActor AddPrim(PhysicsVector position, PhysicsVector size, AxiomQuaternion rotation)
         {
-            return new BulletXPrim(this, position, size, rotation);
+            BulletXPrim newPrim = null;
+            lock (BulletXLock)
+            {
+                newPrim = new BulletXPrim(this, position, size, rotation);
+                _prims.Add(newPrim);
+            }
+            return newPrim;
         }
         public override void RemovePrim(PhysicsActor prim)
         {
             if (prim is BulletXPrim)
             {
-                _prims.Remove((BulletXPrim)prim);
+                lock (BulletXLock)
+                {
+                    _prims.Remove((BulletXPrim)prim);
+                }
             }
         }
         public override void Simulate(float timeStep)
         {
+            lock (BulletXLock)
+            {
+                BXSMove(timeStep);
+                ddWorld.StepSimulation(timeStep, 0, timeStep);
+                //Heightmap Validation:
+                BXSValidateHeight();
+                //End heightmap validation.
+                BXSUpdateKinetics();
+            }
+        }
+        private void BXSMove(float timeStep)
+        {
             foreach (BulletXCharacter actor in _characters)
             {
                 actor.Move(timeStep);
-
             }
-            ddWorld.StepSimulation(timeStep, 0, timeStep);
+            foreach (BulletXPrim prim in _prims)
+            {
+            }
+        }
+        private void BXSValidateHeight()
+        {
+            float _height;
             foreach (BulletXCharacter actor in _characters)
             {
-                actor.ValidateHeight(this._heightmap[
+                if ((actor.RigidBodyHorizontalPosition.x < 0) || (actor.RigidBodyHorizontalPosition.y < 0))
+                {
+                    _height = 0;
+                }
+                else
+                {
+                    _height = this._heightmap[
                     (int)Math.Round(actor.RigidBodyHorizontalPosition.x) * 256
-                    + (int)Math.Round(actor.RigidBodyHorizontalPosition.y)]);
+                    + (int)Math.Round(actor.RigidBodyHorizontalPosition.y)];
+                }
+                actor.ValidateHeight(_height);
             }
+            foreach (BulletXPrim prim in _prims)
+            {
+                if ((prim.RigidBodyHorizontalPosition.x < 0) || (prim.RigidBodyHorizontalPosition.y < 0))
+                {
+                    _height = 0;
+                }
+                else
+                {
+                    _height = this._heightmap[
+                    (int)Math.Round(prim.RigidBodyHorizontalPosition.x) * 256
+                    + (int)Math.Round(prim.RigidBodyHorizontalPosition.y)];
+                }
+                prim.ValidateHeight(_height);
+            }
+        }
+        private void BXSUpdateKinetics()
+        {
+            //UpdatePosition > UpdateKinetics.
+            //Not only position will be updated, also velocity cause acceleration.
             foreach (BulletXCharacter actor in _characters)
             {
-                actor.UpdatePosition();
+                actor.UpdateKinetics();
+            }
+            foreach (BulletXPrim prim in _prims)
+            {
+                prim.UpdateKinetics();
             }
-
         }
         public override void GetResults()
         {
@@ -212,6 +385,10 @@ namespace OpenSim.Region.Physics.BulletXPlugin
                 int y = i >> 8;
                 this._heightmap[i] = heightMap[x * 256 + y];
             }
+            lock (BulletXLock)
+            {
+                //Updating BulletX HeightMap???
+            }
         }
         public override void DeleteTerrain()
         {
@@ -230,6 +407,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         private AxiomQuaternion _orientation;
         private bool flying;
         private RigidBody rigidBody;
+
         public Axiom.Math.Vector2 RigidBodyHorizontalPosition
         {
             get
@@ -245,31 +423,46 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         public BulletXCharacter(BulletXScene parent_scene, PhysicsVector pos, PhysicsVector velocity,
             PhysicsVector size, PhysicsVector acceleration, AxiomQuaternion orientation)
         {
+            //This fields will be removed. They're temporal
+            float _sizeX = 0.5f;
+            float _sizeY = 0.5f;
+            float _sizeZ = 1.6f;
+            //.
             _position = pos;
             _velocity = velocity;
             _size = size;
+            //---
+            _size.X = _sizeX;
+            _size.Y = _sizeY;
+            _size.Z = _sizeZ;
+            //.
             _acceleration = acceleration;
             _orientation = orientation;
             float _mass = 50.0f; //This depends of avatar's dimensions
-            Matrix _startTransform = Matrix.Identity;
-            _startTransform.Translation = BulletXConversions.PhysicsVectorToXnaVector3(pos);
-            Matrix _centerOfMassOffset = Matrix.Identity;
-            CollisionShape _collisionShape = new BoxShape(new MonoXnaCompactMaths.Vector3(0.5f, 0.5f, 1.60f));
-            DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
-            MonoXnaCompactMaths.Vector3 _localInertia = new MonoXnaCompactMaths.Vector3();
-            _collisionShape.CalculateLocalInertia(_mass, out _localInertia); 
-//Always when mass > 0
-
-            //The next values might change
+            //For RigidBody Constructor. The next values might change
             float _linearDamping = 0.0f;
             float _angularDamping = 0.0f;
             float _friction = 0.5f;
             float _restitution = 0.0f;
-
-            rigidBody = new RigidBody(_mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping, _friction, _restitution);
-            rigidBody.ActivationState = ActivationState.DisableDeactivation;
-
-            parent_scene.ddWorld.AddRigidBody(rigidBody);
+            Matrix _startTransform = Matrix.Identity;
+            Matrix _centerOfMassOffset = Matrix.Identity;
+            lock (BulletXScene.BulletXLock)
+            {
+                _startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(pos);
+                //CollisionShape _collisionShape = new BoxShape(new MonoXnaCompactMaths.Vector3(1.0f, 1.0f, 1.60f));
+                //For now, like ODE, collisionShape = sphere of radious = 1.0
+                CollisionShape _collisionShape = new SphereShape(1.0f);
+                DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
+                MonoXnaCompactMaths.Vector3 _localInertia = new MonoXnaCompactMaths.Vector3();
+                _collisionShape.CalculateLocalInertia(_mass, out _localInertia); //Always when mass > 0
+                rigidBody = new RigidBody(_mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping, _friction, _restitution);
+                rigidBody.ActivationState = ActivationState.DisableDeactivation;
+                //It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
+                MonoXnaCompactMaths.Vector3 _vDebugTranslation;
+                _vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition;
+                rigidBody.Translate(_vDebugTranslation);
+                parent_scene.ddWorld.AddRigidBody(rigidBody);
+            }
         }
         public override PhysicsVector Position
         {
@@ -279,7 +472,11 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             }
             set
             {
-                _position = value;
+                lock (BulletXScene.BulletXLock)
+                {
+                    _position = value;
+                    Translate();
+                }
             }
         }
         public override PhysicsVector Velocity
@@ -290,7 +487,11 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             }
             set
             {
-                _velocity = value;
+                lock (BulletXScene.BulletXLock)
+                {
+                    _velocity = value;
+                    Speed();
+                }
             }
         }
         public override PhysicsVector Size
@@ -301,7 +502,10 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             }
             set
             {
-                _size = value;
+                lock (BulletXScene.BulletXLock)
+                {
+                    _size = value;
+                }
             }
         }
         public override PhysicsVector Acceleration
@@ -319,7 +523,10 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             }
             set
             {
-                _orientation = value;
+                lock (BulletXScene.BulletXLock)
+                {
+                    _orientation = value;
+                }
             }
         }
         public override bool Flying
@@ -335,7 +542,10 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         }
         public void SetAcceleration(PhysicsVector accel)
         {
-            _acceleration = accel;
+            lock (BulletXScene.BulletXLock)
+            {
+                _acceleration = accel;
+            }
         }
         public override bool Kinematic
         {
@@ -356,70 +566,96 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         {
 
         }
-        public void Move(float timeStep)
+        internal void Move(float timeStep)
         {
             MonoXnaCompactMaths.Vector3 vec = new MonoXnaCompactMaths.Vector3();
-            //if (this._velocity.X == 0.0f)
-            //    vec.X = this.rigidBody.LinearVelocity.X; //current velocity
-            //else
-                vec.X = this._velocity.X; //overrides current velocity
-
-            //if (this._velocity.Y == 0.0f)
-            //    vec.Y = this.rigidBody.LinearVelocity.Y; //current velocity
-            //else
-                vec.Y = this._velocity.Y; //overrides current velocity
-
-            float nextZVelocity;
-            //if (this._velocity.Z == 0.0f)
-            //    nextZVelocity = this.rigidBody.LinearVelocity.Z; //current velocity
-            //else
-                nextZVelocity = this._velocity.Z; //overrides current velocity
+            //At this point it's supossed that:
+            //_velocity == rigidBody.LinearVelocity
+            vec.X = this._velocity.X; 
+            vec.Y = this._velocity.Y; 
+            vec.Z = this._velocity.Z; 
 
             if (flying)
             {
                 //Antigravity with movement
                 if (this._position.Z <= BulletXScene.HeightLevel0)
                 {
-                    vec.Z = nextZVelocity + BulletXScene.Gravity * timeStep;
+                    vec.Z += BulletXScene.Gravity * timeStep;
                 }
                 //Lowgravity with movement
-                else if((this._position.Z > BulletXScene.HeightLevel0)
+                else if ((this._position.Z > BulletXScene.HeightLevel0)
                     && (this._position.Z <= BulletXScene.HeightLevel1))
                 {
-                    vec.Z = nextZVelocity + BulletXScene.Gravity * timeStep * (1.0f - BulletXScene.LowGravityFactor);
+                    vec.Z += BulletXScene.Gravity * timeStep * (1.0f - BulletXScene.LowGravityFactor);
                 }
                 //Lowgravity with...
                 else if (this._position.Z > BulletXScene.HeightLevel1)
                 {
-                    if(nextZVelocity > 0) //no movement
+                    if (vec.Z > 0) //no movement
                         vec.Z = BulletXScene.Gravity * timeStep * (1.0f - BulletXScene.LowGravityFactor);
                     else
-                        vec.Z = nextZVelocity + BulletXScene.Gravity * timeStep * (1.0f - BulletXScene.LowGravityFactor);
+                        vec.Z += BulletXScene.Gravity * timeStep * (1.0f - BulletXScene.LowGravityFactor);
 
                 }
             }
-            else
-            {
-                vec.Z = nextZVelocity;
-            }
             rigidBody.LinearVelocity = vec;
         }
-        public void UpdatePosition()
-        {
-            this._position = BulletXConversions.XnaVector3ToPhysicsVector(rigidBody.CenterOfMassPosition);
-        }
         //This validation is very basic
         internal void ValidateHeight(float heighmapPositionValue)
         {
-            if (rigidBody.CenterOfMassPosition.Z < heighmapPositionValue)
+            if (rigidBody.CenterOfMassPosition.Z < heighmapPositionValue + _size.Z / 2.0f)
             {
                 Matrix m = rigidBody.WorldTransform;
                 MonoXnaCompactMaths.Vector3 v3 = m.Translation;
-                v3.Z = heighmapPositionValue;
+                v3.Z = heighmapPositionValue + _size.Z / 2.0f;
                 m.Translation = v3;
                 rigidBody.WorldTransform = m;
+                //When an Avie touch the ground it's vertical velocity it's reduced to ZERO
+                Speed(new PhysicsVector(this.rigidBody.LinearVelocity.X, this.rigidBody.LinearVelocity.Y, 0.0f));
             }
         }
+        internal void UpdateKinetics()
+        {
+            this._position = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.CenterOfMassPosition);
+            this._velocity = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.LinearVelocity);
+            //Orientation it seems that it will be the default.
+            ReOrient();
+        }
+
+        #region Methods for updating values of RigidBody
+        private void Translate()
+        {
+            Translate(this._position);
+        }
+        private void Translate(PhysicsVector _newPos)
+        {
+            MonoXnaCompactMaths.Vector3 _translation;
+            _translation = BulletXMaths.PhysicsVectorToXnaVector3(_newPos) - rigidBody.CenterOfMassPosition;
+            rigidBody.Translate(_translation);
+        }
+        private void Speed()
+        {
+            Speed(this._velocity);
+        }
+        private void Speed(PhysicsVector _newSpeed)
+        {
+            MonoXnaCompactMaths.Vector3 _speed;
+            _speed = BulletXMaths.PhysicsVectorToXnaVector3(_newSpeed);
+            rigidBody.LinearVelocity = _speed;
+        }
+        private void ReOrient()
+        {
+            ReOrient(this._orientation);
+        }
+        private void ReOrient(AxiomQuaternion _newOrient)
+        {
+            MonoXnaCompactMaths.Quaternion _newOrientation;
+            _newOrientation = BulletXMaths.AxiomQuaternionToXnaQuaternion(_newOrient);
+            Matrix _comTransform = rigidBody.CenterOfMassTransform;
+            BulletXMaths.SetRotation(ref _comTransform, _newOrientation);
+            rigidBody.CenterOfMassTransform = _comTransform;
+        }
+        #endregion
     }
     /// <summary>
     /// PhysicsActor Prim Class for BulletX
@@ -431,7 +667,20 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         private PhysicsVector _size;
         private PhysicsVector _acceleration;
         private AxiomQuaternion _orientation;
+        //Density it will depends of material. 
+        //For now all prims have the same density, all prims are made of water. Be water my friend! :D
+        private const float _density = 1000.0f;
+        private RigidBody rigidBody;
+        //_physical value will be linked with the prim object value
+        private Boolean _physical = false;
 
+        public Axiom.Math.Vector2 RigidBodyHorizontalPosition
+        {
+            get
+            {
+                return new Axiom.Math.Vector2(this.rigidBody.CenterOfMassPosition.X, this.rigidBody.CenterOfMassPosition.Y);
+            }
+        }
         public BulletXPrim(BulletXScene parent_scene, PhysicsVector pos, PhysicsVector size, AxiomQuaternion rotation)
             : this(parent_scene, pos, new PhysicsVector(), size, new PhysicsVector(), rotation)
         {
@@ -442,19 +691,51 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             _position = pos;
             _velocity = velocity;
             _size = size;
+            if ((size.X == 0) || (size.Y == 0) || (size.Z == 0)) throw new Exception("Size 0");
+
             _acceleration = aceleration;
-            _orientation = rotation;
+            //Because a bug, orientation will be fixed to AxiomQuaternion.Identity
+            _orientation = AxiomQuaternion.Identity;
+            //_orientation = rotation;
+            //---
+            //For RigidBody Constructor. The next values might change
+            float _linearDamping = 0.0f;
+            float _angularDamping = 0.0f;
+            float _friction = 0.5f;
+            float _restitution = 0.0f;
+            Matrix _startTransform = Matrix.Identity;
+            Matrix _centerOfMassOffset = Matrix.Identity;
+            lock (BulletXScene.BulletXLock)
+            {
+                _startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(pos);
+                //For now all prims are boxes
+                CollisionShape _collisionShape = new BoxShape(BulletXMaths.PhysicsVectorToXnaVector3(_size) / 2.0f);
+                DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
+                MonoXnaCompactMaths.Vector3 _localInertia = new MonoXnaCompactMaths.Vector3();
+                _collisionShape.CalculateLocalInertia(Mass, out _localInertia); //Always when mass > 0
+                rigidBody = new RigidBody(Mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping, _friction, _restitution);
+                rigidBody.ActivationState = ActivationState.DisableDeactivation;
+                //It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
+                MonoXnaCompactMaths.Vector3 _vDebugTranslation;
+                _vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition;
+                rigidBody.Translate(_vDebugTranslation);
+                //---
+                parent_scene.ddWorld.AddRigidBody(rigidBody);
+            }
         }
         public override PhysicsVector Position
         {
             get
             {
                 return _position;
-
             }
             set
             {
-                _position = value;
+                lock (BulletXScene.BulletXLock)
+                {
+                    _position = value;
+                    Translate();
+                }
             }
         }
         public override PhysicsVector Velocity
@@ -465,7 +746,11 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             }
             set
             {
-                _velocity = value;
+                lock (BulletXScene.BulletXLock)
+                {
+                    _velocity = value;
+                    Speed();
+                }
             }
         }
         public override PhysicsVector Size
@@ -476,7 +761,11 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             }
             set
             {
-                _size = value;
+                lock (BulletXScene.BulletXLock)
+                {
+                    _size = value;
+                    ReSize();
+                }
             }
         }
         public override PhysicsVector Acceleration
@@ -494,7 +783,19 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             }
             set
             {
-                _orientation = value;
+                lock (BulletXScene.BulletXLock)
+                {
+                    _orientation = value;
+                    ReOrient();
+                }
+            }
+        }
+        public float Mass
+        {
+            get
+            {
+                //For now all prims are boxes
+                return _density * _size.X * _size.Y * _size.Z;
             }
         }
         public override bool Flying
@@ -508,9 +809,23 @@ namespace OpenSim.Region.Physics.BulletXPlugin
 
             }
         }
+        public Boolean Physical
+        {
+            get
+            {
+                return _physical;
+            }
+            set
+            {
+                _physical = value;
+            }
+        }
         public void SetAcceleration(PhysicsVector accel)
         {
-            _acceleration = accel;
+            lock (BulletXScene.BulletXLock)
+            {
+                _acceleration = accel;
+            }
         }
         public override bool Kinematic
         {
@@ -532,6 +847,86 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         {
 
         }
+        internal void ValidateHeight(float heighmapPositionValue)
+        {
+            if (rigidBody.CenterOfMassPosition.Z < heighmapPositionValue + _size.Z / 2.0f)
+            {
+                Matrix m = rigidBody.WorldTransform;
+                MonoXnaCompactMaths.Vector3 v3 = m.Translation;
+                v3.Z = heighmapPositionValue + _size.Z / 2.0f;
+                m.Translation = v3;
+                rigidBody.WorldTransform = m;
+                //When a Prim touch the ground it's vertical velocity it's reduced to ZERO
+                Speed(new PhysicsVector(this.rigidBody.LinearVelocity.X, this.rigidBody.LinearVelocity.Y, 0.0f));
+            }
+        }
+        internal void UpdateKinetics()
+        {
+            if (_physical) //Updates properties. Prim updates its properties physically
+            {
+                this._position = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.CenterOfMassPosition);
+                this._velocity = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.LinearVelocity);
+                //Orientation is not implemented yet in MonoXnaCompactMaths
+                //this._orientation = BulletXMaths.XnaQuaternionToAxiomQuaternion(rigidBody.Orientation); < Good
+                //ReOrient();
+                //---
+                ReOrient();
+            }
+            else //Doesn't updates properties. That's a cancel
+            {
+                Translate();
+                Speed();
+                //Orientation is not implemented yet in MonoXnaCompactMaths
+                //ReOrient();
+                ReOrient();
+            }
+        }
+
+        #region Methods for updating values of RigidBody
+        private void Translate()
+        {
+            Translate(this._position);
+        }
+        private void Translate(PhysicsVector _newPos)
+        {
+            MonoXnaCompactMaths.Vector3 _translation;
+            _translation = BulletXMaths.PhysicsVectorToXnaVector3(_newPos) - rigidBody.CenterOfMassPosition;
+            rigidBody.Translate(_translation);
+        }
+        private void Speed()
+        {
+            Speed(this._velocity);
+        }
+        private void Speed(PhysicsVector _newSpeed)
+        {
+            MonoXnaCompactMaths.Vector3 _speed;
+            _speed = BulletXMaths.PhysicsVectorToXnaVector3(_newSpeed);
+            rigidBody.LinearVelocity = _speed;
+        }
+        private void ReSize()
+        {
+            ReSize(this._size);
+        }
+        private void ReSize(PhysicsVector _newSize)
+        {
+            MonoXnaCompactMaths.Vector3 _newsize;
+            _newsize = BulletXMaths.PhysicsVectorToXnaVector3(_newSize);
+            //For now all prims are Boxes
+            rigidBody.CollisionShape = new BoxShape(BulletXMaths.PhysicsVectorToXnaVector3(_newSize) / 2.0f);
+        }
+        private void ReOrient()
+        {
+            ReOrient(this._orientation);
+        }
+        private void ReOrient(AxiomQuaternion _newOrient)
+        {
+            MonoXnaCompactMaths.Quaternion _newOrientation;
+            _newOrientation = BulletXMaths.AxiomQuaternionToXnaQuaternion(_newOrient);
+            Matrix _comTransform = rigidBody.CenterOfMassTransform;
+            BulletXMaths.SetRotation(ref _comTransform, _newOrientation);
+            rigidBody.CenterOfMassTransform = _comTransform;
+        }
+        #endregion
+
     }
 }
-