diff options
author | Teravus Ovares | 2009-04-16 07:31:48 +0000 |
---|---|---|
committer | Teravus Ovares | 2009-04-16 07:31:48 +0000 |
commit | eac5d4015d8f76d883cfa0c19fc66633de5032dc (patch) | |
tree | ffb39cf0f472fe6cef98e94200a16931422d2d18 /OpenSim/Region/Physics/OdePlugin | |
parent | Correctly flag group owned prims in the land prim list (diff) | |
download | opensim-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 '')
-rw-r--r-- | OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 295 |
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 | } |