/// <summary> /// Computes one iteration of the constraint to meet the solver updateable's goal. /// </summary> /// <returns>The rough applied impulse magnitude.</returns> public override float SolveIteration() { // lambda = -mc * (Jv + b) // P = JT * lambda Vector3 velocity; Vector3.Subtract(ref connectionA.angularVelocity, ref connectionB.angularVelocity, out velocity); #if !WINDOWS Vector2 lambda = new Vector2(); #else Vector2 lambda; #endif Vector3.Dot(ref worldConstrainedAxis1, ref velocity, out lambda.X); Vector3.Dot(ref worldConstrainedAxis2, ref velocity, out lambda.Y); Vector2.Add(ref lambda, ref biasVelocity, out lambda); Vector2 softnessImpulse; Vector2.Multiply(ref accumulatedImpulse, softness, out softnessImpulse); Vector2.Add(ref lambda, ref softnessImpulse, out lambda); Matrix2x2.Transform(ref lambda, ref effectiveMassMatrix, out lambda); Vector2.Add(ref accumulatedImpulse, ref lambda, out accumulatedImpulse); #if !WINDOWS Vector3 impulse = new Vector3(); #else Vector3 impulse; #endif impulse.X = worldConstrainedAxis1.X * lambda.X + worldConstrainedAxis2.X * lambda.Y; impulse.Y = worldConstrainedAxis1.Y * lambda.X + worldConstrainedAxis2.Y * lambda.Y; impulse.Z = worldConstrainedAxis1.Z * lambda.X + worldConstrainedAxis2.Z * lambda.Y; if (connectionA.isDynamic) { connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Negate(ref impulse, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } return(Math.Abs(lambda.X) + Math.Abs(lambda.Y)); }
/// <summary> /// Computes a solution to the constraint. /// </summary> /// <returns>Impulse magnitude computed by the iteration.</returns> public override float SolveIteration() { Vector2 relativeVelocity = RelativeVelocity; Vector2.Add(ref relativeVelocity, ref positionCorrectionBias, out relativeVelocity); //Create the full velocity change, and convert it to an impulse in constraint space. Vector2 lambda; Vector2.Subtract(ref targetVelocity, ref relativeVelocity, out lambda); Matrix2x2.Transform(ref lambda, ref massMatrix, out lambda); //Add and clamp the impulse. Vector2 previousAccumulatedImpulse = accumulatedImpulse; if (MovementMode == MovementMode.Floating) { //If it's floating, clamping rules are different. //The constraint is not permitted to slow down the character; only speed it up. //This offers a hole for an exploit; by jumping and curving just right, //the character can accelerate beyond its maximum speed. A bit like an HL2 speed run. accumulatedImpulse.X = MathHelper.Clamp(accumulatedImpulse.X + lambda.X, 0, maxForceDt); accumulatedImpulse.Y = 0; } else { Vector2.Add(ref lambda, ref accumulatedImpulse, out accumulatedImpulse); float length = accumulatedImpulse.LengthSquared(); if (length > maxForceDt * maxForceDt) { Vector2.Multiply(ref accumulatedImpulse, maxForceDt / (float)Math.Sqrt(length), out accumulatedImpulse); } if (isTryingToMove && accumulatedImpulse.X > maxAccelerationForceDt) { accumulatedImpulse.X = maxAccelerationForceDt; } } Vector2.Subtract(ref accumulatedImpulse, ref previousAccumulatedImpulse, out lambda); //Use the jacobians to put the impulse into world space. #if !WINDOWS Vector3 impulse = new Vector3(); Vector3 torque = new Vector3(); #else Vector3 impulse; Vector3 torque; #endif float x = lambda.X; float y = lambda.Y; impulse.X = linearJacobianA1.X * x + linearJacobianA2.X * y; impulse.Y = linearJacobianA1.Y * x + linearJacobianA2.Y * y; impulse.Z = linearJacobianA1.Z * x + linearJacobianA2.Z * y; characterBody.ApplyLinearImpulse(ref impulse); if (supportEntity != null && supportEntity.IsDynamic) { Vector3.Multiply(ref impulse, -supportForceFactor, out impulse); x *= supportForceFactor; y *= supportForceFactor; torque.X = x * angularJacobianB1.X + y * angularJacobianB2.X; torque.Y = x * angularJacobianB1.Y + y * angularJacobianB2.Y; torque.Z = x * angularJacobianB1.Z + y * angularJacobianB2.Z; supportEntity.ApplyLinearImpulse(ref impulse); supportEntity.ApplyAngularImpulse(ref torque); } return(Math.Abs(lambda.X) + Math.Abs(lambda.Y)); }
/// <summary> /// Calculates and applies corrective impulses. /// Called automatically by space. /// </summary> public override float SolveIteration() { #region Theory //lambda = -mc * (Jv + b) // PraT = [ bx by bz ] * [ 0 raz -ray ] = [ (-by * raz + bz * ray) (bx * raz - bz * rax) (-bx * ray + by * rax) ] // [ cx cy cz ] [ -raz 0 rax ] [ (-cy * raz + cz * ray) (cx * raz - cz * rax) (-cx * ray + cy * rax) ] // [ ray -rax 0 ] // // PrbT = [ bx by bz ] * [ 0 rbz -rby ] = [ (-by * rbz + bz * rby) (bx * rbz - bz * rbx) (-bx * rby + by * rbx) ] // [ cx cy cz ] [ -rbz 0 rbx ] [ (-cy * rbz + cz * rby) (cx * rbz - cz * rbx) (-cx * rby + cy * rbx) ] // [ rby -rbx 0 ] // Jv = [ bx by bz PraT -bx -by -bz -Prbt ] * [ vax ] // [ cx cy cz -cx -cy -cz ] [ vay ] // [ vaz ] // [ wax ] // [ way ] // [ waz ] // [ vbx ] // [ vby ] // [ vbz ] // [ wbx ] // [ wby ] // [ wbz ] // va' = [ bx * vax + by * vay + bz * vaz ] = [ b * va ] // [ cx * vax + cy * vay + cz * vaz ] [ c * va ] // wa' = [ (PraT row 1) * wa ] // [ (PraT row 2) * wa ] // vb' = [ -bx * vbx - by * vby - bz * vbz ] = [ -b * vb ] // [ -cx * vbx - cy * vby - cz * vbz ] [ -c * vb ] // wb' = [ -(PrbT row 1) * wb ] // [ -(PrbT row 2) * wb ] // Jv = [ b * va + (PraT row 1) * wa - b * vb - (PrbT row 1) * wb ] // [ c * va + (PraT row 2) * wa - c * vb - (PrbT row 2) * wb ] // Jv = [ b * (va + wa x ra - vb - wb x rb) ] // [ c * (va + wa x ra - vb - wb x rb) ] //P = JT * lambda #endregion #if !WINDOWS Vector2 lambda = new Vector2(); #else Vector2 lambda; #endif //float va1, va2, wa1, wa2, vb1, vb2, wb1, wb2; //Vector3.Dot(ref worldAxis1, ref myParentA.myInternalLinearVelocity, out va1); //Vector3.Dot(ref worldAxis2, ref myParentA.myInternalLinearVelocity, out va2); //wa1 = prAT.M11 * myParentA.myInternalAngularVelocity.X + prAT.M12 * myParentA.myInternalAngularVelocity.Y + prAT.M13 * myParentA.myInternalAngularVelocity.Z; //wa2 = prAT.M21 * myParentA.myInternalAngularVelocity.X + prAT.M22 * myParentA.myInternalAngularVelocity.Y + prAT.M23 * myParentA.myInternalAngularVelocity.Z; //Vector3.Dot(ref worldAxis1, ref myParentB.myInternalLinearVelocity, out vb1); //Vector3.Dot(ref worldAxis2, ref myParentB.myInternalLinearVelocity, out vb2); //wb1 = prBT.M11 * myParentB.myInternalAngularVelocity.X + prBT.M12 * myParentB.myInternalAngularVelocity.Y + prBT.M13 * myParentB.myInternalAngularVelocity.Z; //wb2 = prBT.M21 * myParentB.myInternalAngularVelocity.X + prBT.M22 * myParentB.myInternalAngularVelocity.Y + prBT.M23 * myParentB.myInternalAngularVelocity.Z; //lambda.X = va1 + wa1 - vb1 - wb1 + biasVelocity.X + mySoftness * accumulatedImpulse.X; //lambda.Y = va2 + wa2 - vb2 - wb2 + biasVelocity.Y + mySoftness * accumulatedImpulse.Y; Vector3 dv; Vector3 aVel, bVel; Vector3.Cross(ref connectionA.angularVelocity, ref rA, out aVel); Vector3.Add(ref aVel, ref connectionA.linearVelocity, out aVel); Vector3.Cross(ref connectionB.angularVelocity, ref rB, out bVel); Vector3.Add(ref bVel, ref connectionB.linearVelocity, out bVel); Vector3.Subtract(ref aVel, ref bVel, out dv); Vector3.Dot(ref dv, ref worldRestrictedAxis1, out lambda.X); Vector3.Dot(ref dv, ref worldRestrictedAxis2, out lambda.Y); lambda.X += biasVelocity.X + softness * accumulatedImpulse.X; lambda.Y += biasVelocity.Y + softness * accumulatedImpulse.Y; //Convert to impulse Matrix2x2.Transform(ref lambda, ref negativeEffectiveMassMatrix, out lambda); Vector2.Add(ref lambda, ref accumulatedImpulse, out accumulatedImpulse); float x = lambda.X; float y = lambda.Y; //Apply impulse #if !WINDOWS Vector3 impulse = new Vector3(); Vector3 torque = new Vector3(); #else Vector3 impulse; Vector3 torque; #endif impulse.X = worldRestrictedAxis1.X * x + worldRestrictedAxis2.X * y; impulse.Y = worldRestrictedAxis1.Y * x + worldRestrictedAxis2.Y * y; impulse.Z = worldRestrictedAxis1.Z * x + worldRestrictedAxis2.Z * y; if (connectionA.isDynamic) { torque.X = x * angularA1.X + y * angularA2.X; torque.Y = x * angularA1.Y + y * angularA2.Y; torque.Z = x * angularA1.Z + y * angularA2.Z; connectionA.ApplyLinearImpulse(ref impulse); connectionA.ApplyAngularImpulse(ref torque); } if (connectionB.isDynamic) { impulse.X = -impulse.X; impulse.Y = -impulse.Y; impulse.Z = -impulse.Z; torque.X = x * angularB1.X + y * angularB2.X; torque.Y = x * angularB1.Y + y * angularB2.Y; torque.Z = x * angularB1.Z + y * angularB2.Z; connectionB.ApplyLinearImpulse(ref impulse); connectionB.ApplyAngularImpulse(ref torque); } return(Math.Abs(lambda.X) + Math.Abs(lambda.Y)); }