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
UbitUmarov
parent
747044f587
commit
ce5d5fc8dc
|
|
@ -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);
|
||||
|
||||
//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);
|
||||
|
||||
|
||||
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);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
if (axisnum <= 0)
|
||||
return;
|
||||
// int dAMotorEuler = 1;
|
||||
// stop it
|
||||
d.BodySetTorque(Body, 0, 0, 0);
|
||||
d.BodySetAngularVel(Body, 0, 0, 0);
|
||||
|
||||
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;
|
||||
d.JointSetAMotorNumAxes(Amotor, axisnum);
|
||||
|
||||
// get current orientation to lock
|
||||
|
||||
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;
|
||||
|
||||
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++;
|
||||
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);
|
||||
}
|
||||
|
||||
// for (int j = 0; j < (int)axisnum; j++)
|
||||
{
|
||||
//d.JointSetAMotorAngle(Amotor, j, 0);
|
||||
}
|
||||
|
||||
//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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue