///<summary> /// Performs the frame's configuration step. ///</summary> ///<param name="dt">Timestep duration.</param> public override void Update(float dt) { //Compute the 'pre'-jacobians Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA); Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB); Vector3.Add(ref worldOffsetA, ref connectionA.position, out worldAnchorA); Vector3.Add(ref worldOffsetB, ref connectionB.position, out worldAnchorB); Vector3.Subtract(ref worldAnchorB, ref connectionA.position, out rA); Matrix3x3.Transform(ref localAxis, ref connectionA.orientationMatrix, out worldAxis); //Compute error #if !WINDOWS Vector3 separation = new Vector3(); #else Vector3 separation; #endif separation.X = worldAnchorB.X - worldAnchorA.X; separation.Y = worldAnchorB.Y - worldAnchorA.Y; separation.Z = worldAnchorB.Z - worldAnchorA.Z; Vector3.Dot(ref separation, ref worldAxis, out unadjustedError); //Compute error if (unadjustedError < minimum) { unadjustedError = minimum - unadjustedError; } else if (unadjustedError > maximum) { unadjustedError = maximum - unadjustedError; } else { unadjustedError = 0; isActiveInSolver = false; accumulatedImpulse = 0; isLimitActive = false; return; } isLimitActive = true; unadjustedError = -unadjustedError; //Adjust Error if (unadjustedError > 0) { error = MathHelper.Max(0, unadjustedError - margin); } else if (unadjustedError < 0) { error = MathHelper.Min(0, unadjustedError + margin); } //Compute jacobians jLinearA = worldAxis; jLinearB.X = -jLinearA.X; jLinearB.Y = -jLinearA.Y; jLinearB.Z = -jLinearA.Z; Vector3.Cross(ref rA, ref jLinearA, out jAngularA); Vector3.Cross(ref worldOffsetB, ref jLinearB, out jAngularB); //Compute bias float errorReductionParameter; springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReductionParameter, out softness); biasVelocity = MathHelper.Clamp(errorReductionParameter * error, -maxCorrectiveVelocity, maxCorrectiveVelocity); if (bounciness > 0) { //Compute currently relative velocity for bounciness. float relativeVelocity, dot; Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out relativeVelocity); Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot); relativeVelocity += dot; Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot); relativeVelocity += dot; Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot); relativeVelocity += dot; if (unadjustedError > 0 && -relativeVelocity > bounceVelocityThreshold) { biasVelocity = Math.Max(biasVelocity, ComputeBounceVelocity(-relativeVelocity)); } else if (unadjustedError < 0 && relativeVelocity > bounceVelocityThreshold) { biasVelocity = Math.Min(biasVelocity, -ComputeBounceVelocity(relativeVelocity)); } } //compute mass matrix float entryA, entryB; Vector3 intermediate; if (connectionA.isDynamic) { Matrix3x3.Transform(ref jAngularA, ref connectionA.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref jAngularA, out entryA); entryA += connectionA.inverseMass; } else { entryA = 0; } if (connectionB.isDynamic) { Matrix3x3.Transform(ref jAngularB, ref connectionB.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref jAngularB, out entryB); entryB += connectionB.inverseMass; } else { entryB = 0; } massMatrix = 1 / (entryA + entryB + softness); }