public IndexedBasisMatrix TimesTranspose(IndexedBasisMatrix m)
{
return(new IndexedBasisMatrix(
_el0.Dot(m._el0), _el0.Dot(m._el1), _el0.Dot(m._el2),
_el1.Dot(m._el0), _el1.Dot(m._el1), _el1.Dot(m._el2),
_el2.Dot(m._el0), _el2.Dot(m._el1), _el2.Dot(m._el2)));
}
public static IndexedMatrix CreateScale(float x, float y, float z)
{
IndexedMatrix IndexedMatrix = IndexedMatrix.Identity;
IndexedMatrix._basis = IndexedBasisMatrix.CreateScale(new IndexedVector3(x, y, z));
return(IndexedMatrix);
}
public static IndexedMatrix CreateScale(IndexedVector3 scales)
{
IndexedMatrix result = IndexedMatrix.Identity;
result._basis = IndexedBasisMatrix.CreateScale(scales);
return(result);
}
public static IndexedMatrix CreateScale(float scale)
{
IndexedMatrix result = IndexedMatrix.Identity;
result._basis = IndexedBasisMatrix.CreateScale(new IndexedVector3(scale));
return(result);
}
public static IndexedBasisMatrix CreateFromAxisAngle(IndexedVector3 axis, float angle)
{
float num1 = axis.X;
float num2 = axis.Y;
float num3 = axis.Z;
float num4 = (float)Math.Sin((double)angle);
float num5 = (float)Math.Cos((double)angle);
float num6 = num1 * num1;
float num7 = num2 * num2;
float num8 = num3 * num3;
float num9 = num1 * num2;
float num10 = num1 * num3;
float num11 = num2 * num3;
IndexedBasisMatrix ibm = new IndexedBasisMatrix(num6 + num5 * (1f - num6),
(float)((double)num9 - (double)num5 * (double)num9 + (double)num4 * (double)num3),
(float)((double)num10 - (double)num5 * (double)num10 - (double)num4 * (double)num2),
(float)((double)num9 - (double)num5 * (double)num9 - (double)num4 * (double)num3),
num7 + num5 * (1f - num7),
(float)((double)num11 - (double)num5 * (double)num11 + (double)num4 * (double)num1),
(float)((double)num10 - (double)num5 * (double)num10 + (double)num4 * (double)num2),
(float)((double)num11 - (double)num5 * (double)num11 - (double)num4 * (double)num1),
num8 + num5 * (1f - num8));
return(ibm);
}
public static IndexedBasisMatrix CreateRotationY(float radians)
{
float num1 = (float)Math.Cos((double)radians);
float num2 = (float)Math.Sin((double)radians);
IndexedBasisMatrix ibm = new IndexedBasisMatrix(num1, 0.0f, -num2, 0.0f, 1f, 0.0f, num2, 0.0f, num1);
return(ibm);
}
//angular constraint between two different rigidbodies
public JacobianEntry(IndexedVector3 jointAxis,
IndexedBasisMatrix world2A,
IndexedBasisMatrix world2B,
IndexedVector3 inertiaInvA,
IndexedVector3 inertiaInvB) : this(ref jointAxis,ref world2A,ref world2B,ref inertiaInvA,ref inertiaInvB)
{
}
public static void CreateRotationZ(float radians, out IndexedBasisMatrix _basis)
{
float num1 = (float)Math.Cos((double)radians);
float num2 = (float)Math.Sin((double)radians);
_basis._el0 = new IndexedVector3(num1, num2, 0);
_basis._el1 = new IndexedVector3(-num2, num1, 0);
_basis._el2 = new IndexedVector3(0, 0, 1);
}
public static IndexedMatrix CreateRotationZ(float radians)
{
IndexedMatrix IndexedMatrix;
IndexedMatrix._basis = IndexedBasisMatrix.CreateRotationZ(radians);
IndexedMatrix._origin = new IndexedVector3(0, 0, 0);
return(IndexedMatrix);
}
public IndexedMatrix InverseTimes(ref IndexedMatrix t)
{
IndexedVector3 v = new IndexedVector3(t._origin.X - _origin.X, t._origin.Y - _origin.Y, t._origin.Z - _origin.Z);
IndexedVector3 v2 = new IndexedVector3();
IndexedBasisMatrix.Multiply(ref v2, ref _basis, ref v);
return(new IndexedMatrix(_basis.TransposeTimes(ref t._basis),
v * _basis));
}
public IndexedBasisMatrix TransposeTimes(ref IndexedBasisMatrix m)
{
return(new IndexedBasisMatrix(
_Row0.X * m._Row0.X + _Row1.X * m._Row1.X + _Row2.X * m._Row2.X,
_Row0.X * m._Row0.Y + _Row1.X * m._Row1.Y + _Row2.X * m._Row2.Y,
_Row0.X * m._Row0.Z + _Row1.X * m._Row1.Z + _Row2.X * m._Row2.Z,
_Row0.Y * m._Row0.X + _Row1.Y * m._Row1.X + _Row2.Y * m._Row2.X,
_Row0.Y * m._Row0.Y + _Row1.Y * m._Row1.Y + _Row2.Y * m._Row2.Y,
_Row0.Y * m._Row0.Z + _Row1.Y * m._Row1.Z + _Row2.Y * m._Row2.Z,
_Row0.Z * m._Row0.X + _Row1.Z * m._Row1.X + _Row2.Z * m._Row2.X,
_Row0.Z * m._Row0.Y + _Row1.Z * m._Row1.Y + _Row2.Z * m._Row2.Y,
_Row0.Z * m._Row0.Z + _Row1.Z * m._Row1.Z + _Row2.Z * m._Row2.Z));
}
public IndexedBasisMatrix TransposeTimes(IndexedBasisMatrix m)
{
return(new IndexedBasisMatrix(
_el0.X * m._el0.X + _el1.X * m._el1.X + _el2.X * m._el2.X,
_el0.X * m._el0.Y + _el1.X * m._el1.Y + _el2.X * m._el2.Y,
_el0.X * m._el0.Z + _el1.X * m._el1.Z + _el2.X * m._el2.Z,
_el0.Y * m._el0.X + _el1.Y * m._el1.X + _el2.Y * m._el2.X,
_el0.Y * m._el0.Y + _el1.Y * m._el1.Y + _el2.Y * m._el2.Y,
_el0.Y * m._el0.Z + _el1.Y * m._el1.Z + _el2.Y * m._el2.Z,
_el0.Z * m._el0.X + _el1.Z * m._el1.X + _el2.Z * m._el2.X,
_el0.Z * m._el0.Y + _el1.Z * m._el1.Y + _el2.Z * m._el2.Y,
_el0.Z * m._el0.Z + _el1.Z * m._el1.Z + _el2.Z * m._el2.Z));
}
//constraint between two different rigidbodies
public JacobianEntry(IndexedBasisMatrix world2A,
IndexedBasisMatrix world2B,
IndexedVector3 rel_pos1,
IndexedVector3 rel_pos2,
IndexedVector3 jointAxis,
IndexedVector3 inertiaInvA,
float massInvA,
IndexedVector3 inertiaInvB,
float massInvB) : this(ref world2A,ref world2B,ref rel_pos1,ref rel_pos2,
ref jointAxis, ref inertiaInvA,massInvA,ref inertiaInvB,massInvB)
{
}
public static float[,] BasisMatrixToFloatArray(ref IndexedBasisMatrix m)
{
float[,] result = new float[3, 3];
result[0, 0] = m._Row0.X;
result[0, 1] = m._Row0.Y;
result[0, 2] = m._Row0.Z;
result[1, 0] = m._Row1.X;
result[1, 1] = m._Row1.Y;
result[1, 2] = m._Row1.Z;
result[2, 0] = m._Row2.X;
result[2, 1] = m._Row2.Y;
result[2, 2] = m._Row2.Z;
return result;
}
public JacobianEntry(ref IndexedVector3 jointAxis,
ref IndexedBasisMatrix world2A,
ref IndexedBasisMatrix world2B,
ref IndexedVector3 inertiaInvA,
ref IndexedVector3 inertiaInvB)
{
m_linearJointAxis = IndexedVector3.Zero;
m_aJ = world2A * jointAxis;
m_bJ = world2B * -jointAxis;
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = IndexedVector3.Dot(m_0MinvJt,m_aJ) + IndexedVector3.Dot(m_1MinvJt,m_bJ);
Debug.Assert(m_Adiag > 0.0f);
}
public JacobianEntry(
ref IndexedBasisMatrix world2A,
ref IndexedBasisMatrix world2B,
ref IndexedVector3 rel_pos1,ref IndexedVector3 rel_pos2,
ref IndexedVector3 jointAxis,
ref IndexedVector3 inertiaInvA,
float massInvA,
ref IndexedVector3 inertiaInvB,
float massInvB)
{
m_linearJointAxis = jointAxis;
m_aJ = world2A * (rel_pos1.Cross(ref m_linearJointAxis));
m_bJ = world2B * (rel_pos2.Cross(-m_linearJointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = massInvA + IndexedVector3.Dot(m_0MinvJt,m_aJ) + massInvB + IndexedVector3.Dot(m_1MinvJt,m_bJ);
Debug.Assert(m_Adiag > 0.0f);
}
public InertiaCallback(ref IndexedVector3 center)
{
m_sum = new IndexedBasisMatrix();
m_center = center;
}
//! Calcs the full invertion of the matrices. Useful for scaling matrices
public void CalcFromFullInvert(ref IndexedMatrix trans0, ref IndexedMatrix trans1)
{
m_R1to0 = trans0._basis.Inverse();
m_T1to0 = m_R1to0 * (-trans0._origin);
m_T1to0 += m_R1to0 * trans1._origin;
m_R1to0 *= trans1._basis;
CalcAbsoluteMatrix();
}
public static IndexedBasisMatrix CreateRotationY(float radians)
{
float num1 = (float)Math.Cos((double)radians);
float num2 = (float)Math.Sin((double)radians);
IndexedBasisMatrix ibm = new IndexedBasisMatrix(num1,0.0f, -num2,0.0f,1f,0.0f,num2,0.0f,num1);
return ibm;
}
public static void Transpose(ref IndexedBasisMatrix IndexedMatrix, out IndexedBasisMatrix result)
{
result = new IndexedBasisMatrix(IndexedMatrix._Row0.X, IndexedMatrix._Row1.X, IndexedMatrix._Row2.X,
IndexedMatrix._Row0.Y, IndexedMatrix._Row1.Y, IndexedMatrix._Row2.Y,
IndexedMatrix._Row0.Z, IndexedMatrix._Row1.Z, IndexedMatrix._Row2.Z);
}
public static float Mat3DotCol(IndexedBasisMatrix mat, ref IndexedVector3 vec3, int colindex)
{
return vec3[0] * mat[0, colindex] + vec3[1] * mat[1, colindex] + vec3[2] * mat[2, colindex];
}
public void CalcAngleInfo2(ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedBasisMatrix invInertiaWorldA, ref IndexedBasisMatrix invInertiaWorldB)
{
m_swingCorrection = 0;
m_twistLimitSign = 0;
m_solveTwistLimit = false;
m_solveSwingLimit = false;
// compute rotation of A wrt B (in constraint space)
if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
{ // it is assumed that setMotorTarget() was alredy called
// and motor target m_qTarget is within constraint limits
// TODO : split rotation to pure swing and pure twist
// compute desired transforms in world
IndexedMatrix trPose = IndexedMatrix.CreateFromQuaternion(m_qTarget);
IndexedMatrix trA = transA * m_rbAFrame;
IndexedMatrix trB = transB * m_rbBFrame;
IndexedMatrix trDeltaAB = trB * trPose * trA.Inverse();
IndexedQuaternion qDeltaAB = trDeltaAB.GetRotation();
IndexedVector3 swingAxis = new IndexedVector3(qDeltaAB.X, qDeltaAB.Y, qDeltaAB.Z);
float swingAxisLen2 = swingAxis.LengthSquared();
if (MathUtil.FuzzyZero(swingAxisLen2))
{
return;
}
m_swingAxis = swingAxis;
m_swingAxis.Normalize();
m_swingCorrection = MathUtil.QuatAngle(ref qDeltaAB);
if (!MathUtil.FuzzyZero(m_swingCorrection))
{
m_solveSwingLimit = true;
}
return;
}
{
// compute rotation of A wrt B (in constraint space)
// Not sure if these need order swapping as well?
IndexedQuaternion qA = transA.GetRotation() * m_rbAFrame.GetRotation();
IndexedQuaternion qB = transB.GetRotation() * m_rbBFrame.GetRotation();
IndexedQuaternion qAB = MathUtil.QuaternionInverse(qB) * qA;
// split rotation into cone and twist
// (all this is done from B's perspective. Maybe I should be averaging axes...)
IndexedVector3 vConeNoTwist = MathUtil.QuatRotate(ref qAB, ref vTwist);
vConeNoTwist.Normalize();
IndexedQuaternion qABCone = MathUtil.ShortestArcQuat(ref vTwist, ref vConeNoTwist);
qABCone.Normalize();
IndexedQuaternion qABTwist = MathUtil.QuaternionInverse(qABCone) * qAB;
qABTwist.Normalize();
if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
{
float swingAngle = 0f, swingLimit = 0f;
IndexedVector3 swingAxis = IndexedVector3.Zero;
ComputeConeLimitInfo(ref qABCone, ref swingAngle, ref swingAxis, ref swingLimit);
if (swingAngle > swingLimit * m_limitSoftness)
{
m_solveSwingLimit = true;
// compute limit ratio: 0->1, where
// 0 == beginning of soft limit
// 1 == hard/real limit
m_swingLimitRatio = 1f;
if (swingAngle < swingLimit && m_limitSoftness < 1f - MathUtil.SIMD_EPSILON)
{
m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness) /
(swingLimit - (swingLimit * m_limitSoftness));
}
// swing correction tries to get back to soft limit
m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
// adjustment of swing axis (based on ellipse normal)
AdjustSwingAxisToUseEllipseNormal(ref swingAxis);
// Calculate necessary axis & factors
m_swingAxis = MathUtil.QuatRotate(qB, -swingAxis);
m_twistAxisA = IndexedVector3.Zero;
m_kSwing = 1f /
(ComputeAngularImpulseDenominator(ref m_swingAxis, ref invInertiaWorldA) +
ComputeAngularImpulseDenominator(ref m_swingAxis, ref invInertiaWorldB));
}
}
else
{
// you haven't set any limits;
// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
// anyway, we have either hinge or fixed joint
IndexedVector3 ivA = transA._basis * m_rbAFrame._basis.GetColumn(0);
IndexedVector3 jvA = transA._basis * m_rbAFrame._basis.GetColumn(1);
IndexedVector3 kvA = transA._basis * m_rbAFrame._basis.GetColumn(2);
IndexedVector3 ivB = transB._basis * m_rbBFrame._basis.GetColumn(0);
IndexedVector3 target = IndexedVector3.Zero;
float x = ivB.Dot(ref ivA);
float y = ivB.Dot(ref jvA);
float z = ivB.Dot(ref kvA);
if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
{ // fixed. We'll need to add one more row to constraint
if ((!MathUtil.FuzzyZero(y)) || (!(MathUtil.FuzzyZero(z))))
{
m_solveSwingLimit = true;
m_swingAxis = -ivB.Cross(ref ivA);
}
}
else
{
if (m_swingSpan1 < m_fixThresh)
{ // hinge around Y axis
if (!(MathUtil.FuzzyZero(y)))
{
m_solveSwingLimit = true;
if (m_swingSpan2 >= m_fixThresh)
{
y = 0;
float span2 = (float)Math.Atan2(z, x);
if (span2 > m_swingSpan2)
{
x = (float)Math.Cos(m_swingSpan2);
z = (float)Math.Sin(m_swingSpan2);
}
else if (span2 < -m_swingSpan2)
{
x = (float)Math.Cos(m_swingSpan2);
z = -(float)Math.Sin(m_swingSpan2);
}
}
}
}
else
{ // hinge around Z axis
if (!MathUtil.FuzzyZero(z))
{
m_solveSwingLimit = true;
if (m_swingSpan1 >= m_fixThresh)
{
z = 0f;
float span1 = (float)Math.Atan2(y, x);
if (span1 > m_swingSpan1)
{
x = (float)Math.Cos(m_swingSpan1);
y = (float)Math.Sin(m_swingSpan1);
}
else if (span1 < -m_swingSpan1)
{
x = (float)Math.Cos(m_swingSpan1);
y = -(float)Math.Sin(m_swingSpan1);
}
}
}
}
target.X = x * ivA.X + y * jvA.X + z * kvA.X;
target.Y = x * ivA.Y + y * jvA.Y + z * kvA.Y;
target.Z = x * ivA.Z + y * jvA.Z + z * kvA.Z;
target.Normalize();
m_swingAxis = -(ivB.Cross(ref target));
m_swingCorrection = m_swingAxis.Length();
m_swingAxis.Normalize();
}
}
if (m_twistSpan >= 0f)
{
IndexedVector3 twistAxis;
ComputeTwistLimitInfo(ref qABTwist, out m_twistAngle, out twistAxis);
if (m_twistAngle > m_twistSpan * m_limitSoftness)
{
m_solveTwistLimit = true;
m_twistLimitRatio = 1f;
if (m_twistAngle < m_twistSpan && m_limitSoftness < 1f - MathUtil.SIMD_EPSILON)
{
m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness) /
(m_twistSpan - m_twistSpan * m_limitSoftness);
}
// twist correction tries to get back to soft limit
m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
m_twistAxis = MathUtil.QuatRotate(qB, -twistAxis);
m_kTwist = 1f /
(ComputeAngularImpulseDenominator(ref m_twistAxis, ref invInertiaWorldA) +
ComputeAngularImpulseDenominator(ref m_twistAxis, ref invInertiaWorldB));
}
if (m_solveSwingLimit)
{
m_twistAxisA = MathUtil.QuatRotate(qA, -twistAxis);
}
}
else
{
m_twistAngle = 0f;
}
}
}
public IndexedMatrix(Matrix m)
{
_origin = new IndexedVector3(m.Translation);
//_basis = new IndexedBasisMatrix(new IndexedVector3(m.Right), new IndexedVector3(m.Up), new IndexedVector3(m.Backward)).Transpose();
_basis = new IndexedBasisMatrix(new IndexedVector3(m.Right), new IndexedVector3(m.Up), new IndexedVector3(m.Backward));
}
public IndexedBasisMatrix TransposeTimes(ref IndexedBasisMatrix m)
{
return new IndexedBasisMatrix(
_el0.X * m._el0.X + _el1.X * m._el1.X + _el2.X * m._el2.X,
_el0.X * m._el0.Y + _el1.X * m._el1.Y + _el2.X * m._el2.Y,
_el0.X * m._el0.Z + _el1.X * m._el1.Z + _el2.X * m._el2.Z,
_el0.Y * m._el0.X + _el1.Y * m._el1.X + _el2.Y * m._el2.X,
_el0.Y * m._el0.Y + _el1.Y * m._el1.Y + _el2.Y * m._el2.Y,
_el0.Y * m._el0.Z + _el1.Y * m._el1.Z + _el2.Y * m._el2.Z,
_el0.Z * m._el0.X + _el1.Z * m._el1.X + _el2.Z * m._el2.X,
_el0.Z * m._el0.Y + _el1.Z * m._el1.Y + _el2.Z * m._el2.Y,
_el0.Z * m._el0.Z + _el1.Z * m._el1.Z + _el2.Z * m._el2.Z);
}
public static IndexedQuaternion GetRotation(ref IndexedBasisMatrix a)
{
return a.GetRotation();
}
public static void Multiply(ref IndexedVector3 vout, ref IndexedVector3 vin, ref IndexedBasisMatrix m)
{
vout = new IndexedVector3(m.TDotX(ref vin), m.TDotY(ref vin), m.TDotZ(ref vin));
}
public IndexedBasisMatrix TimesTranspose(IndexedBasisMatrix m)
{
return new IndexedBasisMatrix(
_el0.Dot(m._el0), _el0.Dot(m._el1), _el0.Dot(m._el2),
_el1.Dot(m._el0), _el1.Dot(m._el1), _el1.Dot(m._el2),
_el2.Dot(m._el0), _el2.Dot(m._el1), _el2.Dot(m._el2));
}
public static void Multiply(ref IndexedVector3 vout, ref IndexedBasisMatrix m, ref IndexedVector3 v)
{
vout = new IndexedVector3(m._el0.X * v.X + m._el0.Y * v.Y + m._el0.Z * v.Z,
m._el1.X * v.X + m._el1.Y * v.Y + m._el1.Z * v.Z,
m._el2.X * v.X + m._el2.Y * v.Y + m._el2.Z * v.Z);
}
public void UpdateWheelTransform(int wheelIndex, bool interpolatedTransform)
{
WheelInfo wheel = m_wheelInfo[ wheelIndex ];
UpdateWheelTransformsWS(wheel,interpolatedTransform);
IndexedVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
IndexedVector3 right = wheel.m_raycastInfo.m_wheelAxleWS;
IndexedVector3 fwd = IndexedVector3.Cross(up,right);
fwd.Normalize();
// up = right.cross(fwd);
// up.normalize();
//rotate around steering over de wheelAxleWS
float steering = wheel.m_steering;
IndexedQuaternion steeringOrn = new IndexedQuaternion(up,steering);//wheel.m_steering);
IndexedBasisMatrix steeringMat = new IndexedBasisMatrix(ref steeringOrn);
IndexedQuaternion rotatingOrn = new IndexedQuaternion(right, -wheel.m_rotation);
IndexedBasisMatrix rotatingMat = new IndexedBasisMatrix(ref rotatingOrn);
IndexedBasisMatrix basis2 = new IndexedBasisMatrix(
right.X,fwd.X,up.X,
right.Y,fwd.Y,up.Y,
right.Z,fwd.Z,up.Z
);
// FIXME MAN - MATRIX ORDER
//wheel.m_worldTransform = steeringMat * rotatingMat * basis2;
wheel.m_worldTransform._basis = steeringMat * rotatingMat * basis2;
wheel.m_worldTransform._origin =
wheel.m_raycastInfo.m_hardPointWS + (wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength);
}
public void CalcAngleInfo2(IndexedMatrix transA, IndexedMatrix transB, IndexedBasisMatrix invInertiaWorldA, IndexedBasisMatrix invInertiaWorldB)
{
CalcAngleInfo2(ref transA, ref transB, ref invInertiaWorldA, ref invInertiaWorldB);
}
public virtual void InternalProcessTriangleIndex(IndexedVector3[] triangle, int partId, int triangleIndex)
{
IndexedBasisMatrix i = new IndexedBasisMatrix();
IndexedVector3 a = triangle[0] - m_center;
IndexedVector3 b = triangle[1] - m_center;
IndexedVector3 c = triangle[2] - m_center;
float volNeg = -Math.Abs(a.Triple(ref b, ref c)) * (1.0f / 6.0f);
for (int j = 0; j < 3; j++)
{
for (int k = 0; k <= j; k++)
{
i[j,k] = i[k,j] = volNeg * (0.1f * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k])
+ 0.05f * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j]));
}
}
float i00 = -i._el0.X;
float i11 = -i._el1.Y;
float i22 = -i._el2.Z;
i[0,0] = i11 + i22;
i[1,1] = i22 + i00;
i[2,2] = i00 + i11;
m_sum._el0 += i._el0;
m_sum._el1 += i._el1;
m_sum._el2 += i._el2;
}
public IndexedMatrix(float m11, float m12, float m13, float m21, float m22, float m23, float m31, float m32, float m33, float m41, float m42, float m43)
{
_basis = new IndexedBasisMatrix(m11, m12, m13, m21, m22, m23, m31, m32, m33);
_origin = new IndexedVector3(m41, m42, m43);
}
public static void GetRotation(ref IndexedBasisMatrix a, out IndexedQuaternion rot)
{
rot = a.GetRotation();
}
public static IndexedBasisMatrix CreateFromAxisAngle(IndexedVector3 axis, float angle)
{
float num1 = axis.X;
float num2 = axis.Y;
float num3 = axis.Z;
float num4 = (float)Math.Sin((double)angle);
float num5 = (float)Math.Cos((double)angle);
float num6 = num1 * num1;
float num7 = num2 * num2;
float num8 = num3 * num3;
float num9 = num1 * num2;
float num10 = num1 * num3;
float num11 = num2 * num3;
IndexedBasisMatrix ibm = new IndexedBasisMatrix(num6 + num5 * (1f - num6),
(float)((double)num9 - (double)num5 * (double)num9 + (double)num4 * (double)num3),
(float)((double)num10 - (double)num5 * (double)num10 - (double)num4 * (double)num2),
(float)((double)num9 - (double)num5 * (double)num9 - (double)num4 * (double)num3),
num7 + num5 * (1f - num7),
(float)((double)num11 - (double)num5 * (double)num11 + (double)num4 * (double)num1),
(float)((double)num10 - (double)num5 * (double)num10 + (double)num4 * (double)num2),
(float)((double)num11 - (double)num5 * (double)num11 - (double)num4 * (double)num1),
num8 + num5 * (1f - num8));
return ibm;
}
public IndexedMatrix Inverse()
{
IndexedBasisMatrix inv = _basis.Transpose();
return(new IndexedMatrix(inv, inv * -_origin));
}
public virtual void ProcessTriangle(IndexedVector3[] triangle, int partId, int triangleIndex)
{
//skip self-collisions
if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))
{
return;
}
//skip duplicates (disabled for now)
//if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex))
// return;
//search for shared vertices and edges
int numshared = 0;
int[] sharedVertsA = new int[] { -1, -1, -1 };
int[] sharedVertsB = new int[] { -1, -1, -1 };
///skip degenerate triangles
float crossBSqr = IndexedVector3.Cross((triangle[1] - triangle[0]), (triangle[2] - triangle[0])).LengthSquared();
if (crossBSqr < m_triangleInfoMap.m_equalVertexThreshold)
{
return;
}
float crossASqr = IndexedVector3.Cross((m_triangleVerticesA[1] - m_triangleVerticesA[0]), (m_triangleVerticesA[2] - m_triangleVerticesA[0])).LengthSquared();
///skip degenerate triangles
if (crossASqr < m_triangleInfoMap.m_equalVertexThreshold)
{
return;
}
#if false
printf("triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n",
m_triangleVerticesA[0].GetX(),m_triangleVerticesA[0].GetY(),m_triangleVerticesA[0].GetZ(),
m_triangleVerticesA[1].GetX(),m_triangleVerticesA[1].GetY(),m_triangleVerticesA[1].GetZ(),
m_triangleVerticesA[2].GetX(),m_triangleVerticesA[2].GetY(),m_triangleVerticesA[2].GetZ());
printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex);
printf("triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n",
triangle[0].GetX(),triangle[0].GetY(),triangle[0].GetZ(),
triangle[1].GetX(),triangle[1].GetY(),triangle[1].GetZ(),
triangle[2].GetX(),triangle[2].GetY(),triangle[2].GetZ());
#endif
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
if ((m_triangleVerticesA[i] - triangle[j]).LengthSquared() < m_triangleInfoMap.m_equalVertexThreshold)
{
sharedVertsA[numshared] = i;
sharedVertsB[numshared] = j;
numshared++;
///degenerate case
if (numshared >= 3)
{
return;
}
}
}
///degenerate case
if (numshared >= 3)
{
return;
}
}
switch (numshared)
{
case 0:
{
break;
}
case 1:
{
//shared vertex
break;
}
case 2:
{
//shared edge
//we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct
if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
{
sharedVertsA[0] = 2;
sharedVertsA[1] = 0;
int tmp = sharedVertsB[1];
sharedVertsB[1] = sharedVertsB[0];
sharedVertsB[0] = tmp;
}
int hash = GetHash(m_partIdA, m_triangleIndexA);
TriangleInfo info = null;
if (m_triangleInfoMap.ContainsKey(hash))
{
info = m_triangleInfoMap[hash];
}
else
{
info = new TriangleInfo();
m_triangleInfoMap[hash] = info;
}
int sumvertsA = sharedVertsA[0] + sharedVertsA[1];
int otherIndexA = 3 - sumvertsA;
IndexedVector3 edge = new IndexedVector3(m_triangleVerticesA[sharedVertsA[1]] - m_triangleVerticesA[sharedVertsA[0]]);
TriangleShape tA = new TriangleShape(m_triangleVerticesA[0], m_triangleVerticesA[1], m_triangleVerticesA[2]);
int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]);
TriangleShape tB = new TriangleShape(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]);
//btTriangleShape tB(triangle[0],triangle[1],triangle[2]);
IndexedVector3 normalA;
IndexedVector3 normalB;
tA.CalcNormal(out normalA);
tB.CalcNormal(out normalB);
edge.Normalize();
IndexedVector3 edgeCrossA = IndexedVector3.Normalize(IndexedVector3.Cross(edge, normalA));
{
IndexedVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]];
if (IndexedVector3.Dot(edgeCrossA, tmp) < 0)
{
edgeCrossA *= -1;
}
}
IndexedVector3 edgeCrossB = IndexedVector3.Cross(edge, normalB).Normalized();
{
IndexedVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]];
if (IndexedVector3.Dot(edgeCrossB, tmp) < 0)
{
edgeCrossB *= -1;
}
}
float angle2 = 0;
float ang4 = 0.0f;
IndexedVector3 calculatedEdge = IndexedVector3.Cross(edgeCrossA, edgeCrossB);
float len2 = calculatedEdge.LengthSquared();
float correctedAngle = 0f;
IndexedVector3 calculatedNormalB = normalA;
bool isConvex = false;
if (len2 < m_triangleInfoMap.m_planarEpsilon)
{
angle2 = 0.0f;
ang4 = 0.0f;
}
else
{
calculatedEdge.Normalize();
IndexedVector3 calculatedNormalA = IndexedVector3.Cross(calculatedEdge, edgeCrossA);
calculatedNormalA.Normalize();
angle2 = GetAngle(ref calculatedNormalA, ref edgeCrossA, ref edgeCrossB);
ang4 = MathUtil.SIMD_PI - angle2;
float dotA = IndexedVector3.Dot(normalA, edgeCrossB);
///@todo: check if we need some epsilon, due to floating point imprecision
isConvex = (dotA < 0f);
correctedAngle = isConvex ? ang4 : -ang4;
IndexedQuaternion orn2 = new IndexedQuaternion(calculatedEdge, -correctedAngle);
IndexedMatrix rotateMatrix = IndexedMatrix.CreateFromQuaternion(orn2);
calculatedNormalB = new IndexedBasisMatrix(orn2) * normalA;
}
//alternatively use
//IndexedVector3 calculatedNormalB2 = quatRotate(orn,normalA);
switch (sumvertsA)
{
case 1:
{
IndexedVector3 edge1 = m_triangleVerticesA[0] - m_triangleVerticesA[1];
IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA);
float bla = IndexedVector3.Dot(computedNormalB, normalB);
if (bla < 0)
{
computedNormalB *= -1;
info.m_flags |= TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB;
}
#if DEBUG_INTERNAL_EDGE
if ((computedNormalB - normalB).Length() > 0.0001f)
{
System.Console.WriteLine("warning: normals not identical");
}
#endif//DEBUG_INTERNAL_EDGE
info.m_edgeV0V1Angle = -correctedAngle;
if (isConvex)
{
info.m_flags |= TriangleInfoMap.TRI_INFO_V0V1_CONVEX;
}
break;
}
case 2:
{
IndexedVector3 edge1 = m_triangleVerticesA[2] - m_triangleVerticesA[0];
IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA);
if (IndexedVector3.Dot(computedNormalB, normalB) < 0)
{
computedNormalB *= -1;
info.m_flags |= TriangleInfoMap.TRI_INFO_V2V0_SWAP_NORMALB;
}
#if DEBUG_INTERNAL_EDGE
if ((computedNormalB - normalB).Length() > 0.0001)
{
System.Console.WriteLine("warning: normals not identical");
}
#endif //DEBUG_INTERNAL_EDGE
info.m_edgeV2V0Angle = -correctedAngle;
if (isConvex)
info.m_flags |= TriangleInfoMap.TRI_INFO_V2V0_CONVEX;
break;
}
case 3:
{
IndexedVector3 edge1 = m_triangleVerticesA[1] - m_triangleVerticesA[2];
IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA);
if (IndexedVector3.Dot(computedNormalB, normalB) < 0)
{
info.m_flags |= TriangleInfoMap.TRI_INFO_V1V2_SWAP_NORMALB;
computedNormalB *= -1;
}
#if DEBUG_INTERNAL_EDGE
if ((computedNormalB - normalB).Length() > 0.0001)
{
System.Console.WriteLine("warning: normals not identical");
}
#endif //DEBUG_INTERNAL_EDGE
info.m_edgeV1V2Angle = -correctedAngle;
if (isConvex)
{
info.m_flags |= TriangleInfoMap.TRI_INFO_V1V2_CONVEX;
}
break;
}
}
break;
}
default:
{
// printf("warning: duplicate triangle\n");
break;
}
}
}
public IndexedMatrix(IndexedBasisMatrix basis, IndexedVector3 origin)
{
_basis = basis;
_origin = origin;
}
public static IndexedBasisMatrix Transpose(IndexedBasisMatrix IndexedMatrix)
{
return new IndexedBasisMatrix(IndexedMatrix._el0.X, IndexedMatrix._el1.X, IndexedMatrix._el2.X,
IndexedMatrix._el0.Y, IndexedMatrix._el1.Y, IndexedMatrix._el2.Y,
IndexedMatrix._el0.Z, IndexedMatrix._el1.Z, IndexedMatrix._el2.Z);
}
public static IndexedBasisMatrix Transpose(IndexedBasisMatrix IndexedMatrix)
{
return(new IndexedBasisMatrix(IndexedMatrix._Row0.X, IndexedMatrix._Row1.X, IndexedMatrix._Row2.X,
IndexedMatrix._Row0.Y, IndexedMatrix._Row1.Y, IndexedMatrix._Row2.Y,
IndexedMatrix._Row0.Z, IndexedMatrix._Row1.Z, IndexedMatrix._Row2.Z));
}
public static void Multiply(ref IndexedVector3 vout, ref IndexedVector3 vin, ref IndexedBasisMatrix m)
{
vout = new IndexedVector3(m.TDotX(ref vin), m.TDotY(ref vin), m.TDotZ(ref vin));
}
public static void Transpose(ref IndexedBasisMatrix IndexedMatrix, out IndexedBasisMatrix result)
{
result = new IndexedBasisMatrix(IndexedMatrix._el0.X, IndexedMatrix._el1.X, IndexedMatrix._el2.X,
IndexedMatrix._el0.Y, IndexedMatrix._el1.Y, IndexedMatrix._el2.Y,
IndexedMatrix._el0.Z, IndexedMatrix._el1.Z, IndexedMatrix._el2.Z);
}
public static void CreateRotationZ(float radians, out IndexedMatrix result)
{
result._basis = IndexedBasisMatrix.CreateRotationZ(radians);
result._origin = new IndexedVector3(0, 0, 0);
}
//! Calc the transformation relative 1 to 0. Inverts matrics by transposing
public void CalcFromHomogenic(ref IndexedMatrix trans0, ref IndexedMatrix trans1)
{
IndexedMatrix temp_trans = trans0.Inverse();
temp_trans = temp_trans * trans1;
m_T1to0 = temp_trans._origin;
m_R1to0 = temp_trans._basis;
CalcAbsoluteMatrix();
}
public static void Multiply(ref IndexedVector3 vout, ref IndexedBasisMatrix m, ref IndexedVector3 v)
{
vout = new IndexedVector3(m._el0.X * v.X + m._el0.Y * v.Y + m._el0.Z * v.Z,
m._el1.X * v.X + m._el1.Y * v.Y + m._el1.Z * v.Z,
m._el2.X * v.X + m._el2.Y * v.Y + m._el2.Z * v.Z);
}
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedBasisMatrix invInertiaWorldA, IndexedBasisMatrix invInertiaWorldB)
{
CalcAngleInfo2(ref transA, ref transB, ref invInertiaWorldA, ref invInertiaWorldB);
Debug.Assert(!m_useSolveConstraintObsolete);
// set jacobian
info.m_solverConstraints[0].m_contactNormal.X = 1f;
info.m_solverConstraints[1].m_contactNormal.Y = 1f;
info.m_solverConstraints[2].m_contactNormal.Z = 1f;
IndexedVector3 a1 = transA._basis * m_rbAFrame._origin;
{
IndexedVector3 a1neg = -a1;
MathUtil.GetSkewSymmetricMatrix(ref a1neg,
out info.m_solverConstraints[0].m_relpos1CrossNormal,
out info.m_solverConstraints[1].m_relpos1CrossNormal,
out info.m_solverConstraints[2].m_relpos1CrossNormal);
}
IndexedVector3 a2 = transB._basis * m_rbBFrame._origin;
{
MathUtil.GetSkewSymmetricMatrix(ref a2,
out info.m_solverConstraints[0].m_relpos2CrossNormal,
out info.m_solverConstraints[1].m_relpos2CrossNormal,
out info.m_solverConstraints[2].m_relpos2CrossNormal);
}
// set right hand side
float linERP = ((m_flags & (int)ConeTwistFlags.BT_CONETWIST_FLAGS_LIN_ERP) != 0) ? m_linERP : info.erp;
float k = info.fps * linERP;
for (int j = 0; j < 3; j++)
{
info.m_solverConstraints[j].m_rhs = k * (a2[j] + transB._origin[j] - a1[j] - transA._origin[j]);
info.m_solverConstraints[j].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[j].m_upperLimit = MathUtil.SIMD_INFINITY;
if ((m_flags & (int)ConeTwistFlags.BT_CONETWIST_FLAGS_LIN_CFM) != 0)
{
info.m_solverConstraints[j].m_cfm = m_linCFM;
}
}
int row = 3;
IndexedVector3 ax1;
// angular limits
if (m_solveSwingLimit)
{
if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
{
IndexedMatrix trA = transA * m_rbAFrame;
IndexedVector3 p = trA._basis.GetColumn(1);
IndexedVector3 q = trA._basis.GetColumn(2);
info.m_solverConstraints[row].m_relpos1CrossNormal = p;
info.m_solverConstraints[row + 1].m_relpos1CrossNormal = q;
info.m_solverConstraints[row].m_relpos2CrossNormal = -p;
info.m_solverConstraints[row + 1].m_relpos2CrossNormal = -q;
float fact = info.fps * m_relaxationFactor;
info.m_solverConstraints[row].m_rhs = fact * m_swingAxis.Dot(ref p);
info.m_solverConstraints[row + 1].m_rhs = fact * m_swingAxis.Dot(ref q);
info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row + 1].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row + 1].m_upperLimit = MathUtil.SIMD_INFINITY;
row += 2;
}
else
{
ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
info.m_solverConstraints[row].m_relpos1CrossNormal = ax1;
info.m_solverConstraints[row].m_relpos2CrossNormal = -ax1;
float k1 = info.fps * m_biasFactor;
info.m_solverConstraints[row].m_rhs = k1 * m_swingCorrection;
if ((m_flags & (int)ConeTwistFlags.BT_CONETWIST_FLAGS_ANG_CFM) != 0)
{
info.m_solverConstraints[row].m_cfm = m_angCFM;
}
// m_swingCorrection is always positive or 0
info.m_solverConstraints[row].m_lowerLimit = 0;
info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY;
++row;
}
}
if (m_solveTwistLimit)
{
ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
info.m_solverConstraints[row].m_relpos1CrossNormal = ax1;
info.m_solverConstraints[row].m_relpos2CrossNormal = -ax1;
float k1 = info.fps * m_biasFactor;
info.m_solverConstraints[row].m_rhs = k1 * m_twistCorrection;
if ((m_flags & (int)ConeTwistFlags.BT_CONETWIST_FLAGS_ANG_CFM) != 0)
{
info.m_solverConstraints[row].m_cfm = m_angCFM;
}
if (m_twistSpan > 0.0f)
{
if (m_twistCorrection > 0.0f)
{
info.m_solverConstraints[row].m_lowerLimit = 0;
info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else
{
info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row].m_upperLimit = 0;
}
}
else
{
info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY;
}
++row;
}
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints)
{
//PrintInfo2(BulletGlobals.g_streamWriter, this, info);
}
}
public static void CreateRotationZ(float radians, out IndexedBasisMatrix _basis)
{
float num1 = (float)Math.Cos((double)radians);
float num2 = (float)Math.Sin((double)radians);
_basis._el0 = new IndexedVector3(num1, num2, 0);
_basis._el1 = new IndexedVector3(-num2, num1, 0);
_basis._el2 = new IndexedVector3(0, 0, 1);
}
public static void CreateScale(float scale, out IndexedMatrix result)
{
result = IndexedMatrix.Identity;
result._basis = IndexedBasisMatrix.CreateScale(new IndexedVector3(scale));
}
//
// solve unilateral raint (equality, direct method)
//
public void ResolveUnilateralPairConstraint(RigidBody body0, RigidBody body1, ref IndexedBasisMatrix world2A,
ref IndexedBasisMatrix world2B,
ref IndexedVector3 invInertiaADiag,
float invMassA,
ref IndexedVector3 linvelA, ref IndexedVector3 angvelA,
ref IndexedVector3 rel_posA1,
ref IndexedVector3 invInertiaBDiag,
float invMassB,
ref IndexedVector3 linvelB, ref IndexedVector3 angvelB,
ref IndexedVector3 rel_posA2,
float depthA, ref IndexedVector3 normalA,
ref IndexedVector3 rel_posB1, ref IndexedVector3 rel_posB2,
float depthB, ref IndexedVector3 normalB,
out float imp0, out float imp1)
{
//(void)linvelA;
//(void)linvelB;
//(void)angvelB;
//(void)angvelA;
imp0 = 0f;
imp1 = 0f;
float len = Math.Abs(normalA.Length()) - 1f;
if (Math.Abs(len) >= MathUtil.SIMD_EPSILON)
return;
Debug.Assert(len < MathUtil.SIMD_EPSILON);
//this jacobian entry could be re-used for all iterations
JacobianEntry jacA = new JacobianEntry(ref world2A,ref world2B,ref rel_posA1,ref rel_posA2,ref normalA,ref invInertiaADiag,invMassA,
ref invInertiaBDiag,invMassB);
JacobianEntry jacB = new JacobianEntry(ref world2A,ref world2B,ref rel_posB1,ref rel_posB2,ref normalB,ref invInertiaADiag,invMassA,
ref invInertiaBDiag,invMassB);
// float vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
// float vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
float vel0 = IndexedVector3.Dot(normalA,(body0.GetVelocityInLocalPoint(ref rel_posA1)-body1.GetVelocityInLocalPoint(ref rel_posA1)));
float vel1 = IndexedVector3.Dot(normalB,(body0.GetVelocityInLocalPoint(ref rel_posB1)-body1.GetVelocityInLocalPoint(ref rel_posB1)));
// float penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
float massTerm = 1f / (invMassA + invMassB);
// calculate rhs (or error) terms
float dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
float dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
// dC/dv * dv = -C
// jacobian * impulse = -error
//
//impulse = jacobianInverse * -error
// inverting 2x2 symmetric system (offdiagonal are equal!)
//
float nonDiag = jacA.GetNonDiagonal(jacB,invMassA,invMassB);
float invDet = 1f / (jacA.GetDiagonal() * jacB.GetDiagonal() - nonDiag * nonDiag );
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
imp0 = dv0 * jacA.GetDiagonal() * invDet + dv1 * -nonDiag * invDet;
imp1 = dv1 * jacB.GetDiagonal() * invDet + dv0 * - nonDiag * invDet;
//[a b] [d -c]
//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
//[jA nD] * [imp0] = [dv0]
//[nD jB] [imp1] [dv1]
}
public static bool ClampNormal(ref IndexedVector3 edge, ref IndexedVector3 tri_normal_org, ref IndexedVector3 localContactNormalOnB, float correctedEdgeAngle, out IndexedVector3 clampedLocalNormal)
{
IndexedVector3 tri_normal = tri_normal_org;
//we only have a local triangle normal, not a local contact normal . only normal in world space...
//either compute the current angle all in local space, or all in world space
IndexedVector3 edgeCross = IndexedVector3.Cross(edge, tri_normal).Normalized();
float curAngle = GetAngle(ref edgeCross, ref tri_normal, ref localContactNormalOnB);
if (correctedEdgeAngle < 0)
{
if (curAngle < correctedEdgeAngle)
{
float diffAngle = correctedEdgeAngle - curAngle;
IndexedQuaternion rotation = new IndexedQuaternion(edge, diffAngle);
clampedLocalNormal = new IndexedBasisMatrix(rotation) * localContactNormalOnB;
return true;
}
}
if (correctedEdgeAngle >= 0)
{
if (curAngle > correctedEdgeAngle)
{
float diffAngle = correctedEdgeAngle - curAngle;
IndexedQuaternion rotation = new IndexedQuaternion(edge, diffAngle);
clampedLocalNormal = new IndexedBasisMatrix(rotation) * localContactNormalOnB;
return true;
}
}
clampedLocalNormal = IndexedVector3.Zero;
return false;
}
public void UpdateInertiaTensor()
{
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugRigidBody)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("[{0}] RigidBody updateInertiaTensor",(String)m_userObjectPointer));
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "invInertiaLocal", m_invInertiaLocal);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, m_worldTransform);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, m_worldTransform._basis.Scaled(ref m_invInertiaLocal));
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, m_worldTransform._basis.Transpose());
}
#endif
m_invInertiaTensorWorld = m_worldTransform._basis.Scaled(ref m_invInertiaLocal) * m_worldTransform._basis.Transpose();
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugRigidBody)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter,m_invInertiaTensorWorld);
}
#endif
}
///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia
///and the center of mass to the current coordinate system. "masses" points to an array of masses of the children. The resulting transform
///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound
///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
///of the collision object by the principal transform.
public void CalculatePrincipalAxisTransform(IList<float> masses, ref IndexedMatrix principal, out IndexedVector3 inertia)
{
int n = m_children.Count;
float totalMass = 0;
IndexedVector3 center = IndexedVector3.Zero;
for (int k = 0; k < n; k++)
{
Debug.Assert(masses[k] > 0f);
center += m_children[k].m_transform._origin * masses[k];
totalMass += masses[k];
}
Debug.Assert(totalMass > 0f);
center /= totalMass;
principal._origin = center;
IndexedBasisMatrix tensor = new IndexedBasisMatrix();
for (int k = 0; k < n; k++)
{
IndexedVector3 i;
m_children[k].m_childShape.CalculateLocalInertia(masses[k], out i);
IndexedMatrix t = m_children[k].m_transform;
IndexedVector3 o = t._origin - center;
//compute inertia tensor in coordinate system of compound shape
IndexedBasisMatrix j = t._basis.Transpose();
j._el0 *= i.X;
j._el1 *= i.Y;
j._el2 *= i.Z;
j = t._basis * j;
//add inertia tensor
tensor._el0 += j._el0;
tensor._el1 += j._el1;
tensor._el2 += j._el2;
//tensor += j;
//compute inertia tensor of pointmass at o
float o2 = o.LengthSquared();
j._el0 = new IndexedVector3(o2, 0, 0);
j._el1 = new IndexedVector3(0, o2, 0);
j._el2 = new IndexedVector3(0, 0, o2);
j._el0 += o * -o.X;
j._el1 += o * -o.Y;
j._el2 += o * -o.Z;
//add inertia tensor of pointmass
tensor._el0 += masses[k] * j._el0;
tensor._el1 += masses[k] * j._el1;
tensor._el2 += masses[k] * j._el2;
}
tensor.Diagonalize(out principal, 0.00001f, 20);
inertia = new IndexedVector3(tensor._el0.X, tensor._el1.Y, tensor._el2.Z);
}
public static void CreateScale(ref IndexedVector3 scales, out IndexedMatrix result)
{
result = IndexedMatrix.Identity;
result._basis = IndexedBasisMatrix.CreateScale(scales);
}