From be176b1e4948059e041d06be2caea1eb10b5ee68 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 29 Apr 2012 08:24:41 +0100 Subject: [PATCH] ubitode fix inertia for same cases. Added a nasty lock on llGetCenterOfMass and simulate --- .../Region/Physics/UbitOdePlugin/ODEPrim.cs | 176 +++++++++--------- .../Region/Physics/UbitOdePlugin/OdeScene.cs | 10 +- 2 files changed, 90 insertions(+), 96 deletions(-) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index c9a453dd22..2bcce3168f 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -449,54 +449,57 @@ namespace OpenSim.Region.Physics.OdePlugin { get { - d.Vector3 dtmp; - if (!childPrim && Body != IntPtr.Zero) + lock (_parent_scene.OdeLock) { - dtmp = d.BodyGetPosition(Body); - return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); - } - else if (prim_geom != IntPtr.Zero) - { - d.Quaternion dq; - d.GeomCopyQuaternion(prim_geom, out dq); - Quaternion q; - q.X = dq.X; - q.Y = dq.Y; - q.Z = dq.Z; - q.W = dq.W; - - Vector3 Ptot = primOOBoffset * q; - dtmp = d.GeomGetPosition(prim_geom); - Ptot.X += dtmp.X; - Ptot.Y += dtmp.Y; - Ptot.Z += dtmp.Z; - -// if(childPrim) we only know about physical linksets - return Ptot; -/* - float tmass = _mass; - Ptot *= tmass; - - float m; - - foreach (OdePrim prm in childrenPrim) + d.Vector3 dtmp; + if (!childPrim && Body != IntPtr.Zero) { - m = prm._mass; - Ptot += prm.CenterOfMass * m; - tmass += m; + dtmp = d.BodyGetPosition(Body); + return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); } + else if (prim_geom != IntPtr.Zero) + { + d.Quaternion dq; + d.GeomCopyQuaternion(prim_geom, out dq); + Quaternion q; + q.X = dq.X; + q.Y = dq.Y; + q.Z = dq.Z; + q.W = dq.W; - if (tmass == 0) - tmass = 0; + Vector3 Ptot = primOOBoffset * q; + dtmp = d.GeomGetPosition(prim_geom); + Ptot.X += dtmp.X; + Ptot.Y += dtmp.Y; + Ptot.Z += dtmp.Z; + + // if(childPrim) we only know about physical linksets + return Ptot; + /* + float tmass = _mass; + Ptot *= tmass; + + float m; + + foreach (OdePrim prm in childrenPrim) + { + m = prm._mass; + Ptot += prm.CenterOfMass * m; + tmass += m; + } + + if (tmass == 0) + tmass = 0; + else + tmass = 1.0f / tmass; + + Ptot *= tmass; + return Ptot; + */ + } else - tmass = 1.0f / tmass; - - Ptot *= tmass; - return Ptot; - */ + return _position; } - else - return _position; } } /* @@ -1511,7 +1514,6 @@ namespace OpenSim.Region.Physics.OdePlugin } Body = IntPtr.Zero; hasOOBoffsetFromMesh = false; - CalcPrimBodyData(); } /* private void ChildSetGeom(OdePrim odePrim) @@ -1601,7 +1603,7 @@ namespace OpenSim.Region.Physics.OdePlugin Body = d.BodyCreate(_parent_scene.world); - DMassDup(ref primdMass, out objdmass); + objdmass = primdMass; // rotate inertia myrot.X = _orientation.X; @@ -1623,9 +1625,9 @@ namespace OpenSim.Region.Physics.OdePlugin d.Mass tmpdmass = new d.Mass { }; Vector3 rcm; - rcm.X = _position.X + objdmass.c.X; - rcm.Y = _position.Y + objdmass.c.Y; - rcm.Z = _position.Z + objdmass.c.Z; + rcm.X = _position.X; + rcm.Y = _position.Y; + rcm.Z = _position.Z; lock (childrenPrim) { @@ -1637,7 +1639,7 @@ namespace OpenSim.Region.Physics.OdePlugin continue; } - DMassCopy(ref prm.primdMass, ref tmpdmass); + tmpdmass = prm.primdMass; // apply prim current rotation to inertia quat.X = prm._orientation.X; @@ -1648,10 +1650,9 @@ namespace OpenSim.Region.Physics.OdePlugin d.MassRotate(ref tmpdmass, ref mat); Vector3 ppos = prm._position; - ppos.X += tmpdmass.c.X - rcm.X; - ppos.Y += tmpdmass.c.Y - rcm.Y; - ppos.Z += tmpdmass.c.Z - rcm.Z; - + ppos.X -= rcm.X; + ppos.Y -= rcm.Y; + ppos.Z -= rcm.Z; // refer inertia to root prim center of mass position d.MassTranslate(ref tmpdmass, ppos.X, @@ -1683,9 +1684,13 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body - myrot.W = -myrot.W; + myrot.X = -myrot.X; + myrot.Y = -myrot.Y; + myrot.Z = -myrot.Z; + d.RfromQ(out mymat, ref myrot); d.MassRotate(ref objdmass, ref mymat); + d.BodySetMass(Body, ref objdmass); _mass = objdmass.mass; @@ -2237,7 +2242,33 @@ namespace OpenSim.Region.Physics.OdePlugin case ProfileShape.HalfCircle: if (_pbs.PathCurve == (byte)Extrusion.Curve1) { - volume *= 0.52359877559829887307710723054658f; + volume *= 0.5236f; + + if (hollowAmount > 0.0) + { + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Circle: + case HollowShape.Triangle: // diference in sl is minor and odd + case HollowShape.Same: + break; + + case HollowShape.Square: + hollowVolume *= 0.909f; + break; + + // case HollowShape.Triangle: + // hollowVolume *= .827f; + // break; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } break; @@ -3704,24 +3735,6 @@ namespace OpenSim.Region.Physics.OdePlugin return true; } - internal static void DMassCopy(ref d.Mass src, ref d.Mass dst) - { - dst.c.W = src.c.W; - dst.c.X = src.c.X; - dst.c.Y = src.c.Y; - dst.c.Z = src.c.Z; - dst.mass = src.mass; - dst.I.M00 = src.I.M00; - dst.I.M01 = src.I.M01; - dst.I.M02 = src.I.M02; - dst.I.M10 = src.I.M10; - dst.I.M11 = src.I.M11; - dst.I.M12 = src.I.M12; - dst.I.M20 = src.I.M20; - dst.I.M21 = src.I.M21; - 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 @@ -3745,25 +3758,6 @@ namespace OpenSim.Region.Physics.OdePlugin theobj.I.M22 -= part.I.M22; } - private static void DMassDup(ref d.Mass src, out d.Mass dst) - { - dst = new d.Mass { }; - - dst.c.W = src.c.W; - dst.c.X = src.c.X; - dst.c.Y = src.c.Y; - dst.c.Z = src.c.Z; - dst.mass = src.mass; - dst.I.M00 = src.I.M00; - dst.I.M01 = src.I.M01; - dst.I.M02 = src.I.M02; - dst.I.M10 = src.I.M10; - dst.I.M11 = src.I.M11; - dst.I.M12 = src.I.M12; - dst.I.M20 = src.I.M20; - dst.I.M21 = src.I.M21; - dst.I.M22 = src.I.M22; - } private void donullchange() { } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index fa3d33e728..84195d34e6 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -301,7 +301,7 @@ namespace OpenSim.Region.Physics.OdePlugin // split static geometry collision into a grid as before private IntPtr[,] staticPrimspace; - private Object OdeLock; + public Object OdeLock; private static Object SimulationLock; public IMesher mesher; @@ -746,8 +746,7 @@ namespace OpenSim.Region.Physics.OdePlugin ); // do volume detection case if ( - (p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect) || - (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect)) + (p1.IsVolumeDtc || p2.IsVolumeDtc)) { collision_accounting_events(p1, p2, maxDepthContact); return; @@ -1024,9 +1023,9 @@ namespace OpenSim.Region.Physics.OdePlugin bool p1events = p1.SubscribedEvents(); bool p2events = p2.SubscribedEvents(); - if (p1 is OdePrim && p1.IsVolumeDtc) + if (p1.IsVolumeDtc) p2events = false; - if (p2 is OdePrim && p2.IsVolumeDtc) + if (p2.IsVolumeDtc) p1events = false; if (!(p2events || p1events)) @@ -1725,6 +1724,7 @@ namespace OpenSim.Region.Physics.OdePlugin // checkThread(); lock (SimulationLock) + lock(OdeLock) { // adjust number of iterations per step try