7 files changed, 303 insertions, 17 deletions

diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
index 545f461..5ac651f 100644
--- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
+++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
@@ -258,7 +258,7 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
 
         public override PhysicsVector Size
         {
-            get { return new PhysicsVector(0, 0, 0); }
+            get { return PhysicsVector.Zero; }
             set { }
         }
 
@@ -269,7 +269,22 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
                 return;
             }
         }
-
+        public override float Mass
+        {
+            get { return 0f; }
+        }
+        public override PhysicsVector Force
+        {
+            get { return PhysicsVector.Zero; }
+        }
+        public override PhysicsVector CenterOfMass
+        {
+            get { return PhysicsVector.Zero; }
+        }
+        public override PhysicsVector GeometricCenter
+        {
+            get { return PhysicsVector.Zero; }
+        }
         public override PhysicsVector Velocity
         {
             get { return _velocity; }
diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
index 1760e50..5602301 100644
--- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
+++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
@@ -511,10 +511,12 @@ namespace OpenSim.Region.Physics.BulletXPlugin
                 GC.Collect();
             }
         }
+
         public override void AddPhysicsActorTaint(PhysicsActor prim)
         {
 
         }
+
         public override float Simulate(float timeStep)
         {
             float fps = 0;
@@ -735,11 +737,13 @@ namespace OpenSim.Region.Physics.BulletXPlugin
                 }
             }
         }
+
         public override PhysicsVector RotationalVelocity
         {
             get { return m_rotationalVelocity; }
             set { m_rotationalVelocity = value; }
         }
+
         public override PhysicsVector Velocity
         {
             get { return _velocity; }
@@ -760,12 +764,28 @@ namespace OpenSim.Region.Physics.BulletXPlugin
                 }
             }
         }
+
         public override PhysicsVector Size
         {
             get { return _size; }
             set { lock (BulletXScene.BulletXLock) { _size = value; } }
         }
 
+        public override PhysicsVector Force
+        {
+            get { return PhysicsVector.Zero; }
+        }
+
+        public override PhysicsVector CenterOfMass
+        {
+            get { return PhysicsVector.Zero; }
+        }
+
+        public override PhysicsVector GeometricCenter
+        {
+            get { return PhysicsVector.Zero; }
+        }
+
         public override PrimitiveBaseShape Shape
         {
             set
@@ -773,15 +793,18 @@ namespace OpenSim.Region.Physics.BulletXPlugin
                 return;
             }
         }
+
         public override bool SetAlwaysRun
         {
             get { return false; }
             set { return; }
         }
+
         public override PhysicsVector Acceleration
         {
             get { return _acceleration; }
         }
+
         public override AxiomQuaternion Orientation
         {
             get { return _orientation; }
@@ -794,10 +817,17 @@ namespace OpenSim.Region.Physics.BulletXPlugin
                 }
             }
         }
-        public virtual float Mass 
+
+        public override float Mass 
         { 
-            get { return 0; } 
+            get { return ActorMass; } 
+        }
+
+        public virtual float ActorMass
+        {
+            get { return 0; }
         }
+
         public override int PhysicsActorType
         {
             get { return (int) m_PhysicsActorType; }
@@ -808,6 +838,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         {
             get { return rigidBody; }
         }
+
         public Vector3 RigidBodyPosition
         {
             get { return this.rigidBody.CenterOfMassPosition; }
@@ -829,21 +860,25 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             get { return false; }
             set { return; }
         }
+
         public override bool IsColliding
         {
             get { return iscolliding; }
             set { iscolliding = value; }
         }
+
         public override bool CollidingGround
         {
             get { return false; }
             set { return; }
         }
+
         public override bool CollidingObj
         {
             get { return false; }
             set { return; }
         }
+
         public virtual void SetAcceleration(PhysicsVector accel)
         {
             lock (BulletXScene.BulletXLock)
@@ -851,6 +886,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
                 _acceleration = accel;
             }
         }
+
         public override bool Kinematic
         {
             get
@@ -862,16 +898,20 @@ namespace OpenSim.Region.Physics.BulletXPlugin
 
             }
         }
+
         public override void AddForce(PhysicsVector force)
         {
 
         }
+
         public override void SetMomentum(PhysicsVector momentum)
         {
         }
+
         internal virtual void ValidateHeight(float heighmapPositionValue)
         {
         }
+
         internal virtual void UpdateKinetics()
         {
         }
@@ -1176,7 +1216,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
             get { return base.Orientation; }
             set { base.Orientation = value; }
         }
-        public override float Mass
+        public override float ActorMass
         {
             get
             {
diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
index 7e8d160..5894ab0 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
@@ -157,6 +157,14 @@ namespace OpenSim.Region.Physics.Manager
 
         public abstract PhysicsVector Position { get; set; }
 
+        public abstract float Mass { get; }
+
+        public abstract PhysicsVector Force { get;}
+
+        public abstract PhysicsVector GeometricCenter { get; }
+
+        public abstract PhysicsVector CenterOfMass { get; }
+
         public abstract PhysicsVector Velocity { get; set; }
 
         public abstract PhysicsVector Acceleration { get; }
@@ -210,7 +218,22 @@ namespace OpenSim.Region.Physics.Manager
             get { return PhysicsVector.Zero; }
             set { return; }
         }
-
+        public override float Mass
+        {
+            get { return 0f; }
+        }
+        public override PhysicsVector Force
+        {
+            get { return PhysicsVector.Zero; }
+        }
+        public override PhysicsVector CenterOfMass
+        {
+            get { return PhysicsVector.Zero; }
+        }
+        public override PhysicsVector GeometricCenter
+        {
+            get { return PhysicsVector.Zero; }
+        }
         public override PrimitiveBaseShape Shape
         {
             set
diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index 6f22ecf..04bf9a0 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -46,6 +46,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private PhysicsVector _target_velocity;
         private PhysicsVector _acceleration;
         private PhysicsVector m_rotationalVelocity;
+        private float m_density = 50f;
         private bool m_pidControllerActive = true;
         private static float PID_D = 3020.0f;
         private static float PID_P = 7000.0f;
@@ -93,7 +94,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
 
                 Shell = d.CreateCapsule(parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH);
-                d.MassSetCapsule(out ShellMass, 50.0f, 3, 0.4f, 1.0f);
+                d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH);
                 Body = d.BodyCreate(parent_scene.world);
                 d.BodySetMass(Body, ref ShellMass);
                 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
@@ -273,7 +274,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     d.GeomDestroy(Shell);
                     //MainLog.Instance.Verbose("PHYSICS", "Set Avatar Height To: " + (CAPSULE_RADIUS + CAPSULE_LENGTH));
                     Shell = d.CreateCapsule(_parent_scene.space, capsuleradius, CAPSULE_LENGTH);
-                    d.MassSetCapsule(out ShellMass, 50.0f, 3, CAPSULE_RADIUS, CAPSULE_LENGTH);
+                    d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH);
                     Body = d.BodyCreate(_parent_scene.world);
                     d.BodySetMass(Body, ref ShellMass);
                     d.BodySetPosition(Body, _position.X, _position.Y, _position.Z + Math.Abs(CAPSULE_LENGTH - prevCapsule));
@@ -283,6 +284,29 @@ namespace OpenSim.Region.Physics.OdePlugin
                 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
             }
         }
+        public override float Mass
+        {
+            get {
+
+                float AVvolume = (float)(Math.PI * Math.Pow(CAPSULE_RADIUS, 2) * CAPSULE_LENGTH);
+                return m_density * AVvolume;
+            }
+        }
+
+        public override PhysicsVector Force
+        {
+            get { return new PhysicsVector(_target_velocity.X,_target_velocity.Y,_target_velocity.Z); }
+        }
+
+        public override PhysicsVector CenterOfMass
+        {
+            get { return PhysicsVector.Zero; }
+        }
+
+        public override PhysicsVector GeometricCenter
+        {
+            get { return PhysicsVector.Zero; }
+        }
 
         public override PrimitiveBaseShape Shape
         {
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 2ec8131..26cb24b 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -65,6 +65,9 @@ namespace OpenSim.Region.Physics.OdePlugin
         private bool m_throttleUpdates = false;
         private int throttleCounter = 0;
         public bool outofBounds = false;
+        private float m_density = 0f;
+        
+        
 
         public bool _zeroFlag = false;
         private bool m_lastUpdateSent = false;
@@ -73,7 +76,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private String m_primName;
         private PhysicsVector _target_velocity;
         public d.Mass pMass;
-        private const float MassMultiplier = 150f; //  Ref: Water: 1000kg..  this iset to 500
+        private const float MassMultiplier = 150f;
         private int debugcounter = 0;
 
         public OdePrim(String primName, OdeScene parent_scene, IntPtr targetSpace, PhysicsVector pos, PhysicsVector size,
@@ -182,15 +185,104 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             _parent_scene.addActivePrim(this);
         }
-        public void setMass()
+        private float CalculateMass()
         {
-            //Sets Mass based on member MassMultiplier.   
+            float volume = 0;
+            
+            // No material is passed to the physics engines yet..  soo..   
+            float density = 2.7f; // Aluminum g/cm3;
+
+            float returnMass = 0;
+
+            switch (_pbs.ProfileShape)
+            {
+                case ProfileShape.Square:
+                    // Profile Volume
+                    
+                    volume = _size.X * _size.Y * _size.Z;
+
+                    // If the user has 'hollowed out' 
+                    if (((float)_pbs.ProfileHollow / 50000f) > 0.0)
+                    {
+                        float hollowAmount = (float)_pbs.ProfileHollow / 50000f;
+                        //break;
+                        float hollowVolume = 0;
+                        switch (_pbs.HollowShape)
+                        {
+                            case HollowShape.Square:
+                            case HollowShape.Same:
+                                // Cube Hollow
+                                float hollowsizex = _size.X * hollowAmount;
+                                float hollowsizey = _size.Y * hollowAmount;
+                                float hollowsizez = _size.Z * hollowAmount;
+                                hollowVolume = hollowsizex * hollowsizey * hollowsizez;
+                                break;
+
+                            case HollowShape.Circle:
+                                // Hollow shape is a perfect cyllinder in respect to the cube's scale
+                                float hRadius = _size.X / 2;
+                                float hLength = _size.Z;
+
+                                // pi * r2 * h
+                                hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount);
+                                break;
+
+                            case HollowShape.Triangle:
+                                float aLength = _size.Y; // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
+                                // 1/2 abh
+                                hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
+                                break;
+
+                            default:
+                                hollowVolume = 0;
+                                break;
+                        }
+                        volume = volume - hollowVolume;
+                        
+                    }
+                    
+                    break;
+
+                default:
+                    volume = _size.X * _size.Y * _size.Z;
+                    break;
+            }
+
+            // Calculate Path cut effect on volume
+            // Not exact, in the triangle hollow example
+            // They should ever be less then zero..   
+            // we'll ignore it if it's less then zero
+            float PathCutEndAmount = _pbs.ProfileEnd;
+            float PathCutStartAmount = _pbs.ProfileBegin;
+            if (((PathCutStartAmount + PathCutEndAmount)/50000f) > 0.0f)
+            {
+
+                float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount) / 50000f);
+                
+                if (pathCutAmount >= 0.99f) 
+                    pathCutAmount=0.99f;
+
+                volume = volume - (volume * pathCutAmount);
+            }
+            
+            returnMass = density * volume;
+
+            return returnMass;
+        }
+
+        public void setMass()
+        {    
             if (Body != (IntPtr)0)
             {
-                d.MassSetBox(out pMass, (_size.X * _size.Y * _size.Z * MassMultiplier), _size.X, _size.Y, _size.Z);
+               //if (_pbs.ProfileShape = ProfileShape.Square) {
+
+                d.MassSetBoxTotal(out pMass, CalculateMass(), _size.X, _size.Y, _size.Z);
                 d.BodySetMass(Body, ref pMass);
             }
         }
+
+       
+
         public void disableBody()
         {
             //this kills the body so things like 'mesh' can re-create it.
@@ -212,7 +304,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             int[] indexList = mesh.getIndexListAsIntLocked(); // Also pinned, needs release after usage
             int VertexCount = vertexList.GetLength(0) / 3;
             int IndexCount = indexList.GetLength(0);
-
+            
             _triMeshData = d.GeomTriMeshDataCreate();
 
             d.GeomTriMeshDataBuildSimple(_triMeshData, vertexList, 3 * sizeof(float), VertexCount, indexList, IndexCount,
@@ -220,7 +312,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             d.GeomTriMeshDataPreprocess(_triMeshData);
 
             prim_geom = d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null);
-
+            
             if (IsPhysical && Body == (IntPtr)0)
             {
                 // Recreate the body
@@ -502,6 +594,26 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+        public override float Mass
+        {
+            get { return CalculateMass(); }
+        }
+
+        public override PhysicsVector Force
+        {
+            get { return PhysicsVector.Zero; }
+        }
+
+        public override PhysicsVector CenterOfMass
+        {
+            get { return PhysicsVector.Zero; }
+        }
+
+        public override PhysicsVector GeometricCenter
+        {
+            get { return PhysicsVector.Zero; }
+        }
+
         public override PrimitiveBaseShape Shape
         {
             set
diff --git a/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs b/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
index a565598..26310f9 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
@@ -383,6 +383,22 @@ namespace OpenSim.Region.Physics.POSPlugin
             get { return new PhysicsVector(0.5f, 0.5f, 1.0f); }
             set { }
         }
+        public override float Mass
+        {
+            get { return 0f; }
+        }
+        public override PhysicsVector Force
+        {
+            get { return PhysicsVector.Zero; }
+        }
+        public override PhysicsVector CenterOfMass
+        {
+            get { return PhysicsVector.Zero; }
+        }
+        public override PhysicsVector GeometricCenter
+        {
+            get { return PhysicsVector.Zero; }
+        }
 
         public override PrimitiveBaseShape Shape
         {
@@ -492,6 +508,26 @@ namespace OpenSim.Region.Physics.POSPlugin
             set { _size = value; }
         }
 
+        public override float Mass
+        {
+            get { return 0f; }
+        }
+
+        public override PhysicsVector Force
+        {
+            get { return PhysicsVector.Zero; }
+        }
+
+        public override PhysicsVector CenterOfMass
+        {
+            get { return PhysicsVector.Zero; }
+        }
+
+        public override PhysicsVector GeometricCenter
+        {
+            get { return PhysicsVector.Zero; }
+        }
+
         public override PrimitiveBaseShape Shape
         {
             set
diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
index bc28626..8525e75 100644
--- a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
+++ b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
@@ -270,10 +270,25 @@ namespace OpenSim.Region.Physics.PhysXPlugin
 
         public override PhysicsVector Size
         {
-            get { return new PhysicsVector(0, 0, 0); }
+            get { return PhysicsVector.Zero; }
             set { }
         }
-
+        public override float Mass
+        {
+            get { return 0f; }
+        }
+        public override PhysicsVector Force
+        {
+            get { return PhysicsVector.Zero; }
+        }
+        public override PhysicsVector CenterOfMass
+        {
+            get { return PhysicsVector.Zero; }
+        }
+        public override PhysicsVector GeometricCenter
+        {
+            get { return PhysicsVector.Zero; }
+        }
         public override PhysicsVector Velocity
         {
             get { return _velocity; }
@@ -507,8 +522,29 @@ namespace OpenSim.Region.Physics.PhysXPlugin
 
         public override PhysicsVector Size
         {
-            get { return new PhysicsVector(0, 0, 0); }
+            get { return PhysicsVector.Zero; }
             set { }
         }
+
+        public override float Mass
+        {
+            get { return 0f; }
+        }
+
+        public override PhysicsVector Force
+        {
+            get { return PhysicsVector.Zero; }
+        }
+
+        public override PhysicsVector CenterOfMass
+        {
+            get { return PhysicsVector.Zero; }
+        }
+
+        public override PhysicsVector GeometricCenter
+        {
+            get { return PhysicsVector.Zero; }
+        }
+
     }
 }