1 files changed, 332 insertions, 33 deletions
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;
+            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);
+            lock (m_meshlock)
-            if (mesh == null)
            {
-                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;
+                }
-            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();
+                mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
-            hasOOBoffsetFromMesh = true;
+                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/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
70	private bool m_fakeisphantom;	70	private bool m_fakeisphantom;
71		71
72	protected bool m_building;	72	protected bool m_building;
		73	protected bool m_forcePosOrRotation;
		74
73	private Quaternion m_lastorientation = new Quaternion();	75	private Quaternion m_lastorientation = new Quaternion();
74	private Quaternion _orientation;	76	private Quaternion _orientation;
75		77
@@ -128,9 +130,10 @@ namespace OpenSim.Region.Physics.OdePlugin
128		130
129	public bool m_disabled;	131	public bool m_disabled;
130		132
131
132	public uint m_localID;	133	public uint m_localID;
133		134
		135	private IMesh m_mesh;
		136	private object m_meshlock = new object();
134	private PrimitiveBaseShape _pbs;	137	private PrimitiveBaseShape _pbs;
135	public OdeScene _parent_scene;	138	public OdeScene _parent_scene;
136		139
@@ -482,6 +485,24 @@ namespace OpenSim.Region.Physics.OdePlugin
482	{	485	{
483	set	486	set
484	{	487	{
		488	/*
		489	IMesh mesh = null;
		490	if (_parent_scene.needsMeshing(value))
		491	{
		492	bool convex;
		493	if (m_shapetype == 0)
		494	convex = false;
		495	else
		496	convex = true;
		497	mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);
		498	}
		499
		500	if (mesh != null)
		501	{
		502	lock (m_meshlock)
		503	m_mesh = mesh;
		504	}
		505	*/
485	AddChange(changes.Shape, value);	506	AddChange(changes.Shape, value);
486	}	507	}
487	}	508	}
@@ -497,7 +518,8 @@ namespace OpenSim.Region.Physics.OdePlugin
497	m_shapetype = value;	518	m_shapetype = value;
498	AddChange(changes.Shape, null);	519	AddChange(changes.Shape, null);
499	}	520	}
500	}	521	}
		522
501		523
502	public override Vector3 Velocity	524	public override Vector3 Velocity
503	{	525	{
@@ -947,6 +969,19 @@ namespace OpenSim.Region.Physics.OdePlugin
947		969
948	CalcPrimBodyData();	970	CalcPrimBodyData();
949		971
		972	m_mesh = null;
		973	if (_parent_scene.needsMeshing(pbs))
		974	{
		975	bool convex;
		976	if (m_shapetype == 0)
		977	convex = false;
		978	else
		979	convex = true;
		980
		981	m_mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);
		982	}
		983
		984
950	m_building = true; // control must set this to false when done	985	m_building = true; // control must set this to false when done
951		986
952	AddChange(changes.Add, null);	987	AddChange(changes.Add, null);
@@ -1049,6 +1084,10 @@ namespace OpenSim.Region.Physics.OdePlugin
1049		1084
1050	private bool setMesh(OdeScene parent_scene)	1085	private bool setMesh(OdeScene parent_scene)
1051	{	1086	{
		1087	IntPtr vertices, indices;
		1088	int vertexCount, indexCount;
		1089	int vertexStride, triStride;
		1090
1052	if (Body != IntPtr.Zero)	1091	if (Body != IntPtr.Zero)
1053	{	1092	{
1054	if (childPrim)	1093	if (childPrim)
@@ -1065,38 +1104,50 @@ namespace OpenSim.Region.Physics.OdePlugin
1065	}	1104	}
1066	}	1105	}
1067		1106
1068	bool convex;	1107	IMesh mesh = null;
1069	if (m_shapetype == 0)	1108
1070	convex = false;
1071	else
1072	convex = true;
1073		1109
1074	IMesh mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true,convex);	1110	lock (m_meshlock)
1075	if (mesh == null)
1076	{	1111	{
1077	m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z);	1112	if (m_mesh == null)
1078	return false;	1113	{
1079	}	1114	bool convex;
		1115	if (m_shapetype == 0)
		1116	convex = false;
		1117	else
		1118	convex = true;
1080		1119
1081	IntPtr vertices, indices;	1120	mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);
1082	int vertexCount, indexCount;	1121	}
1083	int vertexStride, triStride;	1122	else
		1123	{
		1124	mesh = m_mesh;
		1125	}
1084		1126
1085	mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap	1127	if (mesh == null)
1086	mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage	1128	{
		1129	m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z);
		1130	return false;
		1131	}
1087		1132
1088	if (vertexCount == 0 \|\| indexCount == 0)
1089	{
1090	m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}",
1091	Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString());
1092	mesh.releaseSourceMeshData();
1093	return false;
1094	}
1095		1133
1096	primOOBoffset = mesh.GetCentroid();	1134	mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
1097	hasOOBoffsetFromMesh = true;	1135	mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
1098		1136
1099	mesh.releaseSourceMeshData();	1137	if (vertexCount == 0 \|\| indexCount == 0)
		1138	{
		1139	m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}",
		1140	Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString());
		1141	mesh.releaseSourceMeshData();
		1142	return false;
		1143	}
		1144
		1145	primOOBoffset = mesh.GetCentroid();
		1146	hasOOBoffsetFromMesh = true;
		1147
		1148	mesh.releaseSourceMeshData();
		1149	m_mesh = null;
		1150	}
1100		1151
1101	IntPtr geo = IntPtr.Zero;	1152	IntPtr geo = IntPtr.Zero;
1102		1153
@@ -1429,7 +1480,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1429	d.RfromQ(out mymat, ref myrot);	1480	d.RfromQ(out mymat, ref myrot);
1430	d.MassRotate(ref objdmass, ref mymat);	1481	d.MassRotate(ref objdmass, ref mymat);
1431		1482
1432	// set the body rotation and position	1483	// set the body rotation
1433	d.BodySetRotation(Body, ref mymat);	1484	d.BodySetRotation(Body, ref mymat);
1434		1485
1435	// recompute full object inertia if needed	1486	// recompute full object inertia if needed
@@ -1725,6 +1776,215 @@ namespace OpenSim.Region.Physics.OdePlugin
1725	m_collisionscore = 0;	1776	m_collisionscore = 0;
1726	}	1777	}
1727		1778
		1779	private void FixInertia(Vector3 NewPos,Quaternion newrot)
		1780	{
		1781	d.Matrix3 mat = new d.Matrix3();
		1782	d.Quaternion quat = new d.Quaternion();
		1783
		1784	d.Mass tmpdmass = new d.Mass { };
		1785	d.Mass objdmass = new d.Mass { };
		1786
		1787	d.BodyGetMass(Body, out tmpdmass);
		1788	objdmass = tmpdmass;
		1789
		1790	d.Vector3 dobjpos;
		1791	d.Vector3 thispos;
		1792
		1793	// get current object position and rotation
		1794	dobjpos = d.BodyGetPosition(Body);
		1795
		1796	// get prim own inertia in its local frame
		1797	tmpdmass = primdMass;
		1798
		1799	// transform to object frame
		1800	mat = d.GeomGetOffsetRotation(prim_geom);
		1801	d.MassRotate(ref tmpdmass, ref mat);
		1802
		1803	thispos = d.GeomGetOffsetPosition(prim_geom);
		1804	d.MassTranslate(ref tmpdmass,
		1805	thispos.X,
		1806	thispos.Y,
		1807	thispos.Z);
		1808
		1809	// subtract current prim inertia from object
		1810	DMassSubPartFromObj(ref tmpdmass, ref objdmass);
		1811
		1812	// back prim own inertia
		1813	tmpdmass = primdMass;
		1814
		1815	// update to new position and orientation
		1816	_position = NewPos;
		1817	d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
		1818	_orientation = newrot;
		1819	quat.X = newrot.X;
		1820	quat.Y = newrot.Y;
		1821	quat.Z = newrot.Z;
		1822	quat.W = newrot.W;
		1823	d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
		1824
		1825	mat = d.GeomGetOffsetRotation(prim_geom);
		1826	d.MassRotate(ref tmpdmass, ref mat);
		1827
		1828	thispos = d.GeomGetOffsetPosition(prim_geom);
		1829	d.MassTranslate(ref tmpdmass,
		1830	thispos.X,
		1831	thispos.Y,
		1832	thispos.Z);
		1833
		1834	d.MassAdd(ref objdmass, ref tmpdmass);
		1835
		1836	// fix all positions
		1837	IntPtr g = d.BodyGetFirstGeom(Body);
		1838	while (g != IntPtr.Zero)
		1839	{
		1840	thispos = d.GeomGetOffsetPosition(g);
		1841	thispos.X -= objdmass.c.X;
		1842	thispos.Y -= objdmass.c.Y;
		1843	thispos.Z -= objdmass.c.Z;
		1844	d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
		1845	g = d.dBodyGetNextGeom(g);
		1846	}
		1847	d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos);
		1848
		1849	d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
		1850	d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
		1851	d.BodySetMass(Body, ref objdmass);
		1852	_mass = objdmass.mass;
		1853	}
		1854
		1855
		1856
		1857	private void FixInertia(Vector3 NewPos)
		1858	{
		1859	d.Matrix3 primmat = new d.Matrix3();
		1860	d.Mass tmpdmass = new d.Mass { };
		1861	d.Mass objdmass = new d.Mass { };
		1862	d.Mass primmass = new d.Mass { };
		1863
		1864	d.Vector3 dobjpos;
		1865	d.Vector3 thispos;
		1866
		1867	d.BodyGetMass(Body, out objdmass);
		1868
		1869	// get prim own inertia in its local frame
		1870	primmass = primdMass;
		1871	// transform to object frame
		1872	primmat = d.GeomGetOffsetRotation(prim_geom);
		1873	d.MassRotate(ref primmass, ref primmat);
		1874
		1875	tmpdmass = primmass;
		1876
		1877	thispos = d.GeomGetOffsetPosition(prim_geom);
		1878	d.MassTranslate(ref tmpdmass,
		1879	thispos.X,
		1880	thispos.Y,
		1881	thispos.Z);
		1882
		1883	// subtract current prim inertia from object
		1884	DMassSubPartFromObj(ref tmpdmass, ref objdmass);
		1885
		1886	// update to new position
		1887	_position = NewPos;
		1888	d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
		1889
		1890	thispos = d.GeomGetOffsetPosition(prim_geom);
		1891	d.MassTranslate(ref primmass,
		1892	thispos.X,
		1893	thispos.Y,
		1894	thispos.Z);
		1895
		1896	d.MassAdd(ref objdmass, ref primmass);
		1897
		1898	// fix all positions
		1899	IntPtr g = d.BodyGetFirstGeom(Body);
		1900	while (g != IntPtr.Zero)
		1901	{
		1902	thispos = d.GeomGetOffsetPosition(g);
		1903	thispos.X -= objdmass.c.X;
		1904	thispos.Y -= objdmass.c.Y;
		1905	thispos.Z -= objdmass.c.Z;
		1906	d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
		1907	g = d.dBodyGetNextGeom(g);
		1908	}
		1909
		1910	d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
		1911
		1912	// get current object position and rotation
		1913	dobjpos = d.BodyGetPosition(Body);
		1914
		1915	d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
		1916	d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
		1917	d.BodySetMass(Body, ref objdmass);
		1918	_mass = objdmass.mass;
		1919	}
		1920
		1921	private void FixInertia(Quaternion newrot)
		1922	{
		1923	d.Matrix3 mat = new d.Matrix3();
		1924	d.Quaternion quat = new d.Quaternion();
		1925
		1926	d.Mass tmpdmass = new d.Mass { };
		1927	d.Mass objdmass = new d.Mass { };
		1928	d.Vector3 dobjpos;
		1929	d.Vector3 thispos;
		1930
		1931	d.BodyGetMass(Body, out objdmass);
		1932
		1933	// get prim own inertia in its local frame
		1934	tmpdmass = primdMass;
		1935	mat = d.GeomGetOffsetRotation(prim_geom);
		1936	d.MassRotate(ref tmpdmass, ref mat);
		1937	// transform to object frame
		1938	thispos = d.GeomGetOffsetPosition(prim_geom);
		1939	d.MassTranslate(ref tmpdmass,
		1940	thispos.X,
		1941	thispos.Y,
		1942	thispos.Z);
		1943
		1944	// subtract current prim inertia from object
		1945	DMassSubPartFromObj(ref tmpdmass, ref objdmass);
		1946
		1947	// update to new orientation
		1948	_orientation = newrot;
		1949	quat.X = newrot.X;
		1950	quat.Y = newrot.Y;
		1951	quat.Z = newrot.Z;
		1952	quat.W = newrot.W;
		1953	d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
		1954
		1955	tmpdmass = primdMass;
		1956	mat = d.GeomGetOffsetRotation(prim_geom);
		1957	d.MassRotate(ref tmpdmass, ref mat);
		1958	d.MassTranslate(ref tmpdmass,
		1959	thispos.X,
		1960	thispos.Y,
		1961	thispos.Z);
		1962
		1963	d.MassAdd(ref objdmass, ref tmpdmass);
		1964
		1965	// fix all positions
		1966	IntPtr g = d.BodyGetFirstGeom(Body);
		1967	while (g != IntPtr.Zero)
		1968	{
		1969	thispos = d.GeomGetOffsetPosition(g);
		1970	thispos.X -= objdmass.c.X;
		1971	thispos.Y -= objdmass.c.Y;
		1972	thispos.Z -= objdmass.c.Z;
		1973	d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
		1974	g = d.dBodyGetNextGeom(g);
		1975	}
		1976
		1977	d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
		1978	// get current object position and rotation
		1979	dobjpos = d.BodyGetPosition(Body);
		1980
		1981	d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
		1982	d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
		1983	d.BodySetMass(Body, ref objdmass);
		1984	_mass = objdmass.mass;
		1985	}
		1986
		1987
1728	#region Mass Calculation	1988	#region Mass Calculation
1729		1989
1730	private float CalculatePrimVolume()	1990	private float CalculatePrimVolume()
@@ -2126,17 +2386,18 @@ namespace OpenSim.Region.Physics.OdePlugin
2126	{	2386	{
2127	if (prim_geom != IntPtr.Zero)	2387	if (prim_geom != IntPtr.Zero)
2128	{	2388	{
2129	d.Vector3 lpos;
2130	d.GeomCopyPosition(prim_geom, out lpos);
2131	_position.X = lpos.X;
2132	_position.Y = lpos.Y;
2133	_position.Z = lpos.Z;
2134	d.Quaternion qtmp = new d.Quaternion { };	2389	d.Quaternion qtmp = new d.Quaternion { };
2135	d.GeomCopyQuaternion(prim_geom, out qtmp);	2390	d.GeomCopyQuaternion(prim_geom, out qtmp);
2136	_orientation.W = qtmp.W;	2391	_orientation.W = qtmp.W;
2137	_orientation.X = qtmp.X;	2392	_orientation.X = qtmp.X;
2138	_orientation.Y = qtmp.Y;	2393	_orientation.Y = qtmp.Y;
2139	_orientation.Z = qtmp.Z;	2394	_orientation.Z = qtmp.Z;
		2395
		2396	d.Vector3 lpos;
		2397	d.GeomCopyPosition(prim_geom, out lpos);
		2398	_position.X = lpos.X;
		2399	_position.Y = lpos.Y;
		2400	_position.Z = lpos.Z;
2140	}	2401	}
2141	}	2402	}
2142		2403
@@ -2593,6 +2854,13 @@ namespace OpenSim.Region.Physics.OdePlugin
2593	{	2854	{
2594	_position = newPos;	2855	_position = newPos;
2595	}	2856	}
		2857
		2858	else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero)
		2859	{
		2860	FixInertia(newPos);
		2861	if (!d.BodyIsEnabled(Body))
		2862	d.BodyEnable(Body);
		2863	}
2596	}	2864	}
2597	else	2865	else
2598	{	2866	{
@@ -2637,6 +2905,14 @@ namespace OpenSim.Region.Physics.OdePlugin
2637	{	2905	{
2638	_orientation = newOri;	2906	_orientation = newOri;
2639	}	2907	}
		2908	/*
		2909	else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero)
		2910	{
		2911	FixInertia(_position, newOri);
		2912	if (!d.BodyIsEnabled(Body))
		2913	d.BodyEnable(Body);
		2914	}
		2915	*/
2640	}	2916	}
2641	else	2917	else
2642	{	2918	{
@@ -3512,6 +3788,29 @@ namespace OpenSim.Region.Physics.OdePlugin
3512	dst.I.M22 = src.I.M22;	3788	dst.I.M22 = src.I.M22;
3513	}	3789	}
3514		3790
		3791	internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj)
		3792	{
		3793	// assumes object center of mass is zero
		3794	float smass = part.mass;
		3795	theobj.mass -= smass;
		3796
		3797	smass *= 1.0f / (theobj.mass); ;
		3798
		3799	theobj.c.X -= part.c.X * smass;
		3800	theobj.c.Y -= part.c.Y * smass;
		3801	theobj.c.Z -= part.c.Z * smass;
		3802
		3803	theobj.I.M00 -= part.I.M00;
		3804	theobj.I.M01 -= part.I.M01;
		3805	theobj.I.M02 -= part.I.M02;
		3806	theobj.I.M10 -= part.I.M10;
		3807	theobj.I.M11 -= part.I.M11;
		3808	theobj.I.M12 -= part.I.M12;
		3809	theobj.I.M20 -= part.I.M20;
		3810	theobj.I.M21 -= part.I.M21;
		3811	theobj.I.M22 -= part.I.M22;
		3812	}
		3813
3515	private static void DMassDup(ref d.Mass src, out d.Mass dst)	3814	private static void DMassDup(ref d.Mass src, out d.Mass dst)
3516	{	3815	{
3517	dst = new d.Mass { };	3816	dst = new d.Mass { };