diff options
-rw-r--r-- | OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs | 347 |
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"); | |||
2995 | 2995 | ||
2996 | public override float APIDDamping{ set { return; } } | 2996 | public override float APIDDamping{ set { return; } } |
2997 | 2997 | ||
2998 | private void createAMotor(byte axislocks) | 2998 | private void createAMotor(byte axislock) |
2999 | { | 2999 | { |
3000 | if (Body == IntPtr.Zero) | 3000 | if (Body == IntPtr.Zero) |
3001 | return; | 3001 | return; |
@@ -3006,24 +3006,24 @@ Console.WriteLine(" JointCreateFixed"); | |||
3006 | Amotor = IntPtr.Zero; | 3006 | Amotor = IntPtr.Zero; |
3007 | } | 3007 | } |
3008 | 3008 | ||
3009 | if(axislocks == 0) | 3009 | if(axislock == 0) |
3010 | return; | 3010 | return; |
3011 | 3011 | ||
3012 | int axisnum = 0; | 3012 | int axisnum = 0; |
3013 | bool axisX = false; | 3013 | bool axisX = false; |
3014 | bool axisY = false; | 3014 | bool axisY = false; |
3015 | bool axisZ = false; | 3015 | bool axisZ = false; |
3016 | if((axislocks & 0x02) != 0) | 3016 | if((axislock & 0x02) != 0) |
3017 | { | 3017 | { |
3018 | axisnum++; | 3018 | axisnum++; |
3019 | axisX = true; | 3019 | axisX = true; |
3020 | } | 3020 | } |
3021 | if((axislocks & 0x04) != 0) | 3021 | if((axislock & 0x04) != 0) |
3022 | { | 3022 | { |
3023 | axisnum++; | 3023 | axisnum++; |
3024 | axisY = true; | 3024 | axisY = true; |
3025 | } | 3025 | } |
3026 | if((axislocks & 0x08) != 0) | 3026 | if((axislock & 0x08) != 0) |
3027 | { | 3027 | { |
3028 | axisnum++; | 3028 | axisnum++; |
3029 | axisZ = true; | 3029 | axisZ = true; |
@@ -3031,142 +3031,80 @@ Console.WriteLine(" JointCreateFixed"); | |||
3031 | 3031 | ||
3032 | if(axisnum == 0) | 3032 | if(axisnum == 0) |
3033 | return; | 3033 | return; |
3034 | 3034 | // stop it | |
3035 | // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again. | 3035 | d.BodySetTorque(Body, 0, 0, 0); |
3036 | d.Mass objMass; | 3036 | d.BodySetAngularVel(Body, 0, 0, 0); |
3037 | d.MassSetZero(out objMass); | ||
3038 | DMassCopy(ref pMass, ref objMass); | ||
3039 | 3037 | ||
3040 | //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); | 3038 | Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); |
3041 | 3039 | d.JointAttach(Amotor, Body, IntPtr.Zero); | |
3042 | Matrix4 dMassMat = FromDMass(objMass); | ||
3043 | |||
3044 | Matrix4 mathmat = Inverse(dMassMat); | ||
3045 | |||
3046 | /* | ||
3047 | //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]); | ||
3048 | |||
3049 | mathmat = Inverse(mathmat); | ||
3050 | 3040 | ||
3041 | d.JointSetAMotorMode(Amotor, 0); | ||
3051 | 3042 | ||
3052 | objMass = FromMatrix4(mathmat, ref objMass); | 3043 | d.JointSetAMotorNumAxes(Amotor, axisnum); |
3053 | //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); | ||
3054 | 3044 | ||
3055 | mathmat = Inverse(mathmat); | 3045 | // get current orientation to lock |
3056 | */ | ||
3057 | if (axisX) | ||
3058 | { | ||
3059 | mathmat.M33 = 50.0000001f; | ||
3060 | //objMass.I.M22 = 0; | ||
3061 | } | ||
3062 | if (axisY) | ||
3063 | { | ||
3064 | mathmat.M22 = 50.0000001f; | ||
3065 | //objMass.I.M11 = 0; | ||
3066 | } | ||
3067 | if (axisZ) | ||
3068 | { | ||
3069 | mathmat.M11 = 50.0000001f; | ||
3070 | //objMass.I.M00 = 0; | ||
3071 | } | ||
3072 | 3046 | ||
3073 | mathmat = Inverse(mathmat); | 3047 | d.Quaternion dcur = d.BodyGetQuaternion(Body); |
3074 | objMass = FromMatrix4(mathmat, ref objMass); | 3048 | Quaternion curr; // crap convertion between identical things |
3075 | //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); | 3049 | curr.X = dcur.X; |
3076 | 3050 | curr.Y = dcur.Y; | |
3077 | //return; | 3051 | curr.Z = dcur.Z; |
3078 | if (d.MassCheck(ref objMass)) | 3052 | curr.W = dcur.W; |
3079 | { | 3053 | Vector3 ax; |
3080 | d.BodySetMass(Body, ref objMass); | ||
3081 | } | ||
3082 | else | ||
3083 | { | ||
3084 | //m_log.Debug("[PHYSICS]: Mass invalid, ignoring"); | ||
3085 | } | ||
3086 | 3054 | ||
3087 | if (axisnum <= 0) | ||
3088 | return; | ||
3089 | // int dAMotorEuler = 1; | ||
3090 | |||
3091 | Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); | ||
3092 | d.JointAttach(Amotor, Body, IntPtr.Zero); | ||
3093 | d.JointSetAMotorMode(Amotor, 0); | ||
3094 | |||
3095 | d.JointSetAMotorNumAxes(Amotor,(int)axisnum); | ||
3096 | int i = 0; | 3055 | int i = 0; |
3097 | 3056 | int j = 0; | |
3098 | if (axisX) | 3057 | if (axisX) |
3099 | { | 3058 | { |
3100 | d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0); | 3059 | ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X |
3060 | d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); | ||
3061 | d.JointSetAMotorAngle(Amotor, 0, 0); | ||
3062 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, 0f); | ||
3063 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0f); | ||
3064 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); | ||
3065 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); | ||
3066 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); | ||
3067 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.CFM, 0f); | ||
3068 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); | ||
3069 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); | ||
3070 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); | ||
3101 | i++; | 3071 | i++; |
3072 | j = 256; // move to next axis set | ||
3102 | } | 3073 | } |
3103 | 3074 | ||
3104 | if (axisY) | 3075 | if (axisY) |
3105 | { | 3076 | { |
3106 | d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0); | 3077 | ax = (new Vector3(0, 1, 0)) * curr; |
3078 | d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); | ||
3079 | d.JointSetAMotorAngle(Amotor, i, 0); | ||
3080 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f); | ||
3081 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); | ||
3082 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); | ||
3083 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); | ||
3084 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); | ||
3085 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f); | ||
3086 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); | ||
3087 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); | ||
3088 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); | ||
3107 | i++; | 3089 | i++; |
3090 | j += 256; | ||
3108 | } | 3091 | } |
3109 | 3092 | ||
3110 | if (axisZ) | 3093 | if (axisZ) |
3111 | { | 3094 | { |
3112 | d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1); | 3095 | ax = (new Vector3(0, 0, 1)) * curr; |
3113 | i++; | 3096 | d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); |
3114 | } | 3097 | d.JointSetAMotorAngle(Amotor, i, 0); |
3115 | 3098 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f); | |
3116 | // for (int j = 0; j < (int)axisnum; j++) | 3099 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); |
3117 | { | 3100 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); |
3118 | //d.JointSetAMotorAngle(Amotor, j, 0); | 3101 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); |
3102 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); | ||
3103 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f); | ||
3104 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); | ||
3105 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); | ||
3106 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); | ||
3119 | } | 3107 | } |
3120 | |||
3121 | //d.JointSetAMotorAngle(Amotor, 1, 0); | ||
3122 | //d.JointSetAMotorAngle(Amotor, 2, 0); | ||
3123 | |||
3124 | // These lowstops and high stops are effectively (no wiggle room) | ||
3125 | d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f); | ||
3126 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f); | ||
3127 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f); | ||
3128 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); | ||
3129 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); | ||
3130 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); | ||
3131 | //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f); | ||
3132 | d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f); | ||
3133 | d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);// | ||
3134 | } | ||
3135 | |||
3136 | private Matrix4 FromDMass(d.Mass pMass) | ||
3137 | { | ||
3138 | Matrix4 obj; | ||
3139 | obj.M11 = pMass.I.M00; | ||
3140 | obj.M12 = pMass.I.M01; | ||
3141 | obj.M13 = pMass.I.M02; | ||
3142 | obj.M14 = 0; | ||
3143 | obj.M21 = pMass.I.M10; | ||
3144 | obj.M22 = pMass.I.M11; | ||
3145 | obj.M23 = pMass.I.M12; | ||
3146 | obj.M24 = 0; | ||
3147 | obj.M31 = pMass.I.M20; | ||
3148 | obj.M32 = pMass.I.M21; | ||
3149 | obj.M33 = pMass.I.M22; | ||
3150 | obj.M34 = 0; | ||
3151 | obj.M41 = 0; | ||
3152 | obj.M42 = 0; | ||
3153 | obj.M43 = 0; | ||
3154 | obj.M44 = 1; | ||
3155 | return obj; | ||
3156 | } | ||
3157 | |||
3158 | private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) | ||
3159 | { | ||
3160 | obj.I.M00 = pMat[0, 0]; | ||
3161 | obj.I.M01 = pMat[0, 1]; | ||
3162 | obj.I.M02 = pMat[0, 2]; | ||
3163 | obj.I.M10 = pMat[1, 0]; | ||
3164 | obj.I.M11 = pMat[1, 1]; | ||
3165 | obj.I.M12 = pMat[1, 2]; | ||
3166 | obj.I.M20 = pMat[2, 0]; | ||
3167 | obj.I.M21 = pMat[2, 1]; | ||
3168 | obj.I.M22 = pMat[2, 2]; | ||
3169 | return obj; | ||
3170 | } | 3108 | } |
3171 | 3109 | ||
3172 | public override void SubscribeEvents(int ms) | 3110 | public override void SubscribeEvents(int ms) |
@@ -3211,173 +3149,6 @@ Console.WriteLine(" JointCreateFixed"); | |||
3211 | return false; | 3149 | return false; |
3212 | } | 3150 | } |
3213 | 3151 | ||
3214 | public static Matrix4 Inverse(Matrix4 pMat) | ||
3215 | { | ||
3216 | if (determinant3x3(pMat) == 0) | ||
3217 | { | ||
3218 | return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible | ||
3219 | } | ||
3220 | |||
3221 | return (Adjoint(pMat) / determinant3x3(pMat)); | ||
3222 | } | ||
3223 | |||
3224 | public static Matrix4 Adjoint(Matrix4 pMat) | ||
3225 | { | ||
3226 | Matrix4 adjointMatrix = new Matrix4(); | ||
3227 | for (int i=0; i<4; i++) | ||
3228 | { | ||
3229 | for (int j=0; j<4; j++) | ||
3230 | { | ||
3231 | Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); | ||
3232 | } | ||
3233 | } | ||
3234 | |||
3235 | adjointMatrix = Transpose(adjointMatrix); | ||
3236 | return adjointMatrix; | ||
3237 | } | ||
3238 | |||
3239 | public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol) | ||
3240 | { | ||
3241 | Matrix4 minor = new Matrix4(); | ||
3242 | int m = 0, n = 0; | ||
3243 | for (int i = 0; i < 4; i++) | ||
3244 | { | ||
3245 | if (i == iRow) | ||
3246 | continue; | ||
3247 | n = 0; | ||
3248 | for (int j = 0; j < 4; j++) | ||
3249 | { | ||
3250 | if (j == iCol) | ||
3251 | continue; | ||
3252 | Matrix4SetValue(ref minor, m,n, matrix[i, j]); | ||
3253 | n++; | ||
3254 | } | ||
3255 | m++; | ||
3256 | } | ||
3257 | |||
3258 | return minor; | ||
3259 | } | ||
3260 | |||
3261 | public static Matrix4 Transpose(Matrix4 pMat) | ||
3262 | { | ||
3263 | Matrix4 transposeMatrix = new Matrix4(); | ||
3264 | for (int i = 0; i < 4; i++) | ||
3265 | for (int j = 0; j < 4; j++) | ||
3266 | Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]); | ||
3267 | return transposeMatrix; | ||
3268 | } | ||
3269 | |||
3270 | public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val) | ||
3271 | { | ||
3272 | switch (r) | ||
3273 | { | ||
3274 | case 0: | ||
3275 | switch (c) | ||
3276 | { | ||
3277 | case 0: | ||
3278 | pMat.M11 = val; | ||
3279 | break; | ||
3280 | case 1: | ||
3281 | pMat.M12 = val; | ||
3282 | break; | ||
3283 | case 2: | ||
3284 | pMat.M13 = val; | ||
3285 | break; | ||
3286 | case 3: | ||
3287 | pMat.M14 = val; | ||
3288 | break; | ||
3289 | } | ||
3290 | |||
3291 | break; | ||
3292 | case 1: | ||
3293 | switch (c) | ||
3294 | { | ||
3295 | case 0: | ||
3296 | pMat.M21 = val; | ||
3297 | break; | ||
3298 | case 1: | ||
3299 | pMat.M22 = val; | ||
3300 | break; | ||
3301 | case 2: | ||
3302 | pMat.M23 = val; | ||
3303 | break; | ||
3304 | case 3: | ||
3305 | pMat.M24 = val; | ||
3306 | break; | ||
3307 | } | ||
3308 | |||
3309 | break; | ||
3310 | case 2: | ||
3311 | switch (c) | ||
3312 | { | ||
3313 | case 0: | ||
3314 | pMat.M31 = val; | ||
3315 | break; | ||
3316 | case 1: | ||
3317 | pMat.M32 = val; | ||
3318 | break; | ||
3319 | case 2: | ||
3320 | pMat.M33 = val; | ||
3321 | break; | ||
3322 | case 3: | ||
3323 | pMat.M34 = val; | ||
3324 | break; | ||
3325 | } | ||
3326 | |||
3327 | break; | ||
3328 | case 3: | ||
3329 | switch (c) | ||
3330 | { | ||
3331 | case 0: | ||
3332 | pMat.M41 = val; | ||
3333 | break; | ||
3334 | case 1: | ||
3335 | pMat.M42 = val; | ||
3336 | break; | ||
3337 | case 2: | ||
3338 | pMat.M43 = val; | ||
3339 | break; | ||
3340 | case 3: | ||
3341 | pMat.M44 = val; | ||
3342 | break; | ||
3343 | } | ||
3344 | |||
3345 | break; | ||
3346 | } | ||
3347 | } | ||
3348 | |||
3349 | private static float determinant3x3(Matrix4 pMat) | ||
3350 | { | ||
3351 | float det = 0; | ||
3352 | float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2]; | ||
3353 | float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0]; | ||
3354 | float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1]; | ||
3355 | float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2]; | ||
3356 | float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0]; | ||
3357 | float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1]; | ||
3358 | |||
3359 | det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6); | ||
3360 | return det; | ||
3361 | } | ||
3362 | |||
3363 | private static void DMassCopy(ref d.Mass src, ref d.Mass dst) | ||
3364 | { | ||
3365 | dst.c.W = src.c.W; | ||
3366 | dst.c.X = src.c.X; | ||
3367 | dst.c.Y = src.c.Y; | ||
3368 | dst.c.Z = src.c.Z; | ||
3369 | dst.mass = src.mass; | ||
3370 | dst.I.M00 = src.I.M00; | ||
3371 | dst.I.M01 = src.I.M01; | ||
3372 | dst.I.M02 = src.I.M02; | ||
3373 | dst.I.M10 = src.I.M10; | ||
3374 | dst.I.M11 = src.I.M11; | ||
3375 | dst.I.M12 = src.I.M12; | ||
3376 | dst.I.M20 = src.I.M20; | ||
3377 | dst.I.M21 = src.I.M21; | ||
3378 | dst.I.M22 = src.I.M22; | ||
3379 | } | ||
3380 | |||
3381 | public override void SetMaterial(int pMaterial) | 3152 | public override void SetMaterial(int pMaterial) |
3382 | { | 3153 | { |
3383 | m_material = pMaterial; | 3154 | m_material = pMaterial; |