public static TSMatrix Multiply(TSMatrix matrix1, long scaleFactor) { TSMatrix result; TSMatrix.Multiply(ref matrix1, scaleFactor, out result); return(result); }
public static TSMatrix operator -(TSMatrix value1, TSMatrix value2) { TSMatrix result; TSMatrix.Multiply(ref value2, -FP.One, out value2); TSMatrix.Add(ref value1, ref value2, out result); return(result); }
public static TSMatrix Multiply(TSMatrix matrix1, TSMatrix matrix2) { TSMatrix result; TSMatrix.Multiply(ref matrix1, ref matrix2, out result); return(result); }
public static TSMatrix operator *(TSMatrix value1, TSMatrix value2) { TSMatrix result; TSMatrix.Multiply(ref value1, ref value2, out result); return(result); }
public virtual void Update() { bool flag = this.isParticle; if (flag) { this.inertia = TSMatrix.Zero; this.invInertia = (this.invInertiaWorld = TSMatrix.Zero); this.invOrientation = (this.orientation = TSMatrix.Identity); this.boundingBox = this.shape.boundingBox; TSVector.Add(ref this.boundingBox.min, ref this.position, out this.boundingBox.min); TSVector.Add(ref this.boundingBox.max, ref this.position, out this.boundingBox.max); this.angularVelocity.MakeZero(); } else { TSMatrix.Transpose(ref this.orientation, out this.invOrientation); this.Shape.GetBoundingBox(ref this.orientation, out this.boundingBox); TSVector.Add(ref this.boundingBox.min, ref this.position, out this.boundingBox.min); TSVector.Add(ref this.boundingBox.max, ref this.position, out this.boundingBox.max); bool flag2 = !this.isStatic; if (flag2) { TSMatrix.Multiply(ref this.invOrientation, ref this.invInertia, out this.invInertiaWorld); TSMatrix.Multiply(ref this.invInertiaWorld, ref this.orientation, out this.invInertiaWorld); } } }
public static FP CalculateMassInertia(Shape shape, out TSVector centerOfMass, out TSMatrix inertia) { FP fP = FP.Zero; centerOfMass = TSVector.zero; inertia = TSMatrix.Zero; bool flag = shape is Multishape; if (flag) { throw new ArgumentException("Can't calculate inertia of multishapes.", "shape"); } List <TSVector> list = new List <TSVector>(); shape.MakeHull(ref list, 3); FP fP2 = FP.One / (60 * FP.One); FP fP3 = FP.One / (120 * FP.One); TSMatrix value = new TSMatrix(fP2, fP3, fP3, fP3, fP2, fP3, fP3, fP3, fP2); for (int i = 0; i < list.Count; i += 3) { TSVector tSVector = list[i]; TSVector tSVector2 = list[i + 1]; TSVector tSVector3 = list[i + 2]; TSMatrix tSMatrix = new TSMatrix(tSVector.x, tSVector2.x, tSVector3.x, tSVector.y, tSVector2.y, tSVector3.y, tSVector.z, tSVector2.z, tSVector3.z); FP fP4 = tSMatrix.Determinant(); TSMatrix value2 = TSMatrix.Multiply(tSMatrix * value * TSMatrix.Transpose(tSMatrix), fP4); TSVector value3 = FP.One / (4 * FP.One) * (list[i] + list[i + 1] + list[i + 2]); FP fP5 = FP.One / (6 * FP.One) * fP4; inertia += value2; centerOfMass += fP5 * value3; fP += fP5; } inertia = TSMatrix.Multiply(TSMatrix.Identity, inertia.Trace()) - inertia; centerOfMass *= FP.One / fP; FP x = centerOfMass.x; FP y = centerOfMass.y; FP z = centerOfMass.z; TSMatrix tSMatrix2 = new TSMatrix(-fP * (y * y + z * z), fP * x * y, fP * x * z, fP * y * x, -fP * (z * z + x * x), fP * y * z, fP * z * x, fP * z * y, -fP * (x * x + y * y)); TSMatrix.Add(ref inertia, ref tSMatrix2, out inertia); return(fP); }
public override void PrepareForIteration(FP timestep) { this.effectiveMass = this.body1.invInertiaWorld + this.body2.invInertiaWorld; this.softnessOverDt = this.softness / timestep; this.effectiveMass.M11 = this.effectiveMass.M11 + this.softnessOverDt; this.effectiveMass.M22 = this.effectiveMass.M22 + this.softnessOverDt; this.effectiveMass.M33 = this.effectiveMass.M33 + this.softnessOverDt; TSMatrix.Inverse(ref this.effectiveMass, out this.effectiveMass); TSMatrix value; TSMatrix.Multiply(ref this.initialOrientation1, ref this.initialOrientation2, out value); TSMatrix.Transpose(ref value, out value); TSMatrix tSMatrix = value * this.body2.invOrientation * this.body1.orientation; FP fP = tSMatrix.M32 - tSMatrix.M23; FP fP2 = tSMatrix.M13 - tSMatrix.M31; FP fP3 = tSMatrix.M21 - tSMatrix.M12; FP fP4 = TSMath.Sqrt(fP * fP + fP2 * fP2 + fP3 * fP3); FP x = tSMatrix.M11 + tSMatrix.M22 + tSMatrix.M33; FP value2 = FP.Atan2(fP4, x - 1); TSVector value3 = new TSVector(fP, fP2, fP3) * value2; bool flag = fP4 != FP.Zero; if (flag) { value3 *= FP.One / fP4; } this.bias = value3 * this.biasFactor * (-FP.One / timestep); bool flag2 = !this.body1.IsStatic; if (flag2) { this.body1.angularVelocity += TSVector.Transform(this.accumulatedImpulse, this.body1.invInertiaWorld); } bool flag3 = !this.body2.IsStatic; if (flag3) { this.body2.angularVelocity += TSVector.Transform(-FP.One * this.accumulatedImpulse, this.body2.invInertiaWorld); } }