public static TSVector2 Lerp(TSVector2 value1, TSVector2 value2, FP amount)
{
amount = TSMath.Clamp(amount, 0, 1);
return(new TSVector2(
TSMath.Lerp(value1.x, value2.x, amount),
TSMath.Lerp(value1.y, value2.y, amount)));
}
public static TSQuaternion Slerp(TSQuaternion from, TSQuaternion to, FP t)
{
t = TSMath.Clamp(t, 0, 1);
FP dot = Dot(from, to);
if (dot < 0.0f)
{
to = Multiply(to, -1);
dot = -dot;
}
FP halfTheta = FP.Acos(dot);
return(Multiply(Multiply(from, FP.Sin((1 - t) * halfTheta)) + Multiply(to, FP.Sin(t * halfTheta)), 1 / FP.Sin(halfTheta)));
}
internal void Solve(Manifold manifold)
{
VoltBody bodyA = manifold.ShapeA.Body;
VoltBody bodyB = manifold.ShapeB.Body;
FP elasticity = bodyA.World.Elasticity;
// Calculate relative bias velocity
TSVector2 vb1 = bodyA.BiasVelocity + (bodyA.BiasRotation * this.toALeft);
TSVector2 vb2 = bodyB.BiasVelocity + (bodyB.BiasRotation * this.toBLeft);
FP vbn = TSVector2.Dot((vb1 - vb2), this.normal);
// Calculate and clamp the bias impulse
FP jbn = this.nMass * (vbn - this.bias);
jbn = TSMath.Max(-this.jBias, jbn);
this.jBias += jbn;
// Apply the bias impulse
this.ApplyNormalBiasImpulse(bodyA, bodyB, jbn);
// Calculate relative velocity
TSVector2 vr = this.RelativeVelocity(bodyA, bodyB);
FP vrn = TSVector2.Dot(vr, this.normal);
// Calculate and clamp the normal impulse
FP jn = nMass * (vrn + (this.restitution * elasticity));
jn = TSMath.Max(-this.cachedNormalImpulse, jn);
this.cachedNormalImpulse += jn;
// Calculate the relative tangent velocity
FP vrt = TSVector2.Dot(vr, this.normal.Left());
// Calculate and clamp the friction impulse
FP jtMax = manifold.Friction * this.cachedNormalImpulse;
FP jt = vrt * tMass;
FP result = TSMath.Clamp(this.cachedTangentImpulse + jt, -jtMax, jtMax);
jt = result - this.cachedTangentImpulse;
this.cachedTangentImpulse = result;
// Apply the normal and tangent impulse
this.ApplyContactImpulse(bodyA, bodyB, jn, jt);
}
public static void Clamp(ref TSVector2 value1, ref TSVector2 min, ref TSVector2 max, out TSVector2 result)
{
result = new TSVector2(
TSMath.Clamp(value1.x, min.x, max.x),
TSMath.Clamp(value1.y, min.y, max.y));
}
public static TSVector2 Clamp(TSVector2 value1, TSVector2 min, TSVector2 max)
{
return(new TSVector2(
TSMath.Clamp(value1.x, min.x, max.x),
TSMath.Clamp(value1.y, min.y, max.y)));
}
public static TSQuaternion Lerp(TSQuaternion a, TSQuaternion b, FP t)
{
t = TSMath.Clamp(t, FP.Zero, FP.One);
return(LerpUnclamped(a, b, t));
}