6 files changed, 462 insertions, 145 deletions

diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
index 5f33452..54e8d50 100644
--- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
@@ -2181,7 +2181,7 @@ namespace OpenSim.Region.Framework.Scenes
             }
         }
 
-        public void setAngularImpulse(Vector3 impulse)
+        public void ApplyAngularImpulse(Vector3 impulse)
         {
             PhysicsActor pa = RootPart.PhysActor;
 
@@ -2189,7 +2189,7 @@ namespace OpenSim.Region.Framework.Scenes
             {
                 if (!IsAttachment)
                 {
-                    pa.Torque = impulse;
+                    pa.AddAngularForce(impulse, true);
                     m_scene.PhysicsScene.AddPhysicsActorTaint(pa);
                 }
             }
diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
index aed25a7..951e987 100644
--- a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
@@ -1789,7 +1789,7 @@ namespace OpenSim.Region.Framework.Scenes
                 impulse = newimpulse;
             }
 
-            ParentGroup.setAngularImpulse(impulse);
+            ParentGroup.ApplyAngularImpulse(impulse);
         }
 
         /// <summary>
@@ -2153,26 +2153,26 @@ namespace OpenSim.Region.Framework.Scenes
                                 if (ParentGroup.RootPart == this)
                                     AngularVelocity = new Vector3(0, 0, 0);
 
-                                if (PhysActor.Phantom)
+                                if (pa.Phantom)
                                 {
                                     RemoveFromPhysics();
                                     return;
                                 }
 
-                                PhysActor.IsPhysical = UsePhysics;
-                                PhysActor.OnRequestTerseUpdate -= PhysicsRequestingTerseUpdate;
-                                PhysActor.OnOutOfBounds -= PhysicsOutOfBounds;
-                                PhysActor.delink();
+                                pa.IsPhysical = UsePhysics;
+                                pa.OnRequestTerseUpdate -= PhysicsRequestingTerseUpdate;
+                                pa.OnOutOfBounds -= PhysicsOutOfBounds;
+                                pa.delink();
                                 if (ParentGroup.Scene.PhysicsScene.SupportsNINJAJoints)
                                 {
                                     // destroy all joints connected to this now deactivated body
-                                    ParentGroup.Scene.PhysicsScene.RemoveAllJointsConnectedToActorThreadLocked(PhysActor);
+                                    ParentGroup.Scene.PhysicsScene.RemoveAllJointsConnectedToActorThreadLocked(pa);
                                 }
                             }
                         }
 
-                        if (PhysActor.IsPhysical != UsePhysics)
-                            PhysActor.IsPhysical = UsePhysics;
+                        if (pa.IsPhysical != UsePhysics)
+                            pa.IsPhysical = UsePhysics;
 
                         if (UsePhysics)
                         {
@@ -2186,26 +2186,19 @@ namespace OpenSim.Region.Framework.Scenes
 
                             if (ParentID != 0 && ParentID != LocalId)
                             {
-                                ParentGroup.Scene.AddPhysicalPrim(1);
+                                PhysicsActor parentPa = ParentGroup.RootPart.PhysActor;
 
-                                pa.OnRequestTerseUpdate += PhysicsRequestingTerseUpdate;
-                                pa.OnOutOfBounds += PhysicsOutOfBounds;
-                                if (ParentID != 0 && ParentID != LocalId)
+                                if (parentPa != null)
                                 {
-                                    PhysicsActor parentPa = ParentGroup.RootPart.PhysActor;
-
-                                    if (parentPa != null)
-                                    {
-                                        pa.link(parentPa);
-                                    }
+                                    pa.link(parentPa);
                                 }
                             }
                         }                           
                     }
 
                     bool phan = ((Flags & PrimFlags.Phantom) != 0);
-                    if (PhysActor.Phantom != phan)
-                        PhysActor.Phantom = phan;
+                    if (pa.Phantom != phan)
+                        pa.Phantom = phan;
 
 
                     // If this part is a sculpt then delay the physics update until we've asynchronously loaded the
@@ -2327,7 +2320,10 @@ namespace OpenSim.Region.Framework.Scenes
             PhysicsActor pa = PhysActor;
 
             if (pa != null)
-                return new Vector3(pa.CenterOfMass.X, pa.CenterOfMass.Y, pa.CenterOfMass.Z);
+            {
+                Vector3 vtmp = pa.CenterOfMass;
+                return vtmp;
+            }
             else
                 return new Vector3(0, 0, 0);
         }
@@ -3531,18 +3527,12 @@ namespace OpenSim.Region.Framework.Scenes
             PhysicsActor pa = PhysActor;
 
             if (pa != null)
-                pa.FloatOnWater = floatYN == 1;
+                pa.FloatOnWater = (floatYN == 1);
         }
 
         public void SetForce(Vector3 force)
         {
             Force = force;
-/*
-            if (PhysActor != null)
-            {
-                PhysActor.Force = force;
-            }
- */
         }
 
         public SOPVehicle sopVehicle
@@ -5073,33 +5063,32 @@ namespace OpenSim.Region.Framework.Scenes
             }
 
             PhysicsActor pa = PhysActor;
-
-            if (
-                ((AggregateScriptEvents & scriptEvents.collision) != 0) ||
-                ((AggregateScriptEvents & scriptEvents.collision_end) != 0) ||
-                ((AggregateScriptEvents & scriptEvents.collision_start) != 0) ||
-                ((AggregateScriptEvents & scriptEvents.land_collision_start) != 0) ||
-                ((AggregateScriptEvents & scriptEvents.land_collision) != 0) ||
-                ((AggregateScriptEvents & scriptEvents.land_collision_end) != 0) ||
-                (CollisionSound != UUID.Zero)
-                )
+            if (pa != null)
             {
-                // subscribe to physics updates.
-                if (pa != null)
+                const scriptEvents NeededSubsEvents = (
+                            scriptEvents.collision | scriptEvents.collision_start| scriptEvents.collision_end |
+                            scriptEvents.land_collision | scriptEvents.land_collision_start | scriptEvents.land_collision_end
+                            );
+                if (
+//                    ((AggregateScriptEvents & scriptEvents.collision) != 0) ||
+//                    ((AggregateScriptEvents & scriptEvents.collision_end) != 0) ||
+//                    ((AggregateScriptEvents & scriptEvents.collision_start) != 0) ||
+//                    ((AggregateScriptEvents & scriptEvents.land_collision_start) != 0) ||
+//                    ((AggregateScriptEvents & scriptEvents.land_collision) != 0) ||
+//                    ((AggregateScriptEvents & scriptEvents.land_collision_end) != 0) ||
+                    ((AggregateScriptEvents & NeededSubsEvents) != 0) || (CollisionSound != UUID.Zero)
+                    )
                 {
+                    // subscribe to physics updates.
                     pa.OnCollisionUpdate += PhysicsCollision;
                     pa.SubscribeEvents(1000);
                 }
-            }
-            else
-            {
-                if (pa != null)
+                else
                 {
                     pa.UnSubscribeEvents();
                     pa.OnCollisionUpdate -= PhysicsCollision;
                 }
             }
-
             //if ((GetEffectiveObjectFlags() & (uint)PrimFlags.Scripted) != 0)
             //{
             //    ParentGroup.Scene.EventManager.OnScriptTimerEvent += handleTimerAccounting;
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
index 9a22331..4266fda 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -137,7 +137,6 @@ namespace OpenSim.Region.Physics.OdePlugin
         public bool bad = false;
 
         float mu;
-        float bounce;
 
         public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float density, float walk_divisor, float rundivisor)
         {
@@ -170,7 +169,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             m_mass = 80f; // sure we have a default
 
             mu = parent_scene.AvatarFriction;
-            bounce = parent_scene.AvatarBounce;
 
             walkDivisor = walk_divisor;
             runDivisor = rundivisor;
@@ -194,7 +192,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         public override void getContactData(ref ContactData cdata)
         {
             cdata.mu = mu;
-            cdata.bounce = bounce;
+            cdata.bounce = 0;
             cdata.softcolide = false;
         }
 
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
index d0b4546..dcd02e2 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
@@ -153,7 +153,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (m_linearFrictionTimescale.Z < timestep) m_linearFrictionTimescale.Z = timestep;
 
             m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
-            if (m_linearMotorDecayTimescale < 0.5f) m_linearMotorDecayTimescale = 0.5f;
+            if (m_linearMotorDecayTimescale < timestep) m_linearMotorDecayTimescale = timestep;
             m_linearMotorDecayTimescale *= invtimestep;
 
             m_linearMotorTimescale = vd.m_linearMotorTimescale;
@@ -168,7 +168,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (m_angularMotorTimescale < timestep) m_angularMotorTimescale = timestep;
 
             m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
-            if (m_angularMotorDecayTimescale < 0.5f) m_angularMotorDecayTimescale = 0.5f;
+            if (m_angularMotorDecayTimescale < timestep) m_angularMotorDecayTimescale = timestep;
             m_angularMotorDecayTimescale *= invtimestep;
 
             m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
@@ -230,9 +230,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_angularDeflectionTimescale = pValue;
                     break;
                 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
-                    //                    if (pValue < timestep) pValue = timestep;
+                    if (pValue < timestep) pValue = timestep;
                     // try to make impulses to work a bit better
-                    if (pValue < 0.5f) pValue = 0.5f;
+//                    if (pValue < 0.5f) pValue = 0.5f;
                     else if (pValue > 120) pValue = 120;
                     m_angularMotorDecayTimescale = pValue * invtimestep;
                     break;
@@ -281,9 +281,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_linearDeflectionTimescale = pValue;
                     break;
                 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
-                    //                    if (pValue < timestep) pValue = timestep;
+                    if (pValue < timestep) pValue = timestep;
                     // try to make impulses to work a bit better
-                    if (pValue < 0.5f) pValue = 0.5f;
+                    //if (pValue < 0.5f) pValue = 0.5f;
                     else if (pValue > 120) pValue = 120;
                     m_linearMotorDecayTimescale = pValue * invtimestep;
                     break;
@@ -444,9 +444,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
                     m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
                     m_linearMotorTimescale = 1000;
-                    m_linearMotorDecayTimescale = 120;
+                    m_linearMotorDecayTimescale = 120 * invtimestep;
                     m_angularMotorTimescale = 1000;
-                    m_angularMotorDecayTimescale = 1000;
+                    m_angularMotorDecayTimescale = 1000  * invtimestep;
                     m_VhoverHeight = 0;
                     m_VhoverEfficiency = 1;
                     m_VhoverTimescale = 1000;
@@ -901,7 +901,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                 GetRollPitch(irotq, out roll, out pitch);
 
                 float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE;
-                float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE;
+                float ftmp2;
+                if (m_bankingEfficiency == 0)
+                    ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE;
+                else
+                    ftmp2 = 0;
 
                 if (roll > halfpi)
                     roll = pi - roll;
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
index 71aec4c..c4dc793 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -70,6 +70,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         private bool m_fakeisphantom;

 

         protected bool m_building;

+        protected bool m_forcePosOrRotation;

+

         private Quaternion m_lastorientation = new Quaternion();

         private Quaternion _orientation;

 

@@ -128,9 +130,10 @@ namespace OpenSim.Region.Physics.OdePlugin
 

         public bool m_disabled;

 

-

         public uint m_localID;

 

+        private IMesh m_mesh;

+        private object m_meshlock = new object();

         private PrimitiveBaseShape _pbs;

         public OdeScene _parent_scene;

 

@@ -482,6 +485,24 @@ namespace OpenSim.Region.Physics.OdePlugin
         {

             set

             {

+/*                

+                IMesh mesh = null;

+                if (_parent_scene.needsMeshing(value))

+                {

+                    bool convex;

+                    if (m_shapetype == 0)

+                        convex = false;

+                    else

+                        convex = true;

+                    mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);

+                }

+

+                if (mesh != null)

+                {

+                    lock (m_meshlock)

+                        m_mesh = mesh;

+                }

+*/

                 AddChange(changes.Shape, value);

             }

         }

@@ -497,7 +518,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 m_shapetype = value;

                 AddChange(changes.Shape, null);

             }

-        }


+        }

+

 

         public override Vector3 Velocity

         {

@@ -947,6 +969,19 @@ namespace OpenSim.Region.Physics.OdePlugin
 

             CalcPrimBodyData();

 

+            m_mesh = null;

+            if (_parent_scene.needsMeshing(pbs))

+            {

+                bool convex;

+                if (m_shapetype == 0)

+                    convex = false;

+                else

+                    convex = true;

+

+                m_mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);

+            }

+

+

             m_building = true; // control must set this to false when done

 

             AddChange(changes.Add, null);

@@ -1049,6 +1084,10 @@ namespace OpenSim.Region.Physics.OdePlugin
 

         private bool setMesh(OdeScene parent_scene)

         {

+            IntPtr vertices, indices;

+            int vertexCount, indexCount;

+            int vertexStride, triStride;

+

             if (Body != IntPtr.Zero)

             {

                 if (childPrim)

@@ -1065,38 +1104,50 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }

             }

 

-            bool convex;

-            if (m_shapetype == 0)

-                convex = false;

-            else

-                convex = true;

+            IMesh mesh = null;

+

 

-            IMesh mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true,convex);

-            if (mesh == null)

+            lock (m_meshlock)

             {

-                m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z);

-                return false;

-            }

+                if (m_mesh == null)

+                {

+                    bool convex;

+                    if (m_shapetype == 0)

+                        convex = false;

+                    else

+                        convex = true;

 

-            IntPtr vertices, indices;

-            int vertexCount, indexCount;

-            int vertexStride, triStride;

+                    mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);

+                }

+                else

+                {

+                    mesh = m_mesh;

+                }

 

-            mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap

-            mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage

+                if (mesh == null)

+                {

+                    m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z);

+                    return false;

+                }

 

-            if (vertexCount == 0 || indexCount == 0)

-            {

-                m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}",

-                    Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString());

-                mesh.releaseSourceMeshData();

-                return false;

-            }

 

-            primOOBoffset = mesh.GetCentroid();

-            hasOOBoffsetFromMesh = true;

+                mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap

+                mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage

 

-            mesh.releaseSourceMeshData();

+                if (vertexCount == 0 || indexCount == 0)

+                {

+                    m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}",

+                        Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString());

+                    mesh.releaseSourceMeshData();

+                    return false;

+                }

+

+                primOOBoffset = mesh.GetCentroid();

+                hasOOBoffsetFromMesh = true;

+

+                mesh.releaseSourceMeshData();

+                m_mesh = null;

+            }

 

             IntPtr geo = IntPtr.Zero;

 

@@ -1429,7 +1480,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             d.RfromQ(out mymat, ref myrot);

             d.MassRotate(ref objdmass, ref mymat);

 

-            // set the body rotation and position

+            // set the body rotation

             d.BodySetRotation(Body, ref mymat);

 

             // recompute full object inertia if needed

@@ -1725,6 +1776,215 @@ namespace OpenSim.Region.Physics.OdePlugin
             m_collisionscore = 0;

         }

 

+        private void FixInertia(Vector3 NewPos,Quaternion newrot)

+        {

+            d.Matrix3 mat = new d.Matrix3();

+            d.Quaternion quat = new d.Quaternion();

+

+            d.Mass tmpdmass = new d.Mass { };         

+            d.Mass objdmass = new d.Mass { };

+

+            d.BodyGetMass(Body, out tmpdmass);

+            objdmass = tmpdmass;

+

+            d.Vector3 dobjpos;

+            d.Vector3 thispos;

+

+            // get current object position and rotation

+            dobjpos = d.BodyGetPosition(Body);

+

+            // get prim own inertia in its local frame

+            tmpdmass = primdMass;

+

+            // transform to object frame

+            mat = d.GeomGetOffsetRotation(prim_geom);

+            d.MassRotate(ref tmpdmass, ref mat);

+

+            thispos = d.GeomGetOffsetPosition(prim_geom);

+            d.MassTranslate(ref tmpdmass,

+                            thispos.X,

+                            thispos.Y,

+                            thispos.Z);

+

+            // subtract current prim inertia from object

+            DMassSubPartFromObj(ref tmpdmass, ref objdmass);

+

+            // back prim own inertia 

+            tmpdmass = primdMass;

+

+            // update to new position and orientation

+            _position = NewPos;

+            d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);

+            _orientation = newrot;

+            quat.X = newrot.X;

+            quat.Y = newrot.Y;

+            quat.Z = newrot.Z;

+            quat.W = newrot.W;

+            d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);

+

+            mat = d.GeomGetOffsetRotation(prim_geom);

+            d.MassRotate(ref tmpdmass, ref mat);

+

+            thispos = d.GeomGetOffsetPosition(prim_geom);

+            d.MassTranslate(ref tmpdmass,

+                            thispos.X,

+                            thispos.Y,

+                            thispos.Z);

+

+            d.MassAdd(ref objdmass, ref tmpdmass);

+

+            // fix all positions

+            IntPtr g = d.BodyGetFirstGeom(Body);

+            while (g != IntPtr.Zero)

+            {

+                thispos = d.GeomGetOffsetPosition(g);

+                thispos.X -= objdmass.c.X;

+                thispos.Y -= objdmass.c.Y;

+                thispos.Z -= objdmass.c.Z;

+                d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);

+                g = d.dBodyGetNextGeom(g);

+            }

+            d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos);

+

+            d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);

+            d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body

+            d.BodySetMass(Body, ref objdmass);

+            _mass = objdmass.mass;

+        }

+

+

+

+        private void FixInertia(Vector3 NewPos)

+        {

+            d.Matrix3 primmat = new d.Matrix3();

+            d.Mass tmpdmass = new d.Mass { };

+            d.Mass objdmass = new d.Mass { };

+            d.Mass primmass = new d.Mass { };

+

+            d.Vector3 dobjpos;

+            d.Vector3 thispos;

+

+            d.BodyGetMass(Body, out objdmass);

+

+            // get prim own inertia in its local frame

+            primmass = primdMass;

+            // transform to object frame

+            primmat = d.GeomGetOffsetRotation(prim_geom);

+            d.MassRotate(ref primmass, ref primmat);

+

+            tmpdmass = primmass;

+

+            thispos = d.GeomGetOffsetPosition(prim_geom);

+            d.MassTranslate(ref tmpdmass,

+                            thispos.X,

+                            thispos.Y,

+                            thispos.Z);

+

+            // subtract current prim inertia from object

+            DMassSubPartFromObj(ref tmpdmass, ref objdmass);

+

+            // update to new position

+            _position = NewPos;

+            d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);

+

+            thispos = d.GeomGetOffsetPosition(prim_geom);

+            d.MassTranslate(ref primmass,

+                            thispos.X,

+                            thispos.Y,

+                            thispos.Z);

+

+            d.MassAdd(ref objdmass, ref primmass);

+

+            // fix all positions

+            IntPtr g = d.BodyGetFirstGeom(Body);

+            while (g != IntPtr.Zero)

+            {

+                thispos = d.GeomGetOffsetPosition(g);

+                thispos.X -= objdmass.c.X;

+                thispos.Y -= objdmass.c.Y;

+                thispos.Z -= objdmass.c.Z;

+                d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);

+                g = d.dBodyGetNextGeom(g);

+            }

+

+            d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);

+

+            // get current object position and rotation

+            dobjpos = d.BodyGetPosition(Body);

+

+            d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);

+            d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body

+            d.BodySetMass(Body, ref objdmass);

+            _mass = objdmass.mass;

+        }

+

+        private void FixInertia(Quaternion newrot)

+        {

+            d.Matrix3 mat = new d.Matrix3();

+            d.Quaternion quat = new d.Quaternion();

+

+            d.Mass tmpdmass = new d.Mass { };

+            d.Mass objdmass = new d.Mass { };

+            d.Vector3 dobjpos;

+            d.Vector3 thispos;

+

+            d.BodyGetMass(Body, out objdmass);

+

+            // get prim own inertia in its local frame

+            tmpdmass = primdMass;

+            mat = d.GeomGetOffsetRotation(prim_geom);

+            d.MassRotate(ref tmpdmass, ref mat);

+            // transform to object frame

+            thispos = d.GeomGetOffsetPosition(prim_geom);

+            d.MassTranslate(ref tmpdmass,

+                            thispos.X,

+                            thispos.Y,

+                            thispos.Z);

+

+            // subtract current prim inertia from object

+            DMassSubPartFromObj(ref tmpdmass, ref objdmass);

+

+            // update to new orientation

+            _orientation = newrot;

+            quat.X = newrot.X;

+            quat.Y = newrot.Y;

+            quat.Z = newrot.Z;

+            quat.W = newrot.W;

+            d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);

+

+            tmpdmass = primdMass;

+            mat = d.GeomGetOffsetRotation(prim_geom);

+            d.MassRotate(ref tmpdmass, ref mat);

+            d.MassTranslate(ref tmpdmass,

+                            thispos.X,

+                            thispos.Y,

+                            thispos.Z);

+

+            d.MassAdd(ref objdmass, ref tmpdmass);

+

+            // fix all positions

+            IntPtr g = d.BodyGetFirstGeom(Body);

+            while (g != IntPtr.Zero)

+            {

+                thispos = d.GeomGetOffsetPosition(g);

+                thispos.X -= objdmass.c.X;

+                thispos.Y -= objdmass.c.Y;

+                thispos.Z -= objdmass.c.Z;

+                d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);

+                g = d.dBodyGetNextGeom(g);

+            }

+

+            d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);

+            // get current object position and rotation

+            dobjpos = d.BodyGetPosition(Body);

+

+            d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);

+            d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body

+            d.BodySetMass(Body, ref objdmass);

+            _mass = objdmass.mass;

+        }

+

+

         #region Mass Calculation

 

         private float CalculatePrimVolume()

@@ -2126,17 +2386,18 @@ namespace OpenSim.Region.Physics.OdePlugin
         {

             if (prim_geom != IntPtr.Zero)

             {

-                d.Vector3 lpos;

-                d.GeomCopyPosition(prim_geom, out lpos);

-                _position.X = lpos.X;

-                _position.Y = lpos.Y;

-                _position.Z = lpos.Z;

                 d.Quaternion qtmp = new d.Quaternion { };

                 d.GeomCopyQuaternion(prim_geom, out qtmp);

                 _orientation.W = qtmp.W;

                 _orientation.X = qtmp.X;

                 _orientation.Y = qtmp.Y;

                 _orientation.Z = qtmp.Z;

+

+                d.Vector3 lpos;

+                d.GeomCopyPosition(prim_geom, out lpos);

+                _position.X = lpos.X;

+                _position.Y = lpos.Y;

+                _position.Z = lpos.Z;

             }

         }

 

@@ -2593,6 +2854,13 @@ namespace OpenSim.Region.Physics.OdePlugin
                     {

                         _position = newPos;

                     }

+

+                    else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero)

+                    {

+                        FixInertia(newPos);

+                        if (!d.BodyIsEnabled(Body))

+                            d.BodyEnable(Body);

+                    }

                 }

                 else

                 {

@@ -2637,6 +2905,14 @@ namespace OpenSim.Region.Physics.OdePlugin
                     {

                         _orientation = newOri;

                     }

+                    /*

+                                        else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero)

+                                        {

+                                            FixInertia(_position, newOri);

+                                            if (!d.BodyIsEnabled(Body))

+                                                d.BodyEnable(Body);

+                                        }

+                    */

                 }

                 else

                 {

@@ -3512,6 +3788,29 @@ namespace OpenSim.Region.Physics.OdePlugin
             dst.I.M22 = src.I.M22;

         }

 

+        internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj)

+        {

+            // assumes object center of mass is zero

+            float smass = part.mass;

+            theobj.mass -= smass;

+

+            smass *= 1.0f / (theobj.mass); ;

+

+            theobj.c.X -= part.c.X * smass;

+            theobj.c.Y -= part.c.Y * smass;

+            theobj.c.Z -= part.c.Z * smass;

+

+            theobj.I.M00 -= part.I.M00;

+            theobj.I.M01 -= part.I.M01;

+            theobj.I.M02 -= part.I.M02;

+            theobj.I.M10 -= part.I.M10;

+            theobj.I.M11 -= part.I.M11;

+            theobj.I.M12 -= part.I.M12;

+            theobj.I.M20 -= part.I.M20;

+            theobj.I.M21 -= part.I.M21;

+            theobj.I.M22 -= part.I.M22;

+        }

+

         private static void DMassDup(ref d.Mass src, out d.Mass dst)

         {

             dst = new d.Mass { };

diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
index 76d7746..1a6907d 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -160,8 +160,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         private Random fluidRandomizer = new Random(Environment.TickCount);
 
         const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
-        const float comumContactERP = 0.6f;
-        const float comumSoftContactERP = 0.1f;
+        const float MaxERP = 0.8f;
+        const float minERP = 0.1f;
         const float comumContactCFM = 0.0001f;
         
         float frictionMovementMult = 0.3f;
@@ -169,7 +169,6 @@ namespace OpenSim.Region.Physics.OdePlugin
         float TerrainBounce = 0.1f;
         float TerrainFriction = 0.3f;
 
-        public float AvatarBounce = 0.3f;
         public float AvatarFriction = 0;// 0.9f * 0.5f;
 
         private const uint m_regionWidth = Constants.RegionSize;
@@ -528,7 +527,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         // sets a global contact for a joint for contactgeom , and base contact description)
 
-        private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, bool softerp)
+        private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce,float cfm,float erp)
         {
             if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath)
                 return IntPtr.Zero;
@@ -546,11 +545,8 @@ namespace OpenSim.Region.Physics.OdePlugin
             newcontact.surface.mode = comumContactFlags;
             newcontact.surface.mu = mu;
             newcontact.surface.bounce = bounce;
-            newcontact.surface.soft_cfm = comumContactCFM;
-            if (softerp)
-                newcontact.surface.soft_erp = comumSoftContactERP;
-            else
-                newcontact.surface.soft_erp = comumContactERP;
+            newcontact.surface.soft_cfm = cfm;
+            newcontact.surface.soft_erp = erp;
 
             IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
             Marshal.StructureToPtr(newcontact, contact, true);
@@ -694,9 +690,11 @@ namespace OpenSim.Region.Physics.OdePlugin
             // big messy collision analises
             float mu = 0;
             float bounce = 0;
+            float cfm = 0.0001f;
+            float erp = 0.1f;
+
             ContactData contactdata1 = new ContactData(0, 0, false);
             ContactData contactdata2 = new ContactData(0, 0, false);
-            bool erpSoft = false;
 
             String name = null;
             bool dop1foot = false;
@@ -706,59 +704,65 @@ namespace OpenSim.Region.Physics.OdePlugin
             switch (p1.PhysicsActorType)
             {
                 case (int)ActorTypes.Agent:
-                    switch (p2.PhysicsActorType)
                     {
-                        case (int)ActorTypes.Agent:
-                            p1.getContactData(ref contactdata1);
-                            p2.getContactData(ref contactdata2);
-                            
-                            bounce = contactdata1.bounce * contactdata2.bounce;
-                            
-                            mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
+                        bounce = 0;
+                        mu = 0;
+                        cfm = 0.0001f;
 
-                            if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
-                                mu *= frictionMovementMult;
+                        switch (p2.PhysicsActorType)
+                        {
+                            case (int)ActorTypes.Agent:
+/*
+                                p1.getContactData(ref contactdata1);
+                                p2.getContactData(ref contactdata2);
 
-                            erpSoft = contactdata1.softcolide | contactdata2.softcolide;
-                            p1.CollidingObj = true;
-                            p2.CollidingObj = true;
-                            break;
-                        case (int)ActorTypes.Prim:
-                            p1.getContactData(ref contactdata1);
-                            p2.getContactData(ref contactdata2);
-                            bounce = contactdata1.bounce * contactdata2.bounce;
-                            
-                            mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
+                                mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
 
-                            if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
-                                mu *= frictionMovementMult;
-                            if (p2.Velocity.LengthSquared() > 0.0f)
+                                if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
+                                    mu *= frictionMovementMult;
+*/
+                                p1.CollidingObj = true;
                                 p2.CollidingObj = true;
+                                break;
+                            case (int)ActorTypes.Prim:
+/*
+                                p1.getContactData(ref contactdata1);
+                                p2.getContactData(ref contactdata2);
 
-                            erpSoft = contactdata1.softcolide | contactdata2.softcolide;
 
-                            dop1foot = true;
-                            break;
-                        default:
-                            ignore=true; // avatar to terrain and water ignored
-                            break;
+                                mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
+
+                                if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
+                                    mu *= frictionMovementMult;
+ */
+                                if (p2.Velocity.LengthSquared() > 0.0f)
+                                    p2.CollidingObj = true;
+
+                                dop1foot = true;
+                                break;
+                            default:
+                                ignore = true; // avatar to terrain and water ignored
+                                break;
+                        }
+                        break;
                     }
-                    break;
 
                 case (int)ActorTypes.Prim:
                     switch (p2.PhysicsActorType)
                     {
                         case (int)ActorTypes.Agent:
-                            p1.getContactData(ref contactdata1);
-                            p2.getContactData(ref contactdata2);
-                            bounce = contactdata1.bounce * contactdata2.bounce;
-                            
+//                            p1.getContactData(ref contactdata1);
+//                            p2.getContactData(ref contactdata2);
+
+                            bounce = 0;
+                            mu = 0;
+                            cfm = 0.0001f;
+/*
                             mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
 
                             if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
                                 mu *= frictionMovementMult;
-
-                            erpSoft = contactdata1.softcolide | contactdata2.softcolide;
+*/
                             dop2foot = true;
                             if (p1.Velocity.LengthSquared() > 0.0f)
                                 p1.CollidingObj = true;
@@ -772,9 +776,16 @@ namespace OpenSim.Region.Physics.OdePlugin
                             p1.getContactData(ref contactdata1);
                             p2.getContactData(ref contactdata2);
                             bounce = contactdata1.bounce * contactdata2.bounce;
-                            erpSoft = contactdata1.softcolide | contactdata2.softcolide;
                             mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
 
+                            cfm = p1.Mass;
+                            if (cfm > p2.Mass)
+                                cfm = p2.Mass;
+                            cfm = (float)Math.Sqrt(cfm);
+                            cfm *= 0.0001f;
+                            if (cfm > 0.8f)
+                                cfm = 0.8f;
+
                             if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
                                 mu *= frictionMovementMult;
 
@@ -789,12 +800,17 @@ namespace OpenSim.Region.Physics.OdePlugin
                                     mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
                                     if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f)
                                         mu *= frictionMovementMult;
-                                    erpSoft = contactdata1.softcolide;
                                     p1.CollidingGround = true;
+                                    cfm = p1.Mass;
+                                    cfm = (float)Math.Sqrt(cfm);
+                                    cfm *= 0.0001f;
+                                    if (cfm > 0.8f)
+                                        cfm = 0.8f;
+
                                 }
                                 else if (name == "Water")
                                 {
-                                    erpSoft = true;
+                                    ignore = true;
                                 }
                             }
                             else
@@ -814,7 +830,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 p2.getContactData(ref contactdata2);
                                 bounce = contactdata2.bounce * TerrainBounce;
                                 mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
-                                erpSoft = contactdata2.softcolide;
+                                cfm = p2.Mass;
+                                cfm = (float)Math.Sqrt(cfm);
+                                cfm *= 0.0001f;
+                                if (cfm > 0.8f)
+                                    cfm = 0.8f;
 
                                 if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f)
                                     mu *= frictionMovementMult;
@@ -826,7 +846,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         else if (name == "Water" &&
                             (p2.PhysicsActorType == (int)ActorTypes.Prim || p2.PhysicsActorType == (int)ActorTypes.Agent))
                         {
-                            erpSoft = true;
+                            ignore = true;
                         }
                     }
                     else
@@ -847,7 +867,14 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f)
                     p2.IsColliding = true;
 
-                Joint = CreateContacJoint(ref curContact, mu, bounce, erpSoft);
+
+                erp = curContact.depth;
+                if (erp < minERP)
+                    erp = minERP;
+                else if (erp > MaxERP)
+                    erp = MaxERP;
+
+                Joint = CreateContacJoint(ref curContact, mu, bounce,cfm,erp);
                 d.JointAttach(Joint, b1, b2);
 
                 if (++m_global_contactcount >= maxContactsbeforedeath)