Exemplo n.º 1
0
        public static TSMatrix Transpose(TSMatrix matrix)
        {
            TSMatrix result;

            TSMatrix.Transpose(ref matrix, out result);
            return(result);
        }
Exemplo n.º 2
0
        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);
                }
            }
        }
Exemplo n.º 3
0
 public TransformedShape(Shape shape, TSMatrix orientation, TSVector position)
 {
     this.position    = position;
     this.orientation = orientation;
     TSMatrix.Transpose(ref orientation, out this.invOrientation);
     this.shape       = shape;
     this.boundingBox = default(TSBBox);
     this.UpdateBoundingBox();
 }
Exemplo n.º 4
0
        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);
        }
Exemplo n.º 5
0
        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);
            }
        }