applied Darok"s BulletXPlugin changes 003.patch

OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs

@@ -30,18 +30,28 @@
 #region References
 using System;
 using System.Collections.Generic;
-using OpenSim.Framework.Types;
 using OpenSim.Region.Physics.Manager;
+using OpenSim.Framework.Types;
 using Axiom.Math;
 using AxiomQuaternion = Axiom.Math.Quaternion;
 //Specific References for BulletXPlugin
-using MonoXnaCompactMaths; //Called as MXCM
+using MonoXnaCompactMaths;
 using XnaDevRu.BulletX;
 using XnaDevRu.BulletX.Dynamics;
+
 #endregion
 
 namespace OpenSim.Region.Physics.BulletXPlugin
 {
+    /// <summary>
+    /// This class is only here for compilations reasons
+    /// </summary>
+    public class Mesh
+    {
+        public Mesh()
+        {
+        }
+    }
     /// <summary>
     /// BulletXConversions are called now BulletXMaths
     /// This Class converts objects and types for BulletX and give some operations
@@ -222,12 +232,14 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         private const int maxXY = 256;
         private const int maxZ = 4096;
         private const int maxHandles = 32766; //Why? I don't know
-        private static float gravity = 9.8f;
-        private static float heightLevel0 = 77.0f;
-        private static float heightLevel1 = 200.0f;
-        private static float lowGravityFactor = 0.2f;
-
-        private float[] _heightmap;
+        private const float gravity = 9.8f;
+        private const float heightLevel0 = 77.0f;
+        private const float heightLevel1 = 200.0f;
+        private const float lowGravityFactor = 0.2f;
+        //OpenSim calls Simulate 10 times per seconds. So FPS = "Simulate Calls" * simulationSubSteps = 100 FPS
+        private const int simulationSubSteps = 10;
+        //private float[] _heightmap;
+        private BulletXPlanet _simFlatPlanet;
         private List<BulletXCharacter> _characters = new List<BulletXCharacter>();
         private List<BulletXPrim> _prims = new List<BulletXPrim>();
 
@@ -235,6 +247,11 @@ 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; } }
+        public static int MaxXY { get { return maxXY; } }
+        public static int MaxZ { get { return maxZ; } }
+
+        private List<RigidBody> _forgottenRigidBodies = new List<RigidBody>();
+        internal string is_ex_message = "Can't remove rigidBody!: ";
         #endregion
 
         public BulletXScene()
@@ -250,10 +267,9 @@ namespace OpenSim.Region.Physics.BulletXPlugin
                 ddWorld = new DiscreteDynamicsWorld(cDispatcher, opCache, sicSolver);
                 ddWorld.Gravity = new MonoXnaCompactMaths.Vector3(0, 0, -gravity);
             }
-
-            this._heightmap = new float[65536];
+            //this._heightmap = new float[65536];
         }
-        public override PhysicsActor AddAvatar(string avName, PhysicsVector position)
+        public override PhysicsActor  AddAvatar(string avName, PhysicsVector position)
         {
             PhysicsVector pos = new PhysicsVector();
             pos.X = position.X;
@@ -262,7 +278,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             BulletXCharacter newAv = null;
             lock (BulletXLock)
             {
-                newAv = new BulletXCharacter(this, pos);
+                newAv = new BulletXCharacter(avName, this, pos);
                 _characters.Add(newAv);
             }
             return newAv;
@@ -273,48 +289,98 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             {
                 lock (BulletXLock)
                 {
+                    try
+                    {
+                        ddWorld.RemoveRigidBody(((BulletXCharacter)actor).RigidBody);
+                    }
+                    catch (Exception ex)
+                    {
+                        BulletXMessage(is_ex_message + ex.Message, true);
+                        ((BulletXCharacter)actor).RigidBody.ActivationState = ActivationState.DisableSimulation;
+                        AddForgottenRigidBody(((BulletXCharacter)actor).RigidBody);
+                    }
                     _characters.Remove((BulletXCharacter)actor);
                 }
+                GC.Collect();
+            }
+        }
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position, PhysicsVector size, AxiomQuaternion rotation)
+        {
+            PhysicsActor result;
+
+            switch (pbs.ProfileShape)
+            {
+                case ProfileShape.Square:
+                    /// support simple box & hollow box now; later, more shapes
+                    if (pbs.ProfileHollow == 0)
+                    {
+                        result = AddPrim(primName, position, size, rotation, null, null);
+                    }
+                    else
+                    {
+                        Mesh mesh = null;
+                        result = AddPrim(primName, position, size, rotation, mesh, pbs);
+                    }
+                    break;
+
+                default:
+                    result = AddPrim(primName, position, size, rotation, null, null);
+                    break;
             }
+
+            return result;
         }
-        PhysicsActor AddPrim(PhysicsVector position, PhysicsVector size, AxiomQuaternion rotation)
+        public PhysicsActor AddPrim(PhysicsVector position, PhysicsVector size, Axiom.Math.Quaternion rotation)
+        {
+            return AddPrim("", position, size, rotation, null, null);
+        }
+        public PhysicsActor AddPrim(String name, PhysicsVector position, PhysicsVector size, AxiomQuaternion rotation, Mesh mesh, PrimitiveBaseShape pbs)
         {
             BulletXPrim newPrim = null;
             lock (BulletXLock)
             {
-                newPrim = new BulletXPrim(this, position, size, rotation);
+                newPrim = new BulletXPrim(name, this, position, size, rotation, mesh, pbs);
                 _prims.Add(newPrim);
             }
             return newPrim;
         }
-        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position, PhysicsVector size, AxiomQuaternion rotation)
-        {
-            return AddPrim(position, size, rotation);
-        }
-
         public override void RemovePrim(PhysicsActor prim)
         {
             if (prim is BulletXPrim)
             {
                 lock (BulletXLock)
                 {
+                    try
+                    {
+                        ddWorld.RemoveRigidBody(((BulletXPrim)prim).RigidBody);
+                    }
+                    catch (Exception ex)
+                    {
+                        BulletXMessage(is_ex_message + ex.Message, true);
+                        ((BulletXPrim)prim).RigidBody.ActivationState = ActivationState.DisableSimulation;
+                        AddForgottenRigidBody(((BulletXPrim)prim).RigidBody);
+                    }
                     _prims.Remove((BulletXPrim)prim);
                 }
+                GC.Collect();
             }
         }
         public override void Simulate(float timeStep)
         {
             lock (BulletXLock)
             {
-                BXSMove(timeStep);
-                ddWorld.StepSimulation(timeStep, 0, timeStep);
-                //Heightmap Validation:
-                BXSValidateHeight();
+                //Try to remove garbage
+                RemoveForgottenRigidBodies();
+                //End of remove
+                MoveAllObjects(timeStep);
+                ddWorld.StepSimulation(timeStep, simulationSubSteps, timeStep);
+                //Extra Heightmap Validation: BulletX's HeightFieldTerrain somestimes doesn't work so fine.
+                ValidateHeightForAll();
                 //End heightmap validation.
-                BXSUpdateKinetics();
+                UpdateKineticsForAll();
             }
         }
-        private void BXSMove(float timeStep)
+        private void MoveAllObjects(float timeStep)
         {
             foreach (BulletXCharacter actor in _characters)
             {
@@ -324,39 +390,33 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             {
             }
         }
-        private void BXSValidateHeight()
+        private void ValidateHeightForAll()
         {
             float _height;
             foreach (BulletXCharacter actor in _characters)
             {
-                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)];
-                }
+                //_height = HeightValue(actor.RigidBodyPosition);
+                _height = _simFlatPlanet.HeightValue(actor.RigidBodyPosition);
                 actor.ValidateHeight(_height);
+                //if (_simFlatPlanet.heightIsNotValid(actor.RigidBodyPosition, out _height)) 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)];
-                }
+                //_height = HeightValue(prim.RigidBodyPosition); 
+                _height = _simFlatPlanet.HeightValue(prim.RigidBodyPosition); 
                 prim.ValidateHeight(_height);
+                //if (_simFlatPlanet.heightIsNotValid(prim.RigidBodyPosition, out _height)) prim.ValidateHeight(_height);
             }
+            //foreach (BulletXCharacter actor in _characters)
+            //{
+            //    actor.ValidateHeight(0);
+            //}
+            //foreach (BulletXPrim prim in _prims)
+            //{
+            //    prim.ValidateHeight(0);
+            //}
         }
-        private void BXSUpdateKinetics()
+        private void UpdateKineticsForAll()
         {
             //UpdatePosition > UpdateKinetics.
             //Not only position will be updated, also velocity cause acceleration.
@@ -368,6 +428,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             {
                 prim.UpdateKinetics();
             }
+            //if(this._simFlatPlanet!=null) this._simFlatPlanet.Restore();
         }
         public override void GetResults()
         {
@@ -382,24 +443,97 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         }
         public override void SetTerrain(float[] heightMap)
         {
-            //As the same as ODE, heightmap (x,y) must be swapped for BulletX
-            for (int i = 0; i < 65536; i++)
+            ////As the same as ODE, heightmap (x,y) must be swapped for BulletX
+            //for (int i = 0; i < 65536; i++)
+            //{
+            //    // this._heightmap[i] = (double)heightMap[i];
+            //    // dbm (danx0r) -- heightmap x,y must be swapped for Ode (should fix ODE, but for now...)
+            //    int x = i & 0xff;
+            //    int y = i >> 8;
+            //    this._heightmap[i] = heightMap[x * 256 + y];
+            //}
+
+            //float[] swappedHeightMap = new float[65536];
+            ////As the same as ODE, heightmap (x,y) must be swapped for BulletX
+            //for (int i = 0; i < 65536; i++)
+            //{
+            //    // this._heightmap[i] = (double)heightMap[i];
+            //    // dbm (danx0r) -- heightmap x,y must be swapped for Ode (should fix ODE, but for now...)
+            //    int x = i & 0xff;
+            //    int y = i >> 8;
+            //    swappedHeightMap[i] = heightMap[x * 256 + y];
+            //}
+            DeleteTerrain();
+            //There is a BulletXLock inside the constructor of BulletXPlanet
+            //this._simFlatPlanet = new BulletXPlanet(this, swappedHeightMap);
+            this._simFlatPlanet = new BulletXPlanet(this, heightMap);
+            //this._heightmap = heightMap;
+        }
+        public override void DeleteTerrain()
+        {
+            if (this._simFlatPlanet != null)
             {
-                // this._heightmap[i] = (double)heightMap[i];
-                // dbm (danx0r) -- heightmap x,y must be swapped for Ode (should fix ODE, but for now...)
-                int x = i & 0xff;
-                int y = i >> 8;
-                this._heightmap[i] = heightMap[x * 256 + y];
+                lock (BulletXLock)
+                {
+                    try
+                    {
+                        ddWorld.RemoveRigidBody(this._simFlatPlanet.RigidBody);
+                    }
+                    catch (Exception ex)
+                    {
+                        BulletXMessage(is_ex_message + ex.Message, true);
+                        this._simFlatPlanet.RigidBody.ActivationState = ActivationState.DisableSimulation;
+                        AddForgottenRigidBody(this._simFlatPlanet.RigidBody);
+                    }
+                }
+                this._simFlatPlanet = null;
+                GC.Collect();
+                BulletXMessage("Terrain erased!", false);
             }
-            lock (BulletXLock)
+            //this._heightmap = null;
+        }
+        internal void AddForgottenRigidBody(RigidBody forgottenRigidBody)
+        {
+            _forgottenRigidBodies.Add(forgottenRigidBody);
+        }
+        private void RemoveForgottenRigidBodies()
+        {
+            RigidBody forgottenRigidBody;
+            int nRigidBodies = _forgottenRigidBodies.Count;
+            for(int i = nRigidBodies - 1; i >= 0; i--)
             {
-                //Updating BulletX HeightMap???
+                forgottenRigidBody = _forgottenRigidBodies[i];
+                try
+                {
+                    ddWorld.RemoveRigidBody(forgottenRigidBody);
+                    _forgottenRigidBodies.Remove(forgottenRigidBody);
+                    BulletXMessage("Forgotten Rigid Body Removed", false);
+                }
+                catch (Exception ex)
+                {
+                    BulletXMessage("Can't remove forgottenRigidBody!: " + ex.Message, false);
+                }
             }
+            GC.Collect();
         }
-        public override void DeleteTerrain()
+        internal void BulletXMessage(string message, bool isWarning)
         {
-
+            PhysicsPluginManager.PhysicsPluginMessage("[Modified BulletX]:\t" + message, isWarning);
         }
+        //temp
+        //private float HeightValue(MonoXnaCompactMaths.Vector3 position)
+        //{
+        //    int li_x, li_y;
+        //    float height;
+        //    li_x = (int)Math.Round(position.X); if (li_x < 0) li_x = 0;
+        //    li_y = (int)Math.Round(position.Y); if (li_y < 0) li_y = 0;
+
+        //    height = this._heightmap[li_y * 256 + li_x];
+        //    if (height < 0) height = 0;
+        //    else if (height > maxZ) height = maxZ;
+ 
+        //    return height;
+        //}
     }
     /// <summary>
     /// PhysicsActor Character Class for BulletX
@@ -414,19 +548,20 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         private bool flying;
         private RigidBody rigidBody;
 
-        public Axiom.Math.Vector2 RigidBodyHorizontalPosition
+        public MonoXnaCompactMaths.Vector3 RigidBodyPosition
         {
-            get
-            {
-                return new Axiom.Math.Vector2(this.rigidBody.CenterOfMassPosition.X, this.rigidBody.CenterOfMassPosition.Y);
-            }
+            get { return this.rigidBody.CenterOfMassPosition; }
         }
         public BulletXCharacter(BulletXScene parent_scene, PhysicsVector pos)
-            : this(parent_scene, pos, new PhysicsVector(), new PhysicsVector(), new PhysicsVector(),
+            : this("", parent_scene, pos)
+        {
+        }
+        public BulletXCharacter(String avName, BulletXScene parent_scene, PhysicsVector pos)
+            : this(avName, parent_scene, pos, new PhysicsVector(), new PhysicsVector(), new PhysicsVector(),
             AxiomQuaternion.Identity)
         {
         }
-        public BulletXCharacter(BulletXScene parent_scene, PhysicsVector pos, PhysicsVector velocity,
+        public BulletXCharacter(String avName, BulletXScene parent_scene, PhysicsVector pos, PhysicsVector velocity,
             PhysicsVector size, PhysicsVector acceleration, AxiomQuaternion orientation)
         {
             //This fields will be removed. They're temporal
@@ -462,7 +597,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
                 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;
+                //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;
@@ -535,6 +670,13 @@ namespace OpenSim.Region.Physics.BulletXPlugin
                 }
             }
         }
+        public RigidBody RigidBody
+        {
+            get
+            {
+                return rigidBody;
+            }
+        }
         public override bool Flying
         {
             get
@@ -579,8 +721,8 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             //_velocity == rigidBody.LinearVelocity
             vec.X = this._velocity.X; 
             vec.Y = this._velocity.Y; 
-            vec.Z = this._velocity.Z; 
-
+            vec.Z = this._velocity.Z;
+            if ((vec.X != 0.0f) || (vec.Y != 0.0f) || (vec.Z != 0.0f)) rigidBody.Activate();
             if (flying)
             {
                 //Antigravity with movement
@@ -677,33 +819,37 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         //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;
+        private BulletXScene _parent_scene;
         //_physical value will be linked with the prim object value
         private Boolean _physical = false;
 
-        public Axiom.Math.Vector2 RigidBodyHorizontalPosition
+        public MonoXnaCompactMaths.Vector3 RigidBodyPosition
         {
-            get
-            {
-                return new Axiom.Math.Vector2(this.rigidBody.CenterOfMassPosition.X, this.rigidBody.CenterOfMassPosition.Y);
-            }
+            get { return this.rigidBody.CenterOfMassPosition; }
         }
         public BulletXPrim(BulletXScene parent_scene, PhysicsVector pos, PhysicsVector size, AxiomQuaternion rotation)
-            : this(parent_scene, pos, new PhysicsVector(), size, new PhysicsVector(), rotation)
+            : this("", parent_scene, pos, new PhysicsVector(), size, new PhysicsVector(), rotation, null, null)
         {
         }
-        public BulletXPrim(BulletXScene parent_scene, PhysicsVector pos, PhysicsVector velocity, PhysicsVector size,
-            PhysicsVector aceleration, AxiomQuaternion rotation)
+        public BulletXPrim(String primName, BulletXScene parent_scene, PhysicsVector pos, PhysicsVector size, 
+            AxiomQuaternion rotation, Mesh mesh, PrimitiveBaseShape pbs)
+            : this(primName, parent_scene, pos, new PhysicsVector(), size, new PhysicsVector(), rotation, mesh, pbs)
+        {
+        }
+        public BulletXPrim(String primName, BulletXScene parent_scene, PhysicsVector pos, PhysicsVector velocity, PhysicsVector size,
+            PhysicsVector aceleration, AxiomQuaternion rotation, Mesh mesh, PrimitiveBaseShape pbs)
         {
-            _position = pos;
-            _velocity = velocity;
-            _size = size;
             if ((size.X == 0) || (size.Y == 0) || (size.Z == 0)) throw new Exception("Size 0");
+            if (rotation.Norm == 0f) rotation = AxiomQuaternion.Identity;
 
+            _position = pos;
+            if (_physical) _velocity = velocity; else _velocity = new PhysicsVector();
+            _size = size;
             _acceleration = aceleration;
-            //Because a bug, orientation will be fixed to AxiomQuaternion.Identity
-            _orientation = AxiomQuaternion.Identity;
-            //_orientation = rotation;
-            //---
+            _orientation = rotation;
+
+            _parent_scene = parent_scene;
+
             //For RigidBody Constructor. The next values might change
             float _linearDamping = 0.0f;
             float _angularDamping = 0.0f;
@@ -718,9 +864,9 @@ namespace OpenSim.Region.Physics.BulletXPlugin
                 CollisionShape _collisionShape = new XnaDevRu.BulletX.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
+                if(_physical) _collisionShape.CalculateLocalInertia(Mass, out _localInertia); //Always when mass > 0
                 rigidBody = new RigidBody(Mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping, _friction, _restitution);
-                rigidBody.ActivationState = ActivationState.DisableDeactivation;
+                //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;
@@ -754,8 +900,16 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             {
                 lock (BulletXScene.BulletXLock)
                 {
-                    _velocity = value;
-                    Speed();
+                    //Static objects don' have linear velocity
+                    if (_physical)
+                    {
+                        _velocity = value;
+                        Speed();
+                    }
+                    else
+                    {
+                        _velocity = new PhysicsVector();
+                    }
                 }
             }
         }
@@ -801,7 +955,14 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             get
             {
                 //For now all prims are boxes
-                return _density * _size.X * _size.Y * _size.Z;
+                return (_physical ? 1 : 0) * _density * _size.X * _size.Y * _size.Z;
+            }
+        }
+        public RigidBody RigidBody
+        {
+            get
+            {
+                return rigidBody;
             }
         }
         public override bool Flying
@@ -863,7 +1024,9 @@ namespace OpenSim.Region.Physics.BulletXPlugin
                 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));
+                //Static objects don't have linear velocity
+                if(_physical)
+                    Speed(new PhysicsVector(this.rigidBody.LinearVelocity.X, this.rigidBody.LinearVelocity.Y, 0.0f));
             }
         }
         internal void UpdateKinetics()
@@ -872,18 +1035,12 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             {
                 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();
+                this._orientation = BulletXMaths.XnaQuaternionToAxiomQuaternion(rigidBody.Orientation);
             }
             else //Doesn't updates properties. That's a cancel
             {
                 Translate();
-                Speed();
-                //Orientation is not implemented yet in MonoXnaCompactMaths
-                //ReOrient();
+                //Speed(); //<- Static objects don't have linear velocity
                 ReOrient();
             }
         }
@@ -915,10 +1072,49 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         }
         private void ReSize(PhysicsVector _newSize)
         {
+            //I wonder to know how to resize with a simple instruction in BulletX. It seems that for now there isn't
+            //so i have to do it manually. That's recreating rigidbody
             MonoXnaCompactMaths.Vector3 _newsize;
             _newsize = BulletXMaths.PhysicsVectorToXnaVector3(_newSize);
-            //For now all prims are Boxes
-            rigidBody.CollisionShape = new XnaDevRu.BulletX.BoxShape(BulletXMaths.PhysicsVectorToXnaVector3(_newSize) / 2.0f);
+            if ((_newsize.X == 0) || (_newsize.Y == 0) || (_newsize.Z == 0)) throw new Exception("Size 0");
+
+            //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;
+            RigidBody _tmpRigidBody;
+            _startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(this._position);
+            //For now all prims are boxes
+            CollisionShape _collisionShape = new XnaDevRu.BulletX.BoxShape(BulletXMaths.PhysicsVectorToXnaVector3(_newSize) / 2.0f);
+            DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
+            MonoXnaCompactMaths.Vector3 _localInertia = new MonoXnaCompactMaths.Vector3();
+            if (_physical) _collisionShape.CalculateLocalInertia(Mass, out _localInertia); //Always when mass > 0
+            _tmpRigidBody = 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;
+            _tmpRigidBody.Translate(_vDebugTranslation);
+            //---
+            //There is a bug when trying to remove a rigidBody that is colliding with something..
+            try
+            {
+                this._parent_scene.ddWorld.RemoveRigidBody(rigidBody);
+            }
+            catch(Exception ex)
+            {
+                this._parent_scene.BulletXMessage(this._parent_scene.is_ex_message + ex.Message, true);
+                rigidBody.ActivationState = ActivationState.DisableSimulation;
+                this._parent_scene.AddForgottenRigidBody(rigidBody);
+            }
+            rigidBody = _tmpRigidBody;
+            this._parent_scene.ddWorld.AddRigidBody(rigidBody);
+            if (_physical) Speed();//Static objects don't have linear velocity
+            ReOrient();
+            GC.Collect();
         }
         private void ReOrient()
         {
@@ -935,4 +1131,74 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         #endregion
 
     }
+    /// <summary>
+    /// This Class manage a HeighField as a RigidBody. This is for to be added in the BulletXScene
+    /// </summary>
+    internal class BulletXPlanet
+    {
+        private PhysicsVector _staticPosition;
+        private PhysicsVector _staticVelocity; 
+        private AxiomQuaternion _staticOrientation;
+        private float _mass;
+        private BulletXScene _parentscene;
+        internal float[] _heightField;
+        private RigidBody _flatPlanet;
+        internal RigidBody RigidBody { get { return _flatPlanet; } }
+        internal BulletXPlanet(BulletXScene parent_scene, float[] heightField)
+        {
+            _staticPosition = new PhysicsVector(BulletXScene.MaxXY / 2, BulletXScene.MaxXY/2, 0);
+            _staticVelocity = new PhysicsVector();
+            _staticOrientation = AxiomQuaternion.Identity;
+            _mass = 0; //No active
+            _parentscene = parent_scene;
+            _heightField = heightField;
+
+            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)
+            {
+                try
+                {
+                    _startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(_staticPosition);
+                    CollisionShape _collisionShape = new HeightfieldTerrainShape(BulletXScene.MaxXY, BulletXScene.MaxXY, _heightField, (float)BulletXScene.MaxZ, 2, true, false);
+                    DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
+                    MonoXnaCompactMaths.Vector3 _localInertia = new MonoXnaCompactMaths.Vector3();
+                    //_collisionShape.CalculateLocalInertia(_mass, out _localInertia); //Always when mass > 0
+                    _flatPlanet = new RigidBody(_mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping, _friction, _restitution);
+                    //It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
+                    MonoXnaCompactMaths.Vector3 _vDebugTranslation;
+                    _vDebugTranslation = _startTransform.Translation - _flatPlanet.CenterOfMassPosition;
+                    _flatPlanet.Translate(_vDebugTranslation);
+                    parent_scene.ddWorld.AddRigidBody(_flatPlanet);
+                }
+                catch (Exception ex)
+                {
+                    this._parentscene.BulletXMessage(ex.Message, true);
+                }
+            }
+            this._parentscene.BulletXMessage("BulletXPlanet created.", false);
+        }
+        internal float HeightValue(MonoXnaCompactMaths.Vector3 position)
+        {
+            int li_x, li_y;
+            float height;
+            li_x = (int)Math.Round(position.X); 
+            if (li_x < 0) li_x = 0;
+            if (li_x >= BulletXScene.MaxXY) li_x = BulletXScene.MaxXY - 1;
+            li_y = (int)Math.Round(position.Y); 
+            if (li_y < 0) li_y = 0;
+            if (li_y >= BulletXScene.MaxXY) li_y = BulletXScene.MaxXY - 1;
+
+            height = ((HeightfieldTerrainShape)this._flatPlanet.CollisionShape).getHeightFieldValue(li_x, li_y);
+            if (height < 0) height = 0;
+            else if (height > BulletXScene.MaxZ) height = BulletXScene.MaxZ;
+
+            return height;
+        }
+    }
 }

OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs

@@ -104,6 +104,19 @@ namespace OpenSim.Region.Physics.Manager
 			
 			pluginAssembly = null; 
 		}
+        //---
+        public static void PhysicsPluginMessage(string message, bool isWarning)
+        {
+            if (isWarning)
+            {
+                MainLog.Instance.Warn("PHYSICS", message);
+            }
+            else
+            {
+                MainLog.Instance.Verbose("PHYSICS", message);
+            }
+        }
+        //---
 	}
 
 	public interface IPhysicsPlugin

ThirdPartyLicenses/BulletLicense.txt

@@ -0,0 +1,17 @@
+/*
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+Free for commercial use, but please mail [email protected] to report projects, and join the forum at
+www.continuousphysics.com/Bullet/phpBB2

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/HeightfieldTerrainShape.cs

@@ -0,0 +1,360 @@
+/*
+ * WARNING!: this class is not in the original BulletX
+ * By the way it's based on the Bullet btHeightfieldTerrainShape:
+ * http://www.continuousphysics.com/Bullet/BulletFull/classbtHeightfieldTerrainShape.html
+ *****************************************************************************************
+ * 3RD PARTY LICENSE. The next it's the original 3rd party lincense of Bullet:
+ * ----------------------------------------------------------------------------
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+ * ------------------------------------------------------------------------------
+*/
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+    public class HeightfieldTerrainShape : ConcaveShape
+    {
+        private Vector3 _localAabbMin;
+        private Vector3 _localAabbMax;
+        private Vector3 _localScaling = new Vector3(1f,1f,1f);
+        private int _width;
+        private int _length;
+        private float[] _heightfieldData;
+        private float _maxHeight;
+        private int _upAxis;
+        private bool _useFloatData;
+        private bool _flipQuadEdges;
+        private bool _useDiamondSubdivision = false;
+        private float _defaultCollisionMargin = 0.6f;
+
+        public HeightfieldTerrainShape(int width, int length, float[] heightfieldData, float maxHeight, 
+            int upAxis, bool useFloatData, bool flipQuadEdges)
+        {
+            _width = width;
+            _length = length;
+            _heightfieldData = heightfieldData;
+            _maxHeight = maxHeight;
+            _upAxis = upAxis;
+            _useFloatData = useFloatData;
+            _flipQuadEdges = flipQuadEdges;
+            this.Margin = _defaultCollisionMargin;
+
+            float quantizationMargin = 1f;
+
+            //enlarge the AABB to avoid division by zero when initializing the quantization value
+            Vector3 clampValue = new Vector3(quantizationMargin, quantizationMargin, quantizationMargin);
+            Vector3 halfExtents = new Vector3(0, 0, 0);
+
+            switch (_upAxis)
+            {
+                case 0:
+                    halfExtents.X = _maxHeight;
+                    halfExtents.Y = _width;
+                    halfExtents.Z = _length;
+                    break;
+                case 1:
+                    halfExtents.X = _width;
+                    halfExtents.Y = _maxHeight;
+                    halfExtents.Z = _length;
+                    break;
+                case 2:
+                    halfExtents.X = _width;
+                    halfExtents.Y = _length;
+                    halfExtents.Z = _maxHeight;
+                    break;
+                default:
+                    //need to get valid _upAxis
+                    //btAssert(0);
+                    throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis");
+            }
+
+            halfExtents *= 0.5f;
+
+            _localAabbMin = -halfExtents - clampValue;
+            _localAabbMax = halfExtents + clampValue;
+            //Vector3 aabbSize = new Vector3();
+            //aabbSize = m_localAabbMax - m_localAabbMin;
+
+        }
+
+        protected Vector3 LocalAabbMin 
+        { get { return _localAabbMin; } set { _localAabbMin = value; } }
+        protected Vector3 LocalAabbMax 
+        { get { return _localAabbMax; } set { _localAabbMax = value; } }
+        public override string Name
+        {
+            get
+            {
+                return "HeightfieldTerrain";
+            }
+        }
+        public override Vector3 LocalScaling
+        {
+            get
+            {
+                return _localScaling;
+            }
+            set
+            {
+                _localScaling = value;
+            }
+        }
+        public override float Margin
+        {
+            get
+            {
+                return base.Margin;
+            }
+            set
+            {
+                base.Margin = value;
+            }
+        }
+        public override BroadphaseNativeTypes ShapeType
+        {
+            get { return BroadphaseNativeTypes.Terrain; }
+        }
+        public Vector3 HalfExtents
+        {
+            get
+            {
+                Vector3 halfExtents = new Vector3();
+                switch (_upAxis)
+                {
+                    case 0:
+                        halfExtents.X = 2f;//_maxHeight;
+                        halfExtents.Y = _width;
+                        halfExtents.Z = _length;
+                        break;
+                    case 1:
+                        halfExtents.X = _width;
+                        halfExtents.Y = 2f;// _maxHeight;
+                        halfExtents.Z = _length;
+                        break;
+                    case 2:
+                        halfExtents.X = _width;
+                        halfExtents.Y = _length;
+                        halfExtents.Z = 2f;// _maxHeight;
+                        break;
+                    default:
+                        //need to get valid m_upAxis
+                        //btAssert(0);
+                        throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis");
+                    //break;
+                }
+                halfExtents *= 0.5f;
+                return halfExtents;
+            }
+        }
+
+        public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
+        {
+            //(void)callback;
+            //(void)aabbMax;
+            //(void)aabbMin;
+
+	        //quantize the aabbMin and aabbMax, and adjust the start/end ranges
+
+	        int[] quantizedAabbMin = new int[3];
+	        int[] quantizedAabbMax = new int[3];
+
+	        Vector3	localAabbMin = aabbMin * new Vector3(1f/_localScaling.X,1f/_localScaling.Y,1f/_localScaling.Z );
+	        Vector3	localAabbMax = aabbMax * new Vector3(1f/_localScaling.X,1f/_localScaling.Y,1f/_localScaling.Z);
+        	
+	        quantizeWithClamp(ref quantizedAabbMin, localAabbMin);
+	        quantizeWithClamp(ref quantizedAabbMax, localAabbMax);
+        	
+        	
+
+	        int startX=0;
+	        int endX=_width-1;
+	        int startJ=0;
+	        int endJ=_length-1;
+
+	        switch(_upAxis)
+	        {
+    	        case 0:
+			        quantizedAabbMin[1]+=_width/2-1;
+			        quantizedAabbMax[1]+=_width/2+1;
+			        quantizedAabbMin[2]+=_length/2-1;
+			        quantizedAabbMax[2]+=_length/2+1;
+
+			        if (quantizedAabbMin[1]>startX)
+				        startX = quantizedAabbMin[1];
+			        if (quantizedAabbMax[1]<endX)
+				        endX = quantizedAabbMax[1];
+			        if (quantizedAabbMin[2]>startJ)
+				        startJ = quantizedAabbMin[2];
+			        if (quantizedAabbMax[2]<endJ)
+				        endJ = quantizedAabbMax[2];
+			        break;
+    	        case 1:
+			        quantizedAabbMin[0]+=_width/2-1;
+			        quantizedAabbMax[0]+=_width/2+1;
+			        quantizedAabbMin[2]+=_length/2-1;
+			        quantizedAabbMax[2]+=_length/2+1;
+
+			        if (quantizedAabbMin[0]>startX)
+				        startX = quantizedAabbMin[0];
+			        if (quantizedAabbMax[0]<endX)
+				        endX = quantizedAabbMax[0];
+			        if (quantizedAabbMin[2]>startJ)
+				        startJ = quantizedAabbMin[2];
+			        if (quantizedAabbMax[2]<endJ)
+				        endJ = quantizedAabbMax[2];
+			        break;
+    	        case 2:
+			        quantizedAabbMin[0]+=_width/2-1;
+			        quantizedAabbMax[0]+=_width/2+1;
+			        quantizedAabbMin[1]+=_length/2-1;
+			        quantizedAabbMax[1]+=_length/2+1;
+
+			        if (quantizedAabbMin[0]>startX)
+				        startX = quantizedAabbMin[0];
+			        if (quantizedAabbMax[0]<endX)
+				        endX = quantizedAabbMax[0];
+			        if (quantizedAabbMin[1]>startJ)
+				        startJ = quantizedAabbMin[1];
+			        if (quantizedAabbMax[1]<endJ)
+				        endJ = quantizedAabbMax[1];
+			        break;
+                default:
+        		    //need to get valid m_upAxis
+                    throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis");
+                    //break;
+	        }
+
+	        for(int j=startJ; j<endJ; j++)
+	        {
+		        for(int x=startX; x<endX; x++)
+		        {
+			        Vector3[] vertices = new Vector3[3];
+                    //if (m_flipQuadEdges || (m_useDiamondSubdivision && ((j + x) & 1)))
+			        if (_flipQuadEdges || (_useDiamondSubdivision && (((j + x) & 1) > 0)))
+			        {
+                        //first triangle
+                        getVertex(x,j,ref vertices[0]);
+                        getVertex(x+1,j,ref vertices[1]);
+                        getVertex(x+1,j+1,ref vertices[2]);
+                        //callback->processTriangle(vertices,x,j);
+                        callback.ProcessTriangle(vertices,x,j);
+
+                        //second triangle
+                        getVertex(x,j,ref vertices[0]);
+                        getVertex(x+1,j+1,ref vertices[1]);
+                        getVertex(x,j+1,ref vertices[2]);
+                        //callback->processTriangle(vertices,x,j);
+                        callback.ProcessTriangle(vertices, x, j);
+			        }
+                    else
+			        {
+                        //first triangle
+                        getVertex(x,j,ref vertices[0]);
+                        getVertex(x,j+1,ref vertices[1]);
+                        getVertex(x+1,j,ref vertices[2]);
+                        //callback->processTriangle(vertices,x,j);
+                        callback.ProcessTriangle(vertices,x,j);
+
+                        //second triangle
+                        getVertex(x+1,j,ref vertices[0]);
+                        getVertex(x,j+1,ref vertices[1]);
+                        getVertex(x+1,j+1,ref vertices[2]);
+                        //callback->processTriangle(vertices,x,j);
+                        callback.ProcessTriangle(vertices,x,j);
+                    }
+		        }
+	        }
+        }
+        public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
+        {
+            //aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
+            //aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
+
+            Vector3 halfExtents = (_localAabbMax - _localAabbMin) * _localScaling * 0.5f;
+
+            Vector3 center = t.Translation;
+            Vector3 extent = new Vector3(Math.Abs(halfExtents.X), Math.Abs(halfExtents.Y), Math.Abs(halfExtents.Z));
+            extent += new Vector3(Margin, Margin, Margin);
+
+            aabbMin = center - extent;
+            aabbMax = center + extent;
+        }
+        public override void CalculateLocalInertia(float mass, out Vector3 inertia)
+        {
+            //moving concave objects not supported
+            inertia = new Vector3();
+        }
+        public float getHeightFieldValue(int x,int y)
+        {
+            float  val = 0f;
+            if (_useFloatData)
+	        {
+                val = _heightfieldData[(y * _width) + x];
+	        }
+            else
+	        {
+		        //assume unsigned short int
+                int heightFieldValue = (int)_heightfieldData[(y * _width) + x];
+		        val = heightFieldValue * _maxHeight/65535f;
+	        }
+	        return val;
+        }
+        public void getVertex(int x,int y,ref Vector3 vertex)
+        {
+            if (x < 0) x = 0;
+            if (y < 0) y = 0;
+            if (x >= _width) x = _width - 1;
+            if (y >= _length) y = _length - 1;
+            float height = getHeightFieldValue(x,y);
+            switch(_upAxis)
+            {
+    	        case 0:
+                    vertex.X = height;
+                    vertex.Y = (- _width/2 ) + x;
+                    vertex.Z = (- _length/2 ) + y;
+        			break;
+                case 1:
+                    vertex.X = (- _width/2 ) + x;
+                    vertex.Y = height;
+                    vertex.Z = (- _length/2 ) + y;
+			        break;
+            	case 2:
+                    vertex.X = (- _width/2 ) + x;
+                    vertex.Y = (- _length/2 ) + y;
+                    vertex.Z = height;
+        			break;
+            	default:
+        			//need to get valid m_upAxis
+                    throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis");
+                    //break;
+		    }
+        	vertex *= _localScaling;
+        }
+        public void quantizeWithClamp(ref int[] _out,Vector3 point)
+        {
+	        Vector3 clampedPoint = point;
+            MathHelper.SetMax(ref clampedPoint,_localAabbMin);
+            MathHelper.SetMin(ref clampedPoint, _localAabbMax);
+            Vector3 v = clampedPoint;
+
+	        _out[0] = (int)(v.X);
+	        _out[1] = (int)(v.Y);
+	        _out[2] = (int)(v.Z);
+        	//correct for
+        }
+    }
+}

libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj

@@ -78,6 +78,7 @@
     <Compile Include="Collision\CollisionShapes\CylinderShapeZ.cs" />
     <Compile Include="Collision\CollisionShapes\EmptyShape.cs" />
     <Compile Include="Collision\CollisionShapes\FilteredCallback.cs" />
+    <Compile Include="Collision\CollisionShapes\HeightfieldTerrainShape.cs" />
     <Compile Include="Collision\CollisionShapes\InternalTriangleIndexCallback.cs" />
     <Compile Include="Collision\CollisionShapes\LocalSupportVertexCallback.cs" />
     <Compile Include="Collision\CollisionShapes\MinkowskiSumShape.cs" />

libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs

@@ -573,7 +573,11 @@ namespace MonoXnaCompactMaths
 
         public static Matrix operator /(Matrix matrix1, float divider)
         {
-            throw new NotImplementedException();
+            return new Matrix(
+                matrix1.M11 / divider, matrix1.M12 / divider, matrix1.M13 / divider, matrix1.M14 / divider,
+                matrix1.M21 / divider, matrix1.M22 / divider, matrix1.M23 / divider, matrix1.M24 / divider,
+                matrix1.M31 / divider, matrix1.M32 / divider, matrix1.M33 / divider, matrix1.M34 / divider,
+                matrix1.M41 / divider, matrix1.M42 / divider, matrix1.M43 / divider, matrix1.M44 / divider);
         }
 
 
@@ -658,7 +662,10 @@ namespace MonoXnaCompactMaths
 
         public override string ToString()
         {
-            throw new NotImplementedException();
+            return "[(" + this.M11 + ", " + this.M12 + ", " + this.M13 + ", " + this.M14 + ")\n ("
+                        + this.M21 + ", " + this.M22 + ", " + this.M23 + ", " + this.M24 + ")\n ("
+                        + this.M31 + ", " + this.M32 + ", " + this.M33 + ", " + this.M34 + ")\n ("
+                        + this.M41 + ", " + this.M42 + ", " + this.M43 + ", " + this.M44 + ")]";
         }
 
 

libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs

@@ -90,7 +90,91 @@ namespace MonoXnaCompactMaths
 
         public static Quaternion CreateFromRotationMatrix(Matrix matrix)
         {
-            throw new NotImplementedException();
+            float Omega2 = matrix.M44;
+            if (!isAprox(Omega2, 1f))
+            {
+                //"Normalize" the Rotation matrix. Norma = M44 = Omega2
+                matrix = matrix / Omega2;
+            }
+            //Deducted from: public static Matrix CreateFromQuaternion(Quaternion quaternion)
+            float lambda1pos, lambda2pos, lambda3pos, lambda1neg, lambda2neg, lambda3neg;
+            lambda1pos = (1f - matrix.M11 + matrix.M23 + matrix.M32) / 2f;
+            lambda2pos = (1f - matrix.M22 + matrix.M13 + matrix.M31) / 2f;
+            lambda3pos = (1f - matrix.M33 + matrix.M12 + matrix.M21) / 2f;
+            lambda1neg = (1f - matrix.M11 - matrix.M23 - matrix.M32) / 2f;
+            lambda2neg = (1f - matrix.M22 - matrix.M13 - matrix.M31) / 2f;
+            lambda3neg = (1f - matrix.M33 - matrix.M12 - matrix.M21) / 2f;
+
+            //lambadIS = (qJ + s*qK)^2
+            //q0 = w | q1 = x | q2 = y, q3 = z
+            //Every value of qI (I=1,2,3) has 4 possible values cause the sqrt
+            float[] x = new float[4]; float[] y = new float[4]; float[] z = new float[4];
+            float[] sig1 = {1f, 1f, -1f, -1f};
+            float[] sig2 = {1f, -1f, 1f, -1f};
+            for (int i = 0; i < 4; i++)
+            {
+                x[i] = (sig1[i] * (float)Math.Sqrt(lambda1pos) + sig2[i] * (float)Math.Sqrt(lambda1neg)) / 2f;
+                y[i] = (sig1[i] * (float)Math.Sqrt(lambda2pos) + sig2[i] * (float)Math.Sqrt(lambda2neg)) / 2f;
+                z[i] = (sig1[i] * (float)Math.Sqrt(lambda3pos) + sig2[i] * (float)Math.Sqrt(lambda3neg)) / 2f;
+            }
+
+            //Only a set of x, y, z are the corrects values. So it requires testing
+            int li_i=0, li_j=0, li_k=0;
+            bool lb_testL1P, lb_testL2P, lb_testL3P, lb_testL1N, lb_testL2N, lb_testL3N;
+            bool lb_superLambda = false;
+            while((li_i<4)&&(!lb_superLambda))
+            {
+                while ((li_j < 4) && (!lb_superLambda))
+                {
+                    while ((li_k < 4) && (!lb_superLambda))
+                    {
+                        lb_testL1P = isAprox((float)(
+                            Math.Pow((double)(y[li_j] + z[li_k]), 2.0)), lambda1pos);
+                        lb_testL2P = isAprox((float)(
+                            Math.Pow((double)(x[li_i] + z[li_k]), 2.0)), lambda2pos);
+                        lb_testL3P = isAprox((float)(
+                            Math.Pow((double)(x[li_i] + y[li_j]), 2.0)), lambda3pos);
+                        lb_testL1N = isAprox((float)(
+                            Math.Pow((double)(y[li_j] - z[li_k]), 2.0)), lambda1neg);
+                        lb_testL2N = isAprox((float)(
+                            Math.Pow((double)(x[li_i] - z[li_k]), 2.0)), lambda2neg);
+                        lb_testL3N = isAprox((float)(
+                            Math.Pow((double)(x[li_i] - y[li_j]), 2.0)), lambda3neg);
+
+                        lb_superLambda = (lb_testL1P && lb_testL2P && lb_testL3P
+                            && lb_testL1N && lb_testL2N && lb_testL3N);
+
+                        if (!lb_superLambda) li_k++;
+                    }
+                    if (!lb_superLambda) li_j++;
+                }
+                if (!lb_superLambda) li_i++;
+            }
+
+            Quaternion q = new Quaternion();
+
+            if (lb_superLambda)
+            {
+                q.X = x[li_i]; q.Y = y[li_j]; q.Z = z[li_k];
+                q.W = (matrix.M12 - 2f * q.X * q.Y) / (2f * q.Z);
+
+                if (!isAprox(Omega2, 1f))
+                {
+                    if (Omega2 < 0) throw new Exception("Quaternion.CreateFromRotationMatrix: Omega2 is negative!");
+                    q = q * (float)Math.Sqrt(Omega2);//2 possibles values (+/-). For now only 1.
+                }
+            }
+            else
+            {
+                q = Quaternion.identity;
+            }
+
+            return q;
+        }
+        private static float floatError = 0.000001f;
+        private static bool isAprox(float test, float realValue)
+        {
+            return (((realValue * (1f - floatError)) <= test) && (test <= (realValue * (1f + floatError))));
         }
 
 
@@ -317,7 +401,8 @@ namespace MonoXnaCompactMaths
 
         public static Quaternion operator *(Quaternion quaternion1, float scaleFactor)
         {
-            throw new NotImplementedException();
+            return new Quaternion(quaternion1.X / scaleFactor, quaternion1.Y / scaleFactor,
+                quaternion1.Z / scaleFactor, quaternion1.W / scaleFactor);
         }
 
 
@@ -335,7 +420,7 @@ namespace MonoXnaCompactMaths
 
         public override string ToString()
         {
-            throw new NotImplementedException();
+            return "(" + this.X + ", " + this.Y + ", " + this.Z + ", " + this.W + ")";
         }
 
         private static void Conjugate(ref Quaternion quaternion, out Quaternion result)