public static void ApplyImpulse(ref Projection2Body1DOF data, ref Vector <float> correctiveImpulse,
ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
//Applying the impulse requires transforming the constraint space impulse into a world space velocity change.
//The first step is to transform into a world space impulse, which requires transforming by the transposed jacobian
//(transpose(jacobian) goes from world to constraint space, jacobian goes from constraint to world space).
//That world space impulse is then converted to a corrective velocity change by scaling the impulse by the inverse mass/inertia.
//As an optimization for constraints with smaller jacobians, the jacobian * (inertia or mass) transform is precomputed.
BodyVelocities correctiveVelocityA, correctiveVelocityB;
Vector3Wide.Scale(data.CSIToWSVLinearA, correctiveImpulse, out correctiveVelocityA.Linear);
Vector3Wide.Scale(data.CSIToWSVAngularA, correctiveImpulse, out correctiveVelocityA.Angular);
Vector3Wide.Scale(data.CSIToWSVLinearB, correctiveImpulse, out correctiveVelocityB.Linear);
Vector3Wide.Scale(data.CSIToWSVAngularB, correctiveImpulse, out correctiveVelocityB.Angular);
Vector3Wide.Add(correctiveVelocityA.Linear, wsvA.Linear, out wsvA.Linear);
Vector3Wide.Add(correctiveVelocityA.Angular, wsvA.Angular, out wsvA.Angular);
Vector3Wide.Add(correctiveVelocityB.Linear, wsvB.Linear, out wsvB.Linear);
Vector3Wide.Add(correctiveVelocityB.Angular, wsvB.Angular, out wsvB.Angular);
}
public static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref TwoBody1DOFJacobians jacobians, ref SpringSettingsWide springSettings, ref Vector <float> maximumRecoveryVelocity,
ref Vector <float> positionError, float dt, float inverseDt, out Projection2Body1DOF projection)
{
//unsoftened effective mass = (J * M^-1 * JT)^-1
//where J is a constraintDOF x bodyCount*6 sized matrix, JT is its transpose, and for two bodies3D M^-1 is:
//[inverseMassA, 0, 0, 0]
//[0, inverseInertiaA, 0, 0]
//[0, 0, inverseMassB, 0]
//[0, 0, 0, inverseInertiaB]
//The entries of J match up to this convention, containing the linear and angular components of each body in sequence, so for a 2 body 1DOF constraint J would look like:
//[linearA 1x3, angularA 1x3, linearB 1x3, angularB 1x3]
//Note that it is a row vector by convention. When transforming velocities from world space into constraint space, it is assumed that the velocity vector is organized as a
//row vector matching up to the jacobian (that is, [linearA 1x3, angularA 1x3, linearB 1x3, angularB 1x3]), so for a 2 body 2 DOF constraint,
//worldVelocity * JT would be a [worldVelocity: 1x12] * [JT: 12x2], resulting in a 1x2 constraint space velocity row vector.
//Similarly, when going from constraint space impulse to world space impulse in the above example, we would do [csi: 1x2] * [J: 2x12] to get a 1x12 world impulse row vector.
//Note that the engine uses row vectors for all velocities and positions and so on. Rotation and inertia tensors are constructed for premultiplication.
//In other words, unlike many of the presentations in the space, we use v * JT and csi * J instead of J * v and JT * csi.
//There is no meaningful difference- the two conventions are just transpositions of each other.
//(If you want to know how this stuff works, go read the constraint related presentations: http://box2d.org/downloads/
//Be mindful of the difference in conventions. You'll see J * v instead of v * JT, for example. Everything is still fundamentally the same, though.)
//Due to the block structure of the mass matrix, we can handle each component separately and then sum the results.
//For this 1DOF constraint, the result is a simple scalar.
//Note that we store the intermediate results of J * M^-1 for use when projecting from constraint space impulses to world velocity changes.
//If we didn't store those intermediate values, we could just scale the dot product of jacobians.LinearA with itself to save 4 multiplies.
Vector3Wide.Scale(jacobians.LinearA, inertiaA.InverseMass, out projection.CSIToWSVLinearA);
Vector3Wide.Scale(jacobians.LinearB, inertiaB.InverseMass, out projection.CSIToWSVLinearB);
Vector3Wide.Dot(projection.CSIToWSVLinearA, jacobians.LinearA, out var linearA);
Vector3Wide.Dot(projection.CSIToWSVLinearB, jacobians.LinearB, out var linearB);
//The angular components are a little more involved; (J * I^-1) * JT is explicitly computed.
Symmetric3x3Wide.TransformWithoutOverlap(jacobians.AngularA, inertiaA.InverseInertiaTensor, out projection.CSIToWSVAngularA);
Symmetric3x3Wide.TransformWithoutOverlap(jacobians.AngularB, inertiaB.InverseInertiaTensor, out projection.CSIToWSVAngularB);
Vector3Wide.Dot(projection.CSIToWSVAngularA, jacobians.AngularA, out var angularA);
Vector3Wide.Dot(projection.CSIToWSVAngularB, jacobians.AngularB, out var angularB);
//Now for a digression!
//Softness is applied along the diagonal (which, for a 1DOF constraint, is just the only element).
//Check the the ODE reference for a bit more information: http://ode.org/ode-latest-userguide.html#sec_3_8_0
//And also see Erin Catto's Soft Constraints presentation for more details: http://box2d.org/files/GDC2011/GDC2011_Catto_Erin_Soft_Constraints.pdf)
//There are some very interesting tricks you can use here, though.
//Our core tuning variables are the damping ratio and natural frequency.
//Our runtime used variables are softness and an error reduction feedback scale..
//(For the following, I'll use the ODE terms CFM and ERP, constraint force mixing and error reduction parameter.)
//So first, we need to get from damping ratio and natural frequency to stiffness and damping spring constants.
//From there, we'll go to CFM/ERP.
//Then, we'll create an expression for a softened effective mass matrix (i.e. one that takes into account the CFM term),
//and an expression for the contraint force mixing term in the solve iteration.
//Finally, compute ERP.
//(And then some tricks.)
//1) Convert from damping ratio and natural frequency to stiffness and damping constants.
//The raw expressions are:
//stiffness = effectiveMass * naturalFrequency^2
//damping = effectiveMass * 2 * dampingRatio * naturalFrequency
//Rather than using any single object as the reference for the 'mass' term involved in this conversion, use the effective mass of the constraint.
//In other words, we're dynamically picking the spring constants necessary to achieve the desired behavior for the current constraint configuration.
//(See Erin Catto's presentation above for more details on this.)
//(Note that this is different from BEPUphysics v1. There, users configured stiffness and damping constants. That worked okay, but people often got confused about
//why constraints didn't behave the same when they changed masses. Usually it manifested as someone creating an incredibly high mass object relative to the default
//stiffness/damping, and they'd post on the forum wondering why constraints were so soft. Basically, the defaults were another sneaky tuning factor to get wrong.
//Since damping ratio and natural frequency define the behavior independent of the mass, this problem goes away- and it makes some other interesting things happen...)
//2) Convert from stiffness and damping constants to CFM and ERP.
//CFM = (stiffness * dt + damping)^-1
//ERP = (stiffness * dt) * (stiffness * dt + damping)^-1
//Or, to rephrase:
//ERP = (stiffness * dt) * CFM
//3) Use CFM and ERP to create a softened effective mass matrix and a force mixing term for the solve iterations.
//Start with a base definition which we won't be deriving, the velocity constraint itself (stated as an equality constraint here):
//This means 'world space velocity projected into constraint space should equal the velocity bias term combined with the constraint force mixing term'.
//(The velocity bias term will be computed later- it's the position error scaled by the error reduction parameter, ERP. Position error is used to create a velocity motor goal.)
//We're pulling back from the implementation of sequential impulses here, so rather than using the term 'accumulated impulse', we'll use 'lambda'
//(which happens to be consistent with the ODE documentation covering the same topic). Lambda is impulse that satisfies the constraint.
//wsv * JT = bias - lambda * CFM/dt
//This can be phrased as:
//currentVelocity = targetVelocity
//Or:
//goalVelocityChange = targetVelocity - currentVelocity
//lambda = goalVelocityChange * effectiveMass
//lambda = (targetVelocity - currentVelocity) * effectiveMass
//lambda = (bias - lambda * CFM/dt - currentVelocity) * effectiveMass
//Solving for lambda:
//lambda = (bias - currentVelocity) * effectiveMass - lambda * CFM/dt * effectiveMass
//lambda + lambda * CFM/dt * effectiveMass = (bias - currentVelocity) * effectiveMass
//(lambda + lambda * CFM/dt * effectiveMass) * effectiveMass^-1 = bias - currentVelocity
//lambda * effectiveMass^-1 + lambda * CFM/dt = bias - currentVelocity
//lambda * (effectiveMass^-1 + CFM/dt) = bias - currentVelocity
//lambda = (bias - currentVelocity) * (effectiveMass^-1 + CFM/dt)^-1
//lambda = (bias - wsv * JT) * (effectiveMass^-1 + CFM/dt)^-1
//In other words, we transform the velocity change (bias - wsv * JT) into the constraint-satisfying impulse, lambda, using a matrix (effectiveMass^-1 + CFM/dt)^-1.
//That matrix is the softened effective mass:
//softenedEffectiveMass = (effectiveMass^-1 + CFM/dt)^-1
//Here's where some trickiness occurs. (Be mindful of the distinction between the softened and unsoftened effective mass).
//Start by substituting CFM into the softened effective mass definition:
//CFM/dt = (stiffness * dt + damping)^-1 / dt = (dt * (stiffness * dt + damping))^-1 = (stiffness * dt^2 + damping*dt)^-1
//softenedEffectiveMass = (effectiveMass^-1 + (stiffness * dt^2 + damping * dt)^-1)^-1
//Now substitute the definitions of stiffness and damping, treating the scalar components as uniform scaling matrices of dimension equal to effectiveMass:
//softenedEffectiveMass = (effectiveMass^-1 + ((effectiveMass * naturalFrequency^2) * dt^2 + (effectiveMass * 2 * dampingRatio * naturalFrequency) * dt)^-1)^-1
//Combine the inner effectiveMass coefficients, given matrix multiplication distributes over addition:
//softenedEffectiveMass = (effectiveMass^-1 + (effectiveMass * (naturalFrequency^2 * dt^2) + effectiveMass * (2 * dampingRatio * naturalFrequency * dt))^-1)^-1
//softenedEffectiveMass = (effectiveMass^-1 + (effectiveMass * (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt))^-1)^-1
//Apply the inner matrix inverse:
//softenedEffectiveMass = (effectiveMass^-1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1)^-1
//Once again, combine coefficients of the inner effectiveMass^-1 terms:
//softenedEffectiveMass = ((1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1) * effectiveMass^-1)^-1
//Apply the inverse again:
//softenedEffectiveMass = effectiveMass * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1
//So, to put it another way- because CFM is based on the effective mass, applying it to the effective mass results in a simple downscale.
//What has been gained? Consider what happens in the solve iteration.
//We take the velocity error:
//velocityError = bias - accumulatedImpulse * CFM/dt - wsv * JT
//and convert it to a corrective impulse with the effective mass:
//impulse = (bias - accumulatedImpulse * CFM/dt - wsv * JT) * softenedEffectiveMass
//The effective mass distributes over the set:
//impulse = bias * softenedEffectiveMass - accumulatedImpulse * CFM/dt * softenedEffectiveMass - wsv * JT * softenedEffectiveMass
//Focus on the CFM term:
//-accumulatedImpulse * CFM/dt * softenedEffectiveMass
//What is CFM/dt * softenedEffectiveMass? Substitute.
//(stiffness * dt^2 + damping * dt)^-1 * softenedEffectiveMass
//((effectiveMass * naturalFrequency^2) * dt^2 + (effectiveMass * 2 * dampingRatio * naturalFrequency * dt))^-1 * softenedEffectiveMass
//Combine terms:
//(effectiveMass * (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt))^-1 * softenedEffectiveMass
//Apply inverse:
//(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1 * softenedEffectiveMass
//Expand softened effective mass from earlier:
//(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1 * effectiveMass * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1
//Cancel effective masses: (!)
//(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1
//Because CFM was created from effectiveMass, the CFM/dt * effectiveMass term is actually independent of the effectiveMass!
//The remaining expression is still a matrix, but fortunately it is a simple uniform scaling matrix that we can store and apply as a single scalar.
//4) How do you compute ERP?
//ERP = (stiffness * dt) * CFM
//ERP = (stiffness * dt) * (stiffness * dt + damping)^-1
//ERP = ((effectiveMass * naturalFrequency^2) * dt) * ((effectiveMass * naturalFrequency^2) * dt + (effectiveMass * 2 * dampingRatio * naturalFrequency))^-1
//Combine denominator terms:
//ERP = ((effectiveMass * naturalFrequency^2) * dt) * ((effectiveMass * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency))^-1
//Apply denominator inverse:
//ERP = ((effectiveMass * naturalFrequency^2) * dt) * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1 * effectiveMass^-1
//Uniform scaling matrices commute:
//ERP = (naturalFrequency^2 * dt) * effectiveMass * effectiveMass^-1 * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1
//Cancellation!
//ERP = (naturalFrequency^2 * dt) * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1
//ERP = (naturalFrequency * dt) * (naturalFrequency * dt + 2 * dampingRatio)^-1
//ERP is a simple scalar, independent of mass.
//5) So we can compute CFM, ERP, the softened effective mass matrix, and we have an interesting shortcut on the constraint force mixing term of the solve iterations.
//Is there anything more that can be done? You bet!
//Let's look at the post-distribution impulse computation again:
//impulse = bias * effectiveMass - accumulatedImpulse * CFM/dt * effectiveMass - wsv * JT * effectiveMass
//During the solve iterations, the only quantities that vary are the accumulated impulse and world space velocities. So the rest can be precomputed.
//bias * effectiveMass,
//CFM/dt * effectiveMass,
//JT * effectiveMass
//In other words, we bypass the intermediate velocity state and go directly from source velocities to an impulse.
//Note the sizes of the precomputed types above:
//bias * effective mass is the same size as bias (vector with dimension equal to constrained DOFs)
//CFM/dt * effectiveMass is a single scalar regardless of constrained DOFs,
//JT * effectiveMass is the same size as JT
//But note that we no longer need to load the effective mass! It is implicit.
//The resulting computation is:
//impulse = a - accumulatedImpulse * b - wsv * c
//two DOF-width adds (add/subtract), one DOF-width multiply, and a 1xDOF * DOFx12 jacobian-sized transform.
//Compare to;
//(bias - accumulatedImpulse * CFM/dt - wsv * JT) * effectiveMass
//two DOF-width adds (add/subtract), one DOF width multiply, a 1xDOF * DOFx12 jacobian-sized transform, and a 1xDOF * DOFxDOF transform.
//In other words, we shave off a whole 1xDOF * DOFxDOF transform per iteration.
//So, taken in isolation, this is a strict win both in terms of memory and the amount of computation.
//Unfortunately, it's not quite so simple- jacobians are ALSO used to transform the impulse into world space so that it can be used to change the body velocities.
//We still need to have those around. So while we no longer store the effective mass, our jacobian has sort of been duplicated.
//But wait, there's more!
//That process looks like:
//wsv += impulse * J * M^-1
//So while we need to store something here, we can take advantage of the fact that we aren't using the jacobian anywhere else (it's replaced by the JT * effectiveMass term above).
//Precompute J*M^-1, too.
//So you're still loading a jacobian-sized matrix, but you don't need to load M^-1! That saves you 14 scalars. (symmetric 3x3 + 1 + symmetric 3x3 + 1)
//That saves you the multiplication of (impulse * J) * M^-1, which is 6 multiplies and 6 dot products.
//Note that this optimization's value depends on the number of constrained DOFs.
//Net memory change, opt vs no opt, in scalars:
//1DOF: costs 1x12, saves 1x1 effective mass and the 14 scalar M^-1: -3
//2DOF: costs 2x12, saves 2x2 symmetric effective mass and the 14 scalar M^-1: 7
//3DOF: costs 3x12, saves 3x3 symmetric effective mass and the 14 scalar M^-1: 16
//4DOF: costs 4x12, saves 4x4 symmetric effective mass and the 14 scalar M^-1: 24
//5DOF: costs 5x12, saves 5x5 symmetric effective mass and the 14 scalar M^-1: 31
//6DOF: costs 6x12, saves 6x6 symmetric effective mass and the 14 scalar M^-1: 37
//Net compute savings, opt vs no opt:
//DOF savings = 1xDOF * DOFxDOF (DOF DOFdot products), 2 1x3 * scalar (6 multiplies), 2 1x3 * 3x3 (6 3dot products)
// = (DOF*DOF multiplies + DOF*(DOF-1) adds) + (6 multiplies) + (18 multiplies + 12 adds)
// = DOF*DOF + 24 multiplies, DOF*DOF-DOF + 12 adds
//1DOF: 25 multiplies, 12 adds
//2DOF: 28 multiplies, 14 adds
//3DOF: 33 multiplies, 18 adds
//4DOF: 40 multiplies, 24 adds
//5DOF: 49 multiplies, 32 adds
//6DOF: 60 multiplies, 42 adds
//So does our 'optimization' actually do anything useful?
//In 1 DOF constraints, it's often a win with no downsides.
//2+ are difficult to determine.
//This depends on heavily on the machine's SIMD width. You do every lane's ALU ops in parallel, but the loads are still fundamentally bound by memory bandwidth.
//The loads are coherent, at least- no gathers on this stuff. But I wouldn't be surprised if 3DOF+ constraints end up being faster *without* the pretransformations on wide SIMD.
//This is just something that will require case by case analysis. Constraints can have special structure which change the judgment.
//(Also, note that large DOF jacobians are often very sparse. Consider the jacobians used by a 6DOF weld joint. You could likely do special case optimizations to reduce the
//load further. It is unlikely that you could find a way to do the same to JT * effectiveMass. J * M^-1 might have some savings, though. But J*M^-1 isn't *sparser*
//than J by itself, so the space savings are limited. As long as you precompute, the above load requirement offset will persist.)
//Good news, though! There are a lot of constraints where this trick is applicable.
//We'll start with the unsoftened effective mass, constructed from the contributions computed above:
var effectiveMass = Vector <float> .One / (linearA + linearB + angularA + angularB);
SpringSettingsWide.ComputeSpringiness(springSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);
var softenedEffectiveMass = effectiveMass * effectiveMassCFMScale;
//Note that we use a bit of a hack when computing the bias velocity- even if our damping ratio/natural frequency implies a strongly springy response
//that could cause a significant velocity overshoot, we apply an arbitrary clamping value to keep it reasonable.
//This is useful for a variety of inequality constraints (like contacts) because you don't always want them behaving as true springs.
var biasVelocity = Vector.Min(positionError * positionErrorToVelocity, maximumRecoveryVelocity);
projection.BiasImpulse = biasVelocity * softenedEffectiveMass;
//Precompute the wsv * (JT * softenedEffectiveMass) term.
//Note that we store it in a Vector3Wide as if it's a row vector, but this is really a column (because JT is a column vector).
//So we're really storing (JT * softenedEffectiveMass)T = softenedEffectiveMassT * J.
//Since this constraint is 1DOF, the softenedEffectiveMass is a scalar and the order doesn't matter.
//In the solve iterations, the WSVtoCSI term will be transposed during transformation,
//resulting in the proper wsv * (softenedEffectiveMassT * J)T = wsv * (JT * softenedEffectiveMass).
//You'll see this pattern repeated in higher DOF constraints. We explicitly compute softenedEffectiveMassT * J, and then apply the transpose in the solves.
//(Why? Because creating a Matrix3x2 and Matrix2x3 and 4x3 and 3x4 and 5x3 and 3x5 and so on just doubles the number of representations with little value.)
Vector3Wide.Scale(jacobians.LinearA, softenedEffectiveMass, out projection.WSVtoCSILinearA);
Vector3Wide.Scale(jacobians.AngularA, softenedEffectiveMass, out projection.WSVtoCSIAngularA);
Vector3Wide.Scale(jacobians.LinearB, softenedEffectiveMass, out projection.WSVtoCSILinearB);
Vector3Wide.Scale(jacobians.AngularB, softenedEffectiveMass, out projection.WSVtoCSIAngularB);
}
public static void Solve(ref Projection2Body1DOF projection, ref Vector <float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulse, out var correctiveCSI);
ApplyImpulse(ref projection, ref correctiveCSI, ref wsvA, ref wsvB);
}
public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Projection2Body1DOF projection, ref Vector <float> accumulatedImpulse,
out Vector <float> correctiveCSI)
{
//Take the world space velocity of each body into constraint space by transforming by the transpose(jacobian).
//(The jacobian is a row vector by convention, while we treat our velocity vectors as a 12x1 row vector for the purposes of constraint space velocity calculation.
//So we are multiplying v * JT.)
//Then, transform it into an impulse by applying the effective mass.
//Here, we combine the projection and impulse conversion into a precomputed value, i.e. v * (JT * softenedEffectiveMass).
Vector3Wide.Dot(wsvA.Linear, projection.WSVtoCSILinearA, out var csiaLinear);
Vector3Wide.Dot(wsvA.Angular, projection.WSVtoCSIAngularA, out var csiaAngular);
Vector3Wide.Dot(wsvB.Linear, projection.WSVtoCSILinearB, out var csibLinear);
Vector3Wide.Dot(wsvB.Angular, projection.WSVtoCSIAngularB, out var csibAngular);
//Combine it all together, following:
//constraint space impulse = (targetVelocity - currentVelocity) * softenedEffectiveMass
//constraint space impulse = (bias - accumulatedImpulse * softness - wsv * JT) * softenedEffectiveMass
//constraint space impulse = (bias * softenedEffectiveMass) - accumulatedImpulse * (softness * softenedEffectiveMass) - wsv * (JT * softenedEffectiveMass)
var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular);
var previousAccumulated = accumulatedImpulse;
accumulatedImpulse = Vector.Max(Vector <float> .Zero, accumulatedImpulse + csi);
correctiveCSI = accumulatedImpulse - previousAccumulated;
}
public static void WarmStart(ref Projection2Body1DOF data, ref Vector <float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
//TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore.
//To compensate for this, the accumulated impulse should be scaled if dt changes.
ApplyImpulse(ref data, ref accumulatedImpulse, ref wsvA, ref wsvB);
}