From 6a35a965ff7085b5962745437a10d798c0fb611d Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 31 Mar 2017 20:55:48 +0100 Subject: add OSSL functions to override linksets total mass, center of mass and inertia. replacing the crude automatic estimation based on prims known to physics and density. Changed parameters are still not saved, and are lost on region crossings. only suported by UbODE. EXPERIMENTAL feature, only test it for now.. don't try to use in products. --- OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs | 253 ++++++++++++++++++++----- 1 file changed, 207 insertions(+), 46 deletions(-) (limited to 'OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs') diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs index 45ef273..d560b41 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs @@ -182,14 +182,16 @@ namespace OpenSim.Region.PhysicsModule.ubOde private float m_streamCost; public d.Mass primdMass; // prim inertia information on it's own referencial + private PhysicsInertiaData m_InertiaOverride; float primMass; // prim own mass float primVolume; // prim own volume; - float _mass; // object mass acording to case + float m_mass; // object mass acording to case public int givefakepos; private Vector3 fakepos; public int givefakeori; private Quaternion fakeori; + private PhysicsInertiaData m_fakeInertiaOverride; private int m_eventsubscription; private int m_cureventsubscription; @@ -465,10 +467,110 @@ namespace OpenSim.Region.PhysicsModule.ubOde } } + public override PhysicsInertiaData GetInertiaData() + { + PhysicsInertiaData inertia; + if(childPrim) + { + if(_parent != null) + return _parent.GetInertiaData(); + else + { + inertia = new PhysicsInertiaData(); + inertia.TotalMass = -1; + return inertia; + } + } + + inertia = new PhysicsInertiaData(); + + // double buffering + if(m_fakeInertiaOverride != null) + { + d.Mass objdmass = new d.Mass(); + objdmass.I.M00 = m_fakeInertiaOverride.Inertia.X; + objdmass.I.M11 = m_fakeInertiaOverride.Inertia.Y; + objdmass.I.M22 = m_fakeInertiaOverride.Inertia.Z; + + objdmass.mass = m_fakeInertiaOverride.TotalMass; + + if(Math.Abs(m_fakeInertiaOverride.InertiaRotation.W) < 0.999) + { + d.Matrix3 inertiarotmat = new d.Matrix3(); + d.Quaternion inertiarot = new d.Quaternion(); + + inertiarot.X = m_fakeInertiaOverride.InertiaRotation.X; + inertiarot.Y = m_fakeInertiaOverride.InertiaRotation.Y; + inertiarot.Z = m_fakeInertiaOverride.InertiaRotation.Z; + inertiarot.W = m_fakeInertiaOverride.InertiaRotation.W; + d.RfromQ(out inertiarotmat, ref inertiarot); + d.MassRotate(ref objdmass, ref inertiarotmat); + } + + inertia.TotalMass = m_fakeInertiaOverride.TotalMass; + inertia.CenterOfMass = m_fakeInertiaOverride.CenterOfMass; + inertia.Inertia.X = objdmass.I.M00; + inertia.Inertia.Y = objdmass.I.M11; + inertia.Inertia.Z = objdmass.I.M22; + inertia.InertiaRotation.X = objdmass.I.M01; + inertia.InertiaRotation.Y = objdmass.I.M02; + inertia.InertiaRotation.Z = objdmass.I.M12; + return inertia; + } + + inertia.TotalMass = m_mass; + + if(Body == IntPtr.Zero || prim_geom == IntPtr.Zero) + { + inertia.CenterOfMass = Vector3.Zero; + inertia.Inertia = Vector3.Zero; + inertia.InertiaRotation = Vector4.Zero; + return inertia; + } + + d.Vector3 dtmp; + d.Mass m = new d.Mass(); + lock(_parent_scene.OdeLock) + { + d.AllocateODEDataForThread(0); + dtmp = d.GeomGetOffsetPosition(prim_geom); + d.BodyGetMass(Body, out m); + } + + Vector3 cm = new Vector3(-dtmp.X, -dtmp.Y, -dtmp.Z); + inertia.CenterOfMass = cm; + inertia.Inertia = new Vector3(m.I.M00, m.I.M11, m.I.M22); + inertia.InertiaRotation = new Vector4(m.I.M01, m.I.M02 , m.I.M12, 0); + + return inertia; + } + + public override void SetInertiaData(PhysicsInertiaData inertia) + { + if(childPrim) + { + if(_parent != null) + _parent.SetInertiaData(inertia); + return; + } + + if(inertia.TotalMass > 0) + m_fakeInertiaOverride = new PhysicsInertiaData(inertia); + else + m_fakeInertiaOverride = null; + + if (inertia.TotalMass > _parent_scene.maximumMassObject) + inertia.TotalMass = _parent_scene.maximumMassObject; + AddChange(changes.SetInertia,(object)m_fakeInertiaOverride); + } + public override Vector3 CenterOfMass { get { + if(!childPrim && m_fakeInertiaOverride != null) + return m_fakeInertiaOverride.CenterOfMass; + lock (_parent_scene.OdeLock) { d.AllocateODEDataForThread(0); @@ -922,8 +1024,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde { float old = m_density; m_density = value / 100f; - if(m_density != old) - UpdatePrimBodyData(); + // if(m_density != old) + // UpdatePrimBodyData(); } } public override float GravModifier @@ -1716,26 +1818,29 @@ namespace OpenSim.Region.PhysicsModule.ubOde m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); } - d.Matrix3 mymat = new d.Matrix3(); - d.Quaternion myrot = new d.Quaternion(); - d.Mass objdmass = new d.Mass { }; + bool noInertiaOverride = (m_InertiaOverride == null); Body = d.BodyCreate(_parent_scene.world); - objdmass = primdMass; + d.Matrix3 mymat = new d.Matrix3(); + d.Quaternion myrot = new d.Quaternion(); + d.Mass objdmass = new d.Mass { }; - // rotate inertia myrot.X = _orientation.X; myrot.Y = _orientation.Y; myrot.Z = _orientation.Z; myrot.W = _orientation.W; - d.RfromQ(out mymat, ref myrot); - d.MassRotate(ref objdmass, ref mymat); // set the body rotation d.BodySetRotation(Body, ref mymat); + if(noInertiaOverride) + { + objdmass = primdMass; + d.MassRotate(ref objdmass, ref mymat); + } + // recompute full object inertia if needed if (childrenPrim.Count > 0) { @@ -1758,27 +1863,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde continue; } - tmpdmass = prm.primdMass; - - // apply prim current rotation to inertia quat.X = prm._orientation.X; quat.Y = prm._orientation.Y; quat.Z = prm._orientation.Z; quat.W = prm._orientation.W; d.RfromQ(out mat, ref quat); - d.MassRotate(ref tmpdmass, ref mat); - - Vector3 ppos = prm._position; - 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, - ppos.Y, - ppos.Z); - - d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia + // fix prim colision cats if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) @@ -1791,6 +1881,24 @@ namespace OpenSim.Region.PhysicsModule.ubOde d.GeomSetBody(prm.prim_geom, Body); prm.Body = Body; d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation + + if(noInertiaOverride) + { + tmpdmass = prm.primdMass; + + d.MassRotate(ref tmpdmass, ref mat); + Vector3 ppos = prm._position; + 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, + ppos.Y, + ppos.Z); + + d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia + } } } } @@ -1799,19 +1907,60 @@ namespace OpenSim.Region.PhysicsModule.ubOde // associate root geom with body d.GeomSetBody(prim_geom, Body); - d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); + if(noInertiaOverride) + d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); + else + { + Vector3 ncm = m_InertiaOverride.CenterOfMass * _orientation; + d.BodySetPosition(Body, + _position.X + ncm.X, + _position.Y + ncm.Y, + _position.Z + ncm.Z); + } + 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.X = -myrot.X; - myrot.Y = -myrot.Y; - myrot.Z = -myrot.Z; + if(noInertiaOverride) + { + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + 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.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); - d.BodySetMass(Body, ref objdmass); - _mass = objdmass.mass; + d.BodySetMass(Body, ref objdmass); + m_mass = objdmass.mass; + } + else + { + objdmass.c.X = 0; + objdmass.c.Y = 0; + objdmass.c.Z = 0; + + objdmass.I.M00 = m_InertiaOverride.Inertia.X; + objdmass.I.M11 = m_InertiaOverride.Inertia.Y; + objdmass.I.M22 = m_InertiaOverride.Inertia.Z; + + objdmass.mass = m_InertiaOverride.TotalMass; + + if(Math.Abs(m_InertiaOverride.InertiaRotation.W) < 0.999) + { + d.Matrix3 inertiarotmat = new d.Matrix3(); + d.Quaternion inertiarot = new d.Quaternion(); + + inertiarot.X = m_InertiaOverride.InertiaRotation.X; + inertiarot.Y = m_InertiaOverride.InertiaRotation.Y; + inertiarot.Z = m_InertiaOverride.InertiaRotation.Z; + inertiarot.W = m_InertiaOverride.InertiaRotation.W; + d.RfromQ(out inertiarotmat, ref inertiarot); + d.MassRotate(ref objdmass, ref inertiarotmat); + } + d.BodySetMass(Body, ref objdmass); + + m_mass = objdmass.mass; + } // disconnect from world gravity so we can apply buoyancy d.BodySetGravityMode(Body, false); @@ -1990,7 +2139,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde SetInStaticSpace(prm); } prm.Body = IntPtr.Zero; - prm._mass = prm.primMass; + prm.m_mass = prm.primMass; prm.m_collisionscore = 0; } } @@ -2004,7 +2153,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde } Body = IntPtr.Zero; } - _mass = primMass; + m_mass = primMass; m_collisionscore = 0; } @@ -2081,7 +2230,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde 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; + m_mass = objdmass.mass; } private void FixInertia(Vector3 NewPos) @@ -2145,7 +2294,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde 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; + m_mass = objdmass.mass; } private void FixInertia(Quaternion newrot) @@ -2211,7 +2360,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde 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; + m_mass = objdmass.mass; } @@ -2226,7 +2375,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde if (primMass > _parent_scene.maximumMassObject) primMass = _parent_scene.maximumMassObject; - _mass = primMass; // just in case + m_mass = primMass; // just in case d.MassSetBoxTotal(out primdMass, primMass, 2.0f * m_OBB.X, 2.0f * m_OBB.Y, 2.0f * m_OBB.Z); @@ -3306,6 +3455,15 @@ namespace OpenSim.Region.PhysicsModule.ubOde m_useHoverPID = active; } + private void changeInertia(PhysicsInertiaData inertia) + { + m_InertiaOverride = inertia; + + if (Body != IntPtr.Zero) + DestroyBody(); + MakeBody(); + } + #endregion public void Move() @@ -3346,7 +3504,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde float fy = 0; float fz = 0; - float m_mass = _mass; + float mass = m_mass; if (m_usePID && m_PIDTau > 0) { @@ -3453,9 +3611,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde fz = _parent_scene.gravityz * b; } - fx *= m_mass; - fy *= m_mass; - fz *= m_mass; + fx *= mass; + fy *= mass; + fz *= mass; // constant force fx += m_force.X; @@ -3941,6 +4099,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde changePIDHoverActive((bool)arg); break; + case changes.SetInertia: + changeInertia((PhysicsInertiaData) arg); + break; + case changes.Null: donullchange(); break; @@ -3957,7 +4119,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde _parent_scene.AddChange((PhysicsActor) this, what, arg); } - private struct strVehicleBoolParam { public int param; -- cgit v1.1