From dd745f60c201aee7ff48ba81d567e7b38a8f186c Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 28 Apr 2012 21:36:38 +0100 Subject: fix llGetCenterOfMass ( checked with ubitODE only) --- .../Region/Framework/Scenes/SceneObjectGroup.cs | 33 ++++++++++++++++++ OpenSim/Region/Framework/Scenes/SceneObjectPart.cs | 40 ++++++++++++++++++++-- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 35 ++++++++++++++++--- .../Shared/Api/Implementation/LSL_Api.cs | 2 +- 4 files changed, 101 insertions(+), 9 deletions(-) diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs index ba353c4..b0f2015 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs @@ -4041,6 +4041,39 @@ namespace OpenSim.Region.Framework.Scenes return retmass; } + // center of mass of full object + public Vector3 GetCenterOfMass() + { + PhysicsActor pa = RootPart.PhysActor; + + if(((RootPart.Flags & PrimFlags.Physics) !=0) && pa !=null) + { + // physics knows better about center of mass of physical prims + Vector3 tmp = pa.CenterOfMass; + return tmp; + } + + Vector3 Ptot = Vector3.Zero; + float totmass = 0f; + float m; + + SceneObjectPart[] parts = m_parts.GetArray(); + for (int i = 0; i < parts.Length; i++) + { + m = parts[i].GetMass(); + Ptot += parts[i].GetPartCenterOfMass() * m; + totmass += m; + } + + if (totmass == 0) + totmass = 0; + else + totmass = 1 / totmass; + Ptot *= totmass; + + return Ptot; + } + /// /// If the object is a sculpt/mesh, retrieve the mesh data for each part and reinsert it into each shape so that /// the physics engine can use it. diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs index c9659cb..b198ca7 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs @@ -1865,7 +1865,7 @@ namespace OpenSim.Region.Framework.Scenes { if ((!isPhantom || isPhysical || _VolumeDetectActive) && !ParentGroup.IsAttachment && !(Shape.PathCurve == (byte)Extrusion.Flexible)) - AddToPhysics(isPhysical, isPhantom, building, true); + AddToPhysics(isPhysical, isPhantom, building, isPhysical); else PhysActor = null; // just to be sure } @@ -2308,7 +2308,7 @@ namespace OpenSim.Region.Framework.Scenes */ } - public float GetMass() + public float GetMass() { PhysicsActor pa = PhysActor; @@ -2318,6 +2318,40 @@ namespace OpenSim.Region.Framework.Scenes return 0; } + public Vector3 GetCenterOfMass() + { + if (ParentGroup.RootPart == this) + { + if (ParentGroup.IsDeleted) + return AbsolutePosition; + return ParentGroup.GetCenterOfMass(); + } + + PhysicsActor pa = PhysActor; + + if (pa != null) + { + Vector3 tmp = pa.CenterOfMass; + return tmp; + } + else + return AbsolutePosition; + } + + public Vector3 GetPartCenterOfMass() + { + PhysicsActor pa = PhysActor; + + if (pa != null) + { + Vector3 tmp = pa.CenterOfMass; + return tmp; + } + else + return AbsolutePosition; + } + + public Vector3 GetForce() { return Force; @@ -4802,7 +4836,7 @@ namespace OpenSim.Region.Framework.Scenes { Velocity = velocity; AngularVelocity = rotationalVelocity; - pa.Velocity = velocity; +// pa.Velocity = velocity; pa.RotationalVelocity = rotationalVelocity; // if not vehicle and root part apply force and torque diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 2d587ab..c9a453d 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -450,7 +450,7 @@ namespace OpenSim.Region.Physics.OdePlugin get { d.Vector3 dtmp; - if (IsPhysical && !childPrim && Body != IntPtr.Zero) + if (!childPrim && Body != IntPtr.Zero) { dtmp = d.BodyGetPosition(Body); return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); @@ -465,12 +465,38 @@ namespace OpenSim.Region.Physics.OdePlugin q.Z = dq.Z; q.W = dq.W; - Vector3 vtmp = primOOBoffset * q; + Vector3 Ptot = primOOBoffset * q; dtmp = d.GeomGetPosition(prim_geom); - return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z); + 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 - return Vector3.Zero; + return _position; } } /* @@ -3490,7 +3516,6 @@ namespace OpenSim.Region.Physics.OdePlugin { d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight); d.BodySetLinearVel(Body, vel.X, vel.Y, 0); - return; } else { diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs index 8e73eb1..6ee1a5d 100644 --- a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs +++ b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs @@ -5370,7 +5370,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api public LSL_Vector llGetCenterOfMass() { m_host.AddScriptLPS(1); - Vector3 center = m_host.GetGeometricCenter(); + Vector3 center = m_host.GetCenterOfMass(); return new LSL_Vector(center.X,center.Y,center.Z); } -- cgit v1.1