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 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 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 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); }