aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/OdePlugin
diff options
context:
space:
mode:
authorTeravus Ovares2009-04-16 07:31:48 +0000
committerTeravus Ovares2009-04-16 07:31:48 +0000
commiteac5d4015d8f76d883cfa0c19fc66633de5032dc (patch)
treeffb39cf0f472fe6cef98e94200a16931422d2d18 /OpenSim/Region/Physics/OdePlugin
parentCorrectly flag group owned prims in the land prim list (diff)
downloadopensim-SC_OLD-eac5d4015d8f76d883cfa0c19fc66633de5032dc.zip
opensim-SC_OLD-eac5d4015d8f76d883cfa0c19fc66633de5032dc.tar.gz
opensim-SC_OLD-eac5d4015d8f76d883cfa0c19fc66633de5032dc.tar.bz2
opensim-SC_OLD-eac5d4015d8f76d883cfa0c19fc66633de5032dc.tar.xz
* Committing more BulletDotNETPlugin work
* Tweak the LLSetStatus results in the ODEPlugin. Hopefully it's a little less unstable. * ODEPlugin is using experimental math for LLSetStatus, use with caution! :)
Diffstat (limited to 'OpenSim/Region/Physics/OdePlugin')
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODEPrim.cs295
1 files changed, 286 insertions, 9 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 8711937..92373cd 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -1512,8 +1512,20 @@ namespace OpenSim.Region.Physics.OdePlugin
1512 float fy = 0; 1512 float fy = 0;
1513 float fz = 0; 1513 float fz = 0;
1514 1514
1515
1515 if (IsPhysical && Body != IntPtr.Zero && !m_isSelected) 1516 if (IsPhysical && Body != IntPtr.Zero && !m_isSelected)
1516 { 1517 {
1518 if (d.BodyIsEnabled(Body) && !m_angularlock.IsIdentical(PhysicsVector.Zero, 0.003f))
1519 {
1520 d.Vector3 avel2 = d.BodyGetAngularVel(Body);
1521 if (m_angularlock.X == 1)
1522 avel2.X = 0;
1523 if (m_angularlock.Y == 1)
1524 avel2.Y = 0;
1525 if (m_angularlock.Z == 1)
1526 avel2.Z = 0;
1527 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
1528 }
1517 //float PID_P = 900.0f; 1529 //float PID_P = 900.0f;
1518 1530
1519 float m_mass = CalculateMass(); 1531 float m_mass = CalculateMass();
@@ -2716,15 +2728,66 @@ namespace OpenSim.Region.Physics.OdePlugin
2716 float axisnum = 3; 2728 float axisnum = 3;
2717 2729
2718 axisnum = (axisnum - (axis.X + axis.Y + axis.Z)); 2730 axisnum = (axisnum - (axis.X + axis.Y + axis.Z));
2731
2732 PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z);
2733
2734
2719 2735
2736 /*
2737 // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again.
2738 d.Mass objMass;
2739 d.MassSetZero(out objMass);
2740 DMassCopy(ref pMass, ref objMass);
2741
2742 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);
2743
2744 Matrix4 dMassMat = FromDMass(objMass);
2745
2746 Matrix4 mathmat = Inverse(dMassMat);
2747
2748 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]);
2749
2750 mathmat = Inverse(mathmat);
2751
2752
2753 objMass = FromMatrix4(mathmat, ref objMass);
2754 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);
2755
2756 mathmat = Inverse(mathmat);
2720 2757
2758 if (axis.X == 0)
2759 {
2760 mathmat.M33 = 50.0000001f;
2761 //objMass.I.M22 = 0;
2762 }
2763 if (axis.Y == 0)
2764 {
2765 mathmat.M22 = 50.0000001f;
2766 //objMass.I.M11 = 0;
2767 }
2768 if (axis.Z == 0)
2769 {
2770 mathmat.M11 = 50.0000001f;
2771 //objMass.I.M00 = 0;
2772 }
2773
2774
2775
2776 mathmat = Inverse(mathmat);
2777 objMass = FromMatrix4(mathmat, ref objMass);
2778 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);
2779
2780 //return;
2781
2782 d.BodySetMass(Body, ref objMass);
2783 */
2721 if (axisnum <= 0) 2784 if (axisnum <= 0)
2722 return; 2785 return;
2723 int dAMotorEuler = 1; 2786 int dAMotorEuler = 1;
2724 2787
2725 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); 2788 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
2726 d.JointAttach(Amotor, Body, IntPtr.Zero); 2789 d.JointAttach(Amotor, Body, IntPtr.Zero);
2727 d.JointSetAMotorMode(Amotor, dAMotorEuler); 2790 d.JointSetAMotorMode(Amotor, 0);
2728 2791
2729 d.JointSetAMotorNumAxes(Amotor,(int)axisnum); 2792 d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
2730 int i = 0; 2793 int i = 0;
@@ -2756,15 +2819,52 @@ namespace OpenSim.Region.Physics.OdePlugin
2756 //d.JointSetAMotorAngle(Amotor, 2, 0); 2819 //d.JointSetAMotorAngle(Amotor, 2, 0);
2757 2820
2758 // These lowstops and high stops are effectively (no wiggle room) 2821 // These lowstops and high stops are effectively (no wiggle room)
2759 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0.000000000001f); 2822 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f);
2760 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.000000000001f); 2823 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
2761 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.000000000001f); 2824 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f);
2762 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.000000000001f); 2825 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
2763 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.000000000001f); 2826 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
2764 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.000000000001f); 2827 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
2765 2828 //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f);
2766 d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f); 2829 d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
2767 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, m_tensor * 5);// 2830 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);//
2831
2832 }
2833
2834 public Matrix4 FromDMass(d.Mass pMass)
2835 {
2836 Matrix4 obj;
2837 obj.M11 = pMass.I.M00;
2838 obj.M12 = pMass.I.M01;
2839 obj.M13 = pMass.I.M02;
2840 obj.M14 = 0;
2841 obj.M21 = pMass.I.M10;
2842 obj.M22 = pMass.I.M11;
2843 obj.M23 = pMass.I.M12;
2844 obj.M24 = 0;
2845 obj.M31 = pMass.I.M20;
2846 obj.M32 = pMass.I.M21;
2847 obj.M33 = pMass.I.M22;
2848 obj.M34 = 0;
2849 obj.M41 = 0;
2850 obj.M42 = 0;
2851 obj.M43 = 0;
2852 obj.M44 = 1;
2853 return obj;
2854 }
2855
2856 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
2857 {
2858 obj.I.M00 = pMat[0, 0];
2859 obj.I.M01 = pMat[0, 1];
2860 obj.I.M02 = pMat[0, 2];
2861 obj.I.M10 = pMat[1, 0];
2862 obj.I.M11 = pMat[1, 1];
2863 obj.I.M12 = pMat[1, 2];
2864 obj.I.M20 = pMat[2, 0];
2865 obj.I.M21 = pMat[2, 1];
2866 obj.I.M22 = pMat[2, 2];
2867 return obj;
2768 } 2868 }
2769 2869
2770 public override void SubscribeEvents(int ms) 2870 public override void SubscribeEvents(int ms)
@@ -2805,5 +2905,182 @@ namespace OpenSim.Region.Physics.OdePlugin
2805 return true; 2905 return true;
2806 return false; 2906 return false;
2807 } 2907 }
2908
2909 public static Matrix4 Inverse(Matrix4 pMat)
2910 {
2911 if (determinant3x3(pMat) == 0)
2912 {
2913 return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible
2914 }
2915
2916
2917
2918 return (Adjoint(pMat) / determinant3x3(pMat));
2919 }
2920
2921 public static Matrix4 Adjoint(Matrix4 pMat)
2922 {
2923 Matrix4 adjointMatrix = new Matrix4();
2924 for (int i=0; i<4; i++)
2925 {
2926 for (int j=0; j<4; j++)
2927 {
2928 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
2929 }
2930 }
2931
2932 adjointMatrix = Transpose(adjointMatrix);
2933 return adjointMatrix;
2934 }
2935
2936 public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
2937 {
2938 Matrix4 minor = new Matrix4();
2939 int m = 0, n = 0;
2940 for (int i = 0; i < 4; i++)
2941 {
2942 if (i == iRow)
2943 continue;
2944 n = 0;
2945 for (int j = 0; j < 4; j++)
2946 {
2947 if (j == iCol)
2948 continue;
2949 Matrix4SetValue(ref minor, m,n, matrix[i, j]);
2950 n++;
2951 }
2952 m++;
2953 }
2954 return minor;
2955 }
2956
2957 public static Matrix4 Transpose(Matrix4 pMat)
2958 {
2959 Matrix4 transposeMatrix = new Matrix4();
2960 for (int i = 0; i < 4; i++)
2961 for (int j = 0; j < 4; j++)
2962 Matrix4SetValue( ref transposeMatrix, i, j, pMat[j, i]);
2963 return transposeMatrix;
2964 }
2965
2966 public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
2967 {
2968 switch (r)
2969 {
2970 case 0:
2971 switch (c)
2972 {
2973 case 0:
2974 pMat.M11 = val;
2975 break;
2976 case 1:
2977 pMat.M12 = val;
2978 break;
2979 case 2:
2980 pMat.M13 = val;
2981 break;
2982 case 3:
2983 pMat.M14 = val;
2984 break;
2985 }
2986
2987 break;
2988 case 1:
2989 switch (c)
2990 {
2991 case 0:
2992 pMat.M21 = val;
2993 break;
2994 case 1:
2995 pMat.M22 = val;
2996 break;
2997 case 2:
2998 pMat.M23 = val;
2999 break;
3000 case 3:
3001 pMat.M24 = val;
3002 break;
3003 }
3004
3005 break;
3006 case 2:
3007 switch (c)
3008 {
3009 case 0:
3010 pMat.M31 = val;
3011 break;
3012 case 1:
3013 pMat.M32 = val;
3014 break;
3015 case 2:
3016 pMat.M33 = val;
3017 break;
3018 case 3:
3019 pMat.M34 = val;
3020 break;
3021 }
3022
3023 break;
3024 case 3:
3025 switch (c)
3026 {
3027 case 0:
3028 pMat.M41 = val;
3029 break;
3030 case 1:
3031 pMat.M42 = val;
3032 break;
3033 case 2:
3034 pMat.M43 = val;
3035 break;
3036 case 3:
3037 pMat.M44 = val;
3038 break;
3039 }
3040
3041 break;
3042 }
3043 }
3044 private static float determinant3x3(Matrix4 pMat)
3045 {
3046 float det = 0;
3047 float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2];
3048 float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0];
3049 float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1];
3050 float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2];
3051 float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0];
3052 float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1];
3053
3054 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
3055 return det;
3056
3057 }
3058 private static float Determinant(Matrix4 matrix)
3059 {
3060 float det = 0;
3061
3062 for (int j = 0; j < 4; j++)
3063 det += (matrix[0, j] * Determinant(Minor(matrix, 0, j)) * (int)System.Math.Pow(-1, 0 + j));
3064 return det;
3065 }
3066
3067 private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
3068 {
3069 dst.c.W = src.c.W;
3070 dst.c.X = src.c.X;
3071 dst.c.Y = src.c.Y;
3072 dst.c.Z = src.c.Z;
3073 dst.mass = src.mass;
3074 dst.I.M00 = src.I.M00;
3075 dst.I.M01 = src.I.M01;
3076 dst.I.M02 = src.I.M02;
3077 dst.I.M10 = src.I.M10;
3078 dst.I.M11 = src.I.M11;
3079 dst.I.M12 = src.I.M12;
3080 dst.I.M20 = src.I.M20;
3081 dst.I.M21 = src.I.M21;
3082 dst.I.M22 = src.I.M22;
3083 }
3084
2808 } 3085 }
2809} 3086}