///<summary> /// Performs the frame's configuration step. ///</summary> ///<param name="dt">Timestep duration.</param> public override void Update(float dt) { //Transform local axes into world space Matrix3x3.Transform(ref localRestrictedAxis1, ref connectionA.orientationMatrix, out worldRestrictedAxis1); Matrix3x3.Transform(ref localRestrictedAxis2, ref connectionA.orientationMatrix, out worldRestrictedAxis2); Matrix3x3.Transform(ref localAxisAnchor, ref connectionA.orientationMatrix, out worldLineAnchor); Vector3.Add(ref worldLineAnchor, ref connectionA.position, out worldLineAnchor); Matrix3x3.Transform(ref localLineDirection, ref connectionA.orientationMatrix, out worldLineDirection); //Transform local Matrix3x3.Transform(ref localPoint, ref connectionB.orientationMatrix, out rB); Vector3.Add(ref rB, ref connectionB.position, out worldPoint); //Find the point on the line closest to the world point. Vector3 offset; Vector3.Subtract(ref worldPoint, ref worldLineAnchor, out offset); float distanceAlongAxis; Vector3.Dot(ref offset, ref worldLineDirection, out distanceAlongAxis); Vector3 worldNearPoint; Vector3.Multiply(ref worldLineDirection, distanceAlongAxis, out offset); Vector3.Add(ref worldLineAnchor, ref offset, out worldNearPoint); Vector3.Subtract(ref worldNearPoint, ref connectionA.position, out rA); //Error Vector3 error3D; Vector3.Subtract(ref worldPoint, ref worldNearPoint, out error3D); Vector3.Dot(ref error3D, ref worldRestrictedAxis1, out error.X); Vector3.Dot(ref error3D, ref worldRestrictedAxis2, out error.Y); float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness); float bias = -errorReduction; biasVelocity.X = bias * error.X; biasVelocity.Y = bias * error.Y; //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared(); if (length > maxCorrectiveVelocitySquared) { float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; } //Set up the jacobians Vector3.Cross(ref rA, ref worldRestrictedAxis1, out angularA1); Vector3.Cross(ref worldRestrictedAxis1, ref rB, out angularB1); Vector3.Cross(ref rA, ref worldRestrictedAxis2, out angularA2); Vector3.Cross(ref worldRestrictedAxis2, ref rB, out angularB2); float m11 = 0, m22 = 0, m1221 = 0; float inverseMass; Vector3 intermediate; //Compute the effective mass matrix. if (connectionA.isDynamic) { inverseMass = connectionA.inverseMass; Matrix3x3.Transform(ref angularA1, ref connectionA.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref angularA1, out m11); m11 += inverseMass; Vector3.Dot(ref intermediate, ref angularA2, out m1221); Matrix3x3.Transform(ref angularA2, ref connectionA.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref angularA2, out m22); m22 += inverseMass; } #region Mass Matrix B if (connectionB.isDynamic) { float extra; inverseMass = connectionB.inverseMass; Matrix3x3.Transform(ref angularB1, ref connectionB.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref angularB1, out extra); m11 += inverseMass + extra; Vector3.Dot(ref intermediate, ref angularB2, out extra); m1221 += extra; Matrix3x3.Transform(ref angularB2, ref connectionB.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref angularB2, out extra); m22 += inverseMass + extra; } #endregion negativeEffectiveMassMatrix.M11 = m11 + softness; negativeEffectiveMassMatrix.M12 = m1221; negativeEffectiveMassMatrix.M21 = m1221; negativeEffectiveMassMatrix.M22 = m22 + softness; Matrix2x2.Invert(ref negativeEffectiveMassMatrix, out negativeEffectiveMassMatrix); Matrix2x2.Negate(ref negativeEffectiveMassMatrix, out negativeEffectiveMassMatrix); }
///<summary> /// Performs the frame's configuration step. ///</summary> ///<param name="dt">Timestep duration.</param> public override void Update(float dt) { Matrix3x3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA); Matrix3x3.Transform(ref localAxisB, ref connectionB.orientationMatrix, out worldAxisB); Matrix3x3.Transform(ref localConstrainedAxis1, ref connectionA.orientationMatrix, out worldConstrainedAxis1); Matrix3x3.Transform(ref localConstrainedAxis2, ref connectionA.orientationMatrix, out worldConstrainedAxis2); Vector3 error; Vector3.Cross(ref worldAxisA, ref worldAxisB, out error); Vector3.Dot(ref error, ref worldConstrainedAxis1, out this.error.X); Vector3.Dot(ref error, ref worldConstrainedAxis2, out this.error.Y); float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness); errorReduction = -errorReduction; biasVelocity.X = errorReduction * this.error.X; biasVelocity.Y = errorReduction * this.error.Y; //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared(); if (length > maxCorrectiveVelocitySquared) { float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; } Vector3 axis1I, axis2I; if (connectionA.isDynamic && connectionB.isDynamic) { Matrix3x3 inertiaTensorSum; Matrix3x3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out inertiaTensorSum); Matrix3x3.Transform(ref worldConstrainedAxis1, ref inertiaTensorSum, out axis1I); Matrix3x3.Transform(ref worldConstrainedAxis2, ref inertiaTensorSum, out axis2I); } else if (connectionA.isDynamic && !connectionB.isDynamic) { Matrix3x3.Transform(ref worldConstrainedAxis1, ref connectionA.inertiaTensorInverse, out axis1I); Matrix3x3.Transform(ref worldConstrainedAxis2, ref connectionA.inertiaTensorInverse, out axis2I); } else if (!connectionA.isDynamic && connectionB.isDynamic) { Matrix3x3.Transform(ref worldConstrainedAxis1, ref connectionB.inertiaTensorInverse, out axis1I); Matrix3x3.Transform(ref worldConstrainedAxis2, ref connectionB.inertiaTensorInverse, out axis2I); } else { throw new InvalidOperationException("Cannot constrain two kinematic bodies."); } Vector3.Dot(ref axis1I, ref worldConstrainedAxis1, out effectiveMassMatrix.M11); Vector3.Dot(ref axis1I, ref worldConstrainedAxis2, out effectiveMassMatrix.M12); Vector3.Dot(ref axis2I, ref worldConstrainedAxis1, out effectiveMassMatrix.M21); Vector3.Dot(ref axis2I, ref worldConstrainedAxis2, out effectiveMassMatrix.M22); effectiveMassMatrix.M11 += softness; effectiveMassMatrix.M22 += softness; Matrix2x2.Invert(ref effectiveMassMatrix, out effectiveMassMatrix); Matrix2x2.Negate(ref effectiveMassMatrix, out effectiveMassMatrix); }
/// <summary> /// Gets the mass matrix of the constraint. /// </summary> /// <param name="massMatrix">Constraint's mass matrix.</param> public void GetMassMatrix(out Matrix2x2 massMatrix) { Matrix2x2.Negate(ref negativeEffectiveMassMatrix, out massMatrix); }