Merge branch 'ubitwork'

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);
                 }
             }

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;
+                                pa.IsPhysical = UsePhysics;
-                                PhysActor.OnRequestTerseUpdate -= PhysicsRequestingTerseUpdate;
+                                pa.OnRequestTerseUpdate -= PhysicsRequestingTerseUpdate;
-                                PhysActor.OnOutOfBounds -= PhysicsOutOfBounds;
+                                pa.OnOutOfBounds -= PhysicsOutOfBounds;
-                                PhysActor.delink();
+                                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)
+                        if (pa.IsPhysical != UsePhysics)
-                            PhysActor.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;
+                                if (parentPa != null)
-                                pa.OnOutOfBounds += PhysicsOutOfBounds;
-                                if (ParentID != 0 && ParentID != LocalId)
                                 {
-                                    PhysicsActor parentPa = ParentGroup.RootPart.PhysActor;
+                                    pa.link(parentPa);
-
-                                    if (parentPa != null)
-                                    {
-                                        pa.link(parentPa);
-                                    }
                                 }
                             }
                         }                           
                     }
 
                     bool phan = ((Flags & PrimFlags.Phantom) != 0);
-                    if (PhysActor.Phantom != phan)
+                    if (pa.Phantom != phan)
-                        PhysActor.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 (pa != null)
-            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)
-                )
             {
-                // subscribe to physics updates.
+                const scriptEvents NeededSubsEvents = (
-                if (pa != null)
+                            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
-            else
-            {
-                if (pa != null)
                 {
                     pa.UnSubscribeEvents();
                     pa.OnCollisionUpdate -= PhysicsCollision;
                 }
             }
-
             //if ((GetEffectiveObjectFlags() & (uint)PrimFlags.Scripted) != 0)
             //{
             //    ParentGroup.Scene.EventManager.OnScriptTimerEvent += handleTimerAccounting;

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;
         }
 

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;

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);
             }
         }
@@ -499,6 +520,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+
         public override Vector3 Velocity
         {
             get
@@ -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,39 +1104,51 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
             }
 
-            bool convex;
+            IMesh mesh = null;
-            if (m_shapetype == 0)
-                convex = false;
-            else
-                convex = true;
 
-            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);
+                if (m_mesh == null)
-                return false;
+                {
-            }
+                    bool convex;
+                    if (m_shapetype == 0)
+                        convex = false;
+                    else
+                        convex = true;
 
-            IntPtr vertices, indices;
+                    mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);
-            int vertexCount, indexCount;
+                }
-            int vertexStride, triStride;
+                else
+                {
+                    mesh = m_mesh;
+                }
 
-            mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
+                if (mesh == null)
-            mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
+                {
+                    m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z);
+                    return false;
+                }
+
+
+                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 (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;
 
-            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;
+                m_mesh = null;
             }
 
-            primOOBoffset = mesh.GetCentroid();
-            hasOOBoffsetFromMesh = true;
-
-            mesh.releaseSourceMeshData();
-
             IntPtr geo = IntPtr.Zero;
 
             try
@@ -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 { };

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 MaxERP = 0.8f;
-        const float comumSoftContactERP = 0.1f;
+        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;
+            newcontact.surface.soft_cfm = cfm;
-            if (softerp)
+            newcontact.surface.soft_erp = erp;
-                newcontact.surface.soft_erp = comumSoftContactERP;
-            else
-                newcontact.surface.soft_erp = comumContactERP;
 
             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:
+                        bounce = 0;
-                            p1.getContactData(ref contactdata1);
+                        mu = 0;
-                            p2.getContactData(ref contactdata2);
+                        cfm = 0.0001f;
 
-                            bounce = contactdata1.bounce * contactdata2.bounce;
+                        switch (p2.PhysicsActorType)
+                        {
+                            case (int)ActorTypes.Agent:
+/*
+                                p1.getContactData(ref contactdata1);
+                                p2.getContactData(ref contactdata2);
 
-                            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))
+                                if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
-                                mu *= frictionMovementMult;
+                                    mu *= frictionMovementMult;
-
+*/
-                            erpSoft = contactdata1.softcolide | contactdata2.softcolide;
+                                p1.CollidingObj = true;
-                            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);
-
-                            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;
+                                break;
+                            case (int)ActorTypes.Prim:
+/*
+                                p1.getContactData(ref contactdata1);
+                                p2.getContactData(ref contactdata2);
 
-                            erpSoft = contactdata1.softcolide | contactdata2.softcolide;
 
-                            dop1foot = true;
+                                mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
-                            break;
+
-                        default:
+                                if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
-                            ignore=true; // avatar to terrain and water ignored
+                                    mu *= frictionMovementMult;
-                            break;
+ */
+                                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);
+//                            p1.getContactData(ref contactdata1);
-                            p2.getContactData(ref contactdata2);
+//                            p2.getContactData(ref contactdata2);
-                            bounce = contactdata1.bounce * contactdata2.bounce;
 
+                            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)