コード例 #1
0
        public void Prestep(float inverseDt)
        {
            //Given that we're collecting position, inverse mass, and inertia all at once, it makes no sense to store position separately from inversemass and inertia.
            //Since you should not expect the 4 involved bodies to be in memory *together*, the best you can do is to ensure that the set of values are together.
            //Otherwise you're multiplying cache misses for no reason!
            var inverseMassA = new Vector4(BodyA0.InverseMass, BodyA1.InverseMass, BodyA2.InverseMass, BodyA3.InverseMass);
            var inverseMassB = new Vector4(BodyB0.InverseMass, BodyB1.InverseMass, BodyB2.InverseMass, BodyB3.InverseMass);

            var inverseInertiaTensorA = new Matrix3x3Width4(ref BodyA0.InertiaTensorInverse, ref BodyA1.InertiaTensorInverse, ref BodyA2.InertiaTensorInverse, ref BodyA3.InertiaTensorInverse);
            var inverseInertiaTensorB = new Matrix3x3Width4(ref BodyB0.InertiaTensorInverse, ref BodyB1.InertiaTensorInverse, ref BodyB2.InertiaTensorInverse, ref BodyB3.InertiaTensorInverse);

            Vector3Width4 positionA = new Vector3Width4(ref BodyA0.Position, ref BodyA1.Position, ref BodyA2.Position, ref BodyA3.Position);
            Vector3Width4 positionB = new Vector3Width4(ref BodyA0.Position, ref BodyB1.Position, ref BodyB2.Position, ref BodyB3.Position);

            for (int i = 0; i < ConstraintCount; ++i)
            {
                Constraints[i].Prestep(inverseDt, ref inverseMassA, ref inverseMassB, ref inverseInertiaTensorA, ref inverseInertiaTensorB, ref positionA, ref positionB);
            }

            //for (int i = 0; i < 4; ++i)
            //{
            //    a.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);

            //}

            //a.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
            //b.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
            //c.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
            //d.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
        }
コード例 #2
0
        public void Prestep(float inverseDt)
        {
            //Given that we're collecting position, inverse mass, and inertia all at once, it makes no sense to store position separately from inversemass and inertia.
            //Since you should not expect the 4 involved bodies to be in memory *together*, the best you can do is to ensure that the set of values are together.
            //Otherwise you're multiplying cache misses for no reason!
            var inverseMassA = new Vector4(BodyA0.InverseMass, BodyA1.InverseMass, BodyA2.InverseMass, BodyA3.InverseMass);
            var inverseMassB = new Vector4(BodyB0.InverseMass, BodyB1.InverseMass, BodyB2.InverseMass, BodyB3.InverseMass);

            var inverseInertiaTensorA = new Matrix3x3Width4(ref BodyA0.InertiaTensorInverse, ref BodyA1.InertiaTensorInverse, ref BodyA2.InertiaTensorInverse, ref BodyA3.InertiaTensorInverse);
            var inverseInertiaTensorB = new Matrix3x3Width4(ref BodyB0.InertiaTensorInverse, ref BodyB1.InertiaTensorInverse, ref BodyB2.InertiaTensorInverse, ref BodyB3.InertiaTensorInverse);


            Vector3Width4 positionA = new Vector3Width4(ref BodyA0.Position, ref BodyA1.Position, ref BodyA2.Position, ref BodyA3.Position);
            Vector3Width4 positionB = new Vector3Width4(ref BodyA0.Position, ref BodyB1.Position, ref BodyB2.Position, ref BodyB3.Position);

            for (int i = 0; i < ConstraintCount; ++i)
            {
                Constraints[i].Prestep(inverseDt, ref inverseMassA, ref inverseMassB, ref inverseInertiaTensorA, ref inverseInertiaTensorB, ref positionA, ref positionB);
            }

            //for (int i = 0; i < 4; ++i)
            //{
            //    a.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);

            //}

            //a.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
            //b.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
            //c.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
            //d.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
        }
コード例 #3
0
        public void Prestep(float inverseDt)
        {
            //C = dot(Pa - Pb, N) > 0
            //Jacobians:
            //LinearA: N
            //AngularA: cross(OffsetPa, N)
            //LinearB: -N
            //AngularB: -cross(OffsetPb, N)

            //var positionA = new Vector3Width4();
            //var positionB = new Vector3Width4();

            //Given that we're collecting position, inverse mass, and inertia all at once, it makes no sense to store position separately from inversemass and inertia.
            //Since you should not expect the 4 involved bodies to be in memory *together*, the best you can do is to ensure that the set of values are together.
            //Otherwise you're multiplying cache misses for no reason!
            var InverseMassA = new Vector4(BodyA0.InverseMass, BodyA1.InverseMass, BodyA2.InverseMass, BodyA3.InverseMass);
            var InverseMassB = new Vector4(BodyB0.InverseMass, BodyB1.InverseMass, BodyB2.InverseMass, BodyB3.InverseMass);

            var InverseInertiaTensorA = new Matrix3x3Width4(ref BodyA0.InertiaTensorInverse, ref BodyA1.InertiaTensorInverse, ref BodyA2.InertiaTensorInverse, ref BodyA3.InertiaTensorInverse);
            var InverseInertiaTensorB = new Matrix3x3Width4(ref BodyB0.InertiaTensorInverse, ref BodyB1.InertiaTensorInverse, ref BodyB2.InertiaTensorInverse, ref BodyB3.InertiaTensorInverse);

            Vector3Width4 positionA = new Vector3Width4(ref BodyA0.Position, ref BodyA1.Position, ref BodyA2.Position, ref BodyA3.Position);
            Vector3Width4 positionB = new Vector3Width4(ref BodyA0.Position, ref BodyB1.Position, ref BodyB2.Position, ref BodyB3.Position);


            LinearJacobianA = ContactNormal;
            Vector3Width4.Negate(ref ContactNormal, out LinearJacobianB);

            Vector3Width4 offsetA, offsetB;

            Vector3Width4.Subtract(ref ContactPosition, ref positionA, out offsetA);
            Vector3Width4.Subtract(ref ContactPosition, ref positionB, out offsetB);

            Vector3Width4.Cross(ref offsetA, ref ContactNormal, out AngularJacobianA);
            Vector3Width4.Cross(ref ContactNormal, ref offsetB, out AngularJacobianB);// note negation->parameter reverse


            //Allow velocity that closes a gap, and apply penetration correction against positive depth.
            //Bounciness not yet included.
            PenetrationBias = ContactPenetration * inverseDt;
            PenetrationBias = -Vector4.Min(Vector4.Min(PenetrationBias, PenetrationBias * 0.2f), new Vector4(0.2f));


            //The inertia tensor is in world space, so no jacobian transformation is required.
            Vector3Width4.Multiply(ref LinearJacobianA, ref InverseMassA, out LinearJacobianITA);
            Vector3Width4.Multiply(ref LinearJacobianB, ref InverseMassB, out LinearJacobianITB);
            Matrix3x3Width4.Transform(ref AngularJacobianA, ref InverseInertiaTensorA, out AngularJacobianITA);
            Matrix3x3Width4.Transform(ref AngularJacobianB, ref InverseInertiaTensorB, out AngularJacobianITB);
            Vector4 angularContributionA, angularContributionB;

            Vector3Width4.Dot(ref AngularJacobianITA, ref AngularJacobianITA, out angularContributionA);
            Vector3Width4.Dot(ref AngularJacobianITB, ref AngularJacobianITB, out angularContributionB);
            var inverseEffectiveMass = InverseMassA + InverseMassB + angularContributionA + angularContributionB;

            Vector4 CollisionSoftness = new Vector4(5);

            Softness      = CollisionSoftness * inverseEffectiveMass * inverseDt;
            EffectiveMass = Vector4.One / (Softness + inverseEffectiveMass);
        }
コード例 #4
0
        public void Prestep(float inverseDt,
                            ref Vector4 inverseMassA, ref Vector4 inverseMassB,
                            ref Matrix3x3Width4 inverseInertiaTensorA, ref Matrix3x3Width4 inverseInertiaTensorB,
                            ref Vector3Width4 positionA, ref Vector3Width4 positionB)
        {
            //C = dot(Pa - Pb, N) > 0
            //Jacobians:
            //LinearA: N
            //AngularA: cross(OffsetPa, N)
            //LinearB: -N
            //AngularB: -cross(OffsetPb, N)

            //var positionA = new Vector3Width4();
            //var positionB = new Vector3Width4();

            LinearJacobianA = ContactNormal;
            Vector3Width4.Negate(ref ContactNormal, out LinearJacobianB);

            Vector3Width4 offsetA, offsetB;

            Vector3Width4.Subtract(ref ContactPosition, ref positionA, out offsetA);
            Vector3Width4.Subtract(ref ContactPosition, ref positionB, out offsetB);

            Vector3Width4.Cross(ref offsetA, ref ContactNormal, out AngularJacobianA);
            Vector3Width4.Cross(ref ContactNormal, ref offsetB, out AngularJacobianB);// note negation->parameter reverse


            //Allow velocity that closes a gap, and apply penetration correction against positive depth.
            //Bounciness not yet included.
            PenetrationBias = ContactPenetration * inverseDt;
            PenetrationBias = -Vector4.Min(Vector4.Min(PenetrationBias, PenetrationBias * 0.2f), new Vector4(0.2f));


            //The inertia tensor is in world space, so no jacobian transformation is required.
            Vector3Width4.Multiply(ref LinearJacobianA, ref inverseMassA, out LinearJacobianITA);
            Vector3Width4.Multiply(ref LinearJacobianB, ref inverseMassB, out LinearJacobianITB);
            Matrix3x3Width4.Transform(ref AngularJacobianA, ref inverseInertiaTensorA, out AngularJacobianITA);
            Matrix3x3Width4.Transform(ref AngularJacobianB, ref inverseInertiaTensorB, out AngularJacobianITB);
            Vector4 angularContributionA, angularContributionB;

            Vector3Width4.Dot(ref AngularJacobianITA, ref AngularJacobianITA, out angularContributionA);
            Vector3Width4.Dot(ref AngularJacobianITB, ref AngularJacobianITB, out angularContributionB);
            var inverseEffectiveMass = inverseMassA + inverseMassB + angularContributionA + angularContributionB;

            Vector4 CollisionSoftness = new Vector4(5);

            Softness      = CollisionSoftness * inverseEffectiveMass * inverseDt;
            EffectiveMass = Vector4.One / (Softness + inverseEffectiveMass);
        }
コード例 #5
0
 public static void Transform(ref Vector3Width4 v, ref Matrix3x3Width4 m, out Vector3Width4 result)
 {
     result.X = v.X * m.M11 + v.Y * m.M21 + v.Z * m.M31;
     result.Y = v.X * m.M12 + v.Y * m.M22 + v.Z * m.M32;
     result.Z = v.X * m.M13 + v.Y * m.M23 + v.Z * m.M33;
 }
コード例 #6
0
ファイル: Matrix3x3Width4.cs プロジェクト: jj-jabb/scratchpad
 public static void Transform(ref Vector3Width4 v, ref Matrix3x3Width4 m, out Vector3Width4 result)
 {
     result.X = v.X * m.M11 + v.Y * m.M21 + v.Z * m.M31;
     result.Y = v.X * m.M12 + v.Y * m.M22 + v.Z * m.M32;
     result.Z = v.X * m.M13 + v.Y * m.M23 + v.Z * m.M33;
 }
コード例 #7
0
        public void Prestep(float inverseDt,
            ref Vector4 inverseMassA, ref Vector4 inverseMassB, 
            ref Matrix3x3Width4 inverseInertiaTensorA, ref Matrix3x3Width4 inverseInertiaTensorB,
            ref Vector3Width4 positionA, ref Vector3Width4 positionB)
        {
            //C = dot(Pa - Pb, N) > 0
            //Jacobians:
            //LinearA: N
            //AngularA: cross(OffsetPa, N)
            //LinearB: -N
            //AngularB: -cross(OffsetPb, N)

            //var positionA = new Vector3Width4();
            //var positionB = new Vector3Width4();

            LinearJacobianA = ContactNormal;
            Vector3Width4.Negate(ref ContactNormal, out LinearJacobianB);

            Vector3Width4 offsetA, offsetB;
            Vector3Width4.Subtract(ref ContactPosition, ref positionA, out offsetA);
            Vector3Width4.Subtract(ref ContactPosition, ref positionB, out offsetB);

            Vector3Width4.Cross(ref offsetA, ref ContactNormal, out AngularJacobianA);
            Vector3Width4.Cross(ref ContactNormal, ref offsetB, out AngularJacobianB);// note negation->parameter reverse

            //Allow velocity that closes a gap, and apply penetration correction against positive depth.
            //Bounciness not yet included.
            PenetrationBias = ContactPenetration * inverseDt;
            PenetrationBias = -Vector4.Min(Vector4.Min(PenetrationBias, PenetrationBias * 0.2f), new Vector4(0.2f));

            //The inertia tensor is in world space, so no jacobian transformation is required.
            Vector3Width4.Multiply(ref LinearJacobianA, ref inverseMassA, out LinearJacobianITA);
            Vector3Width4.Multiply(ref LinearJacobianB, ref inverseMassB, out LinearJacobianITB);
            Matrix3x3Width4.Transform(ref AngularJacobianA, ref inverseInertiaTensorA, out AngularJacobianITA);
            Matrix3x3Width4.Transform(ref AngularJacobianB, ref inverseInertiaTensorB, out AngularJacobianITB);
            Vector4 angularContributionA, angularContributionB;
            Vector3Width4.Dot(ref AngularJacobianITA, ref AngularJacobianITA, out angularContributionA);
            Vector3Width4.Dot(ref AngularJacobianITB, ref AngularJacobianITB, out angularContributionB);
            var inverseEffectiveMass = inverseMassA + inverseMassB + angularContributionA + angularContributionB;

            Vector4 CollisionSoftness = new Vector4(5);
            Softness = CollisionSoftness * inverseEffectiveMass * inverseDt;
            EffectiveMass = Vector4.One / (Softness + inverseEffectiveMass);
        }
コード例 #8
0
        public void Prestep(float inverseDt)
        {
            //C = dot(Pa - Pb, N) > 0
            //Jacobians:
            //LinearA: N
            //AngularA: cross(OffsetPa, N)
            //LinearB: -N
            //AngularB: -cross(OffsetPb, N)

            //var positionA = new Vector3Width4();
            //var positionB = new Vector3Width4();

            //Given that we're collecting position, inverse mass, and inertia all at once, it makes no sense to store position separately from inversemass and inertia.
            //Since you should not expect the 4 involved bodies to be in memory *together*, the best you can do is to ensure that the set of values are together.
            //Otherwise you're multiplying cache misses for no reason!
            var InverseMassA = new Vector4(BodyA0.InverseMass, BodyA1.InverseMass, BodyA2.InverseMass, BodyA3.InverseMass);
            var InverseMassB = new Vector4(BodyB0.InverseMass, BodyB1.InverseMass, BodyB2.InverseMass, BodyB3.InverseMass);

            var InverseInertiaTensorA = new Matrix3x3Width4(ref BodyA0.InertiaTensorInverse, ref BodyA1.InertiaTensorInverse, ref BodyA2.InertiaTensorInverse, ref BodyA3.InertiaTensorInverse);
            var InverseInertiaTensorB = new Matrix3x3Width4(ref BodyB0.InertiaTensorInverse, ref BodyB1.InertiaTensorInverse, ref BodyB2.InertiaTensorInverse, ref BodyB3.InertiaTensorInverse);

            Vector3Width4 positionA = new Vector3Width4(ref BodyA0.Position, ref BodyA1.Position, ref BodyA2.Position, ref BodyA3.Position);
            Vector3Width4 positionB = new Vector3Width4(ref BodyA0.Position, ref BodyB1.Position, ref BodyB2.Position, ref BodyB3.Position);

            LinearJacobianA = ContactNormal;
            Vector3Width4.Negate(ref ContactNormal, out LinearJacobianB);

            Vector3Width4 offsetA, offsetB;
            Vector3Width4.Subtract(ref ContactPosition, ref positionA, out offsetA);
            Vector3Width4.Subtract(ref ContactPosition, ref positionB, out offsetB);

            Vector3Width4.Cross(ref offsetA, ref ContactNormal, out AngularJacobianA);
            Vector3Width4.Cross(ref ContactNormal, ref offsetB, out AngularJacobianB);// note negation->parameter reverse

            //Allow velocity that closes a gap, and apply penetration correction against positive depth.
            //Bounciness not yet included.
            PenetrationBias = ContactPenetration * inverseDt;
            PenetrationBias = -Vector4.Min(Vector4.Min(PenetrationBias, PenetrationBias * 0.2f), new Vector4(0.2f));

            //The inertia tensor is in world space, so no jacobian transformation is required.
            Vector3Width4.Multiply(ref LinearJacobianA, ref InverseMassA, out LinearJacobianITA);
            Vector3Width4.Multiply(ref LinearJacobianB, ref InverseMassB, out LinearJacobianITB);
            Matrix3x3Width4.Transform(ref AngularJacobianA, ref InverseInertiaTensorA, out AngularJacobianITA);
            Matrix3x3Width4.Transform(ref AngularJacobianB, ref InverseInertiaTensorB, out AngularJacobianITB);
            Vector4 angularContributionA, angularContributionB;
            Vector3Width4.Dot(ref AngularJacobianITA, ref AngularJacobianITA, out angularContributionA);
            Vector3Width4.Dot(ref AngularJacobianITB, ref AngularJacobianITB, out angularContributionB);
            var inverseEffectiveMass = InverseMassA + InverseMassB + angularContributionA + angularContributionB;

            Vector4 CollisionSoftness = new Vector4(5);
            Softness = CollisionSoftness * inverseEffectiveMass * inverseDt;
            EffectiveMass = Vector4.One / (Softness + inverseEffectiveMass);
        }