the inertia matrix inversions and body inertia changes to implement axis locks need to be avoid, so use ubOde axis locks code. Hopefully this will not cause incompatibilities

1 files changed, 59 insertions, 288 deletions

diff --git a/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs
index 0a57dc9..908b266 100644
--- a/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs
+++ b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs
@@ -2995,7 +2995,7 @@ Console.WriteLine(" JointCreateFixed");
 
         public override float APIDDamping{ set { return; } }
 
-        private void createAMotor(byte axislocks)
+        private void createAMotor(byte axislock)
         {
             if (Body == IntPtr.Zero)
                 return;
@@ -3006,24 +3006,24 @@ Console.WriteLine(" JointCreateFixed");
                 Amotor = IntPtr.Zero;
             }
 
-            if(axislocks == 0)
+            if(axislock == 0)
                 return;
 
             int axisnum = 0;
             bool axisX = false;
             bool axisY = false;
             bool axisZ = false;
-            if((axislocks & 0x02) != 0)
+            if((axislock & 0x02) != 0)
                 {
                 axisnum++;
                 axisX = true;
                 }
-            if((axislocks & 0x04) != 0)
+            if((axislock & 0x04) != 0)
                 {
                 axisnum++;
                 axisY = true;
                 }
-            if((axislocks & 0x08) != 0)
+            if((axislock & 0x08) != 0)
                 {
                 axisnum++;
                 axisZ = true;
@@ -3031,142 +3031,80 @@ Console.WriteLine(" JointCreateFixed");
 
             if(axisnum == 0)
                 return;
-           
-            // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again.
-            d.Mass objMass;
-            d.MassSetZero(out objMass);
-            DMassCopy(ref pMass, ref objMass);
+            // stop it
+            d.BodySetTorque(Body, 0, 0, 0);
+            d.BodySetAngularVel(Body, 0, 0, 0);
 
-            //m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
-
-            Matrix4 dMassMat = FromDMass(objMass);
-
-            Matrix4 mathmat = Inverse(dMassMat);
-
-            /*
-            //m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]);
-
-            mathmat = Inverse(mathmat);
+            Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
+            d.JointAttach(Amotor, Body, IntPtr.Zero);
 
+            d.JointSetAMotorMode(Amotor, 0);
 
-            objMass = FromMatrix4(mathmat, ref objMass);
-            //m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
+            d.JointSetAMotorNumAxes(Amotor, axisnum);
 
-            mathmat = Inverse(mathmat);
-            */
-            if (axisX)
-            {
-                mathmat.M33 = 50.0000001f;
-                //objMass.I.M22 = 0;
-            }
-            if (axisY)
-            {
-                mathmat.M22 = 50.0000001f;
-                //objMass.I.M11 = 0;
-            }
-            if (axisZ)
-            {
-                mathmat.M11 = 50.0000001f;
-                //objMass.I.M00 = 0;
-            }
+            // get current orientation to lock
 
-            mathmat = Inverse(mathmat);
-            objMass = FromMatrix4(mathmat, ref objMass);
-            //m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
-           
-            //return;
-            if (d.MassCheck(ref objMass))
-            {
-                d.BodySetMass(Body, ref objMass);
-            }
-            else
-            {
-                //m_log.Debug("[PHYSICS]: Mass invalid, ignoring");
-            }
+            d.Quaternion dcur = d.BodyGetQuaternion(Body);
+            Quaternion curr; // crap convertion between identical things
+            curr.X = dcur.X;
+            curr.Y = dcur.Y;
+            curr.Z = dcur.Z;
+            curr.W = dcur.W;
+            Vector3 ax;
 
-            if (axisnum <= 0)
-                return;
-            // int dAMotorEuler = 1;
-
-            Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
-            d.JointAttach(Amotor, Body, IntPtr.Zero);
-            d.JointSetAMotorMode(Amotor, 0);
-
-            d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
             int i = 0;
-
+            int j = 0;
             if (axisX)
             {
-                d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0);
+                ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X
+                d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
+                d.JointSetAMotorAngle(Amotor, 0, 0);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.CFM, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);
                 i++;
+                j = 256; // move to next axis set
             }
 
             if (axisY)
             {
-                d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0);
+                ax = (new Vector3(0, 1, 0)) * curr;
+                d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
+                d.JointSetAMotorAngle(Amotor, i, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
                 i++;
+                j += 256;
             }
 
             if (axisZ)
             {
-                d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1);
-                i++;
-            }
-
-//            for (int j = 0; j < (int)axisnum; j++)
-            {
-                //d.JointSetAMotorAngle(Amotor, j, 0);
+                ax = (new Vector3(0, 0, 1)) * curr;
+                d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
+                d.JointSetAMotorAngle(Amotor, i, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
             }
-
-            //d.JointSetAMotorAngle(Amotor, 1, 0);
-            //d.JointSetAMotorAngle(Amotor, 2, 0);
-
-            // These lowstops and high stops are effectively (no wiggle room)
-            d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
-            //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);//
-        }
-
-        private Matrix4 FromDMass(d.Mass pMass)
-        {
-            Matrix4 obj;
-            obj.M11 = pMass.I.M00;
-            obj.M12 = pMass.I.M01;
-            obj.M13 = pMass.I.M02;
-            obj.M14 = 0;
-            obj.M21 = pMass.I.M10;
-            obj.M22 = pMass.I.M11;
-            obj.M23 = pMass.I.M12;
-            obj.M24 = 0;
-            obj.M31 = pMass.I.M20;
-            obj.M32 = pMass.I.M21;
-            obj.M33 = pMass.I.M22;
-            obj.M34 = 0;
-            obj.M41 = 0;
-            obj.M42 = 0;
-            obj.M43 = 0;
-            obj.M44 = 1;
-            return obj;
-        }
-
-        private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
-        {
-            obj.I.M00 = pMat[0, 0];
-            obj.I.M01 = pMat[0, 1];
-            obj.I.M02 = pMat[0, 2];
-            obj.I.M10 = pMat[1, 0];
-            obj.I.M11 = pMat[1, 1];
-            obj.I.M12 = pMat[1, 2];
-            obj.I.M20 = pMat[2, 0];
-            obj.I.M21 = pMat[2, 1];
-            obj.I.M22 = pMat[2, 2];
-            return obj;
         }
 
         public override void SubscribeEvents(int ms)
@@ -3211,173 +3149,6 @@ Console.WriteLine(" JointCreateFixed");
             return false;
         }
 
-        public static Matrix4 Inverse(Matrix4 pMat)
-        {
-            if (determinant3x3(pMat) == 0)
-            {
-                return Matrix4.Identity; // should probably throw an error.  singluar matrix inverse not possible
-            }
-
-            return (Adjoint(pMat) / determinant3x3(pMat));
-        }
-
-        public static Matrix4 Adjoint(Matrix4 pMat)
-        {
-            Matrix4 adjointMatrix = new Matrix4();
-            for (int i=0; i<4; i++)
-            {
-                for (int j=0; j<4; j++)
-                {
-                    Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
-                }
-            }
-
-            adjointMatrix = Transpose(adjointMatrix);
-            return adjointMatrix;
-        }
-
-        public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
-        {
-            Matrix4 minor = new Matrix4();
-            int m = 0, n = 0;
-            for (int i = 0; i < 4; i++)
-            {
-                if (i == iRow)
-                    continue;
-                n = 0;
-                for (int j = 0; j < 4; j++)
-                {
-                    if (j == iCol)
-                        continue;
-                    Matrix4SetValue(ref minor, m,n, matrix[i, j]);
-                    n++;
-                }
-                m++;
-            }
-
-            return minor;
-        }
-
-        public static Matrix4 Transpose(Matrix4 pMat)
-        {
-            Matrix4 transposeMatrix = new Matrix4();
-            for (int i = 0; i < 4; i++)
-                for (int j = 0; j < 4; j++)
-                    Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
-            return transposeMatrix;
-        }
-
-        public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
-        {
-            switch (r)
-            {
-                case 0:
-                    switch (c)
-                    {
-                        case 0:
-                            pMat.M11 = val;
-                            break;
-                        case 1:
-                            pMat.M12 = val;
-                            break;
-                        case 2:
-                            pMat.M13 = val;
-                            break;
-                        case 3:
-                            pMat.M14 = val;
-                            break;
-                    }
-
-                    break;
-                case 1:
-                    switch (c)
-                    {
-                        case 0:
-                            pMat.M21 = val;
-                            break;
-                        case 1:
-                            pMat.M22 = val;
-                            break;
-                        case 2:
-                            pMat.M23 = val;
-                            break;
-                        case 3:
-                            pMat.M24 = val;
-                            break;
-                    }
-
-                    break;
-                case 2:
-                    switch (c)
-                    {
-                        case 0:
-                            pMat.M31 = val;
-                            break;
-                        case 1:
-                            pMat.M32 = val;
-                            break;
-                        case 2:
-                            pMat.M33 = val;
-                            break;
-                        case 3:
-                            pMat.M34 = val;
-                            break;
-                    }
-
-                    break;
-                case 3:
-                    switch (c)
-                    {
-                        case 0:
-                            pMat.M41 = val;
-                            break;
-                        case 1:
-                            pMat.M42 = val;
-                            break;
-                        case 2:
-                            pMat.M43 = val;
-                            break;
-                        case 3:
-                            pMat.M44 = val;
-                            break;
-                    }
-
-                    break;
-            }
-        }
-
-        private static float determinant3x3(Matrix4 pMat)
-        {
-            float det = 0;
-            float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2];
-            float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0];
-            float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1];
-            float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2];
-            float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0];
-            float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1];
-
-            det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
-            return det;
-        }
-        
-        private 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;
-        }
-
         public override void SetMaterial(int pMaterial)
         {
             m_material = pMaterial;