public Fix64 GetPossibleAirTime(Fix64 appliedForce)
{
Fix64 maxHeight = (appliedForce * appliedForce) / (appliedGravity * 2);
maxHeight += worldTransform.position.y;
return(FPMath.Sqrt(maxHeight * 2 / appliedGravity));
}
private void setVerticalData(Fix64 appliedForce)
{
Fix64 maxHeight = (appliedForce * appliedForce) / (appliedGravity * 2);
maxHeight += worldTransform.position.y;
airTime = FPMath.Sqrt(maxHeight * 2 / appliedGravity);
verticalTotalForce = appliedGravity * airTime;
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
effectiveMass = body1.invInertiaWorld + body2.invInertiaWorld;
softnessOverDt = softness / timestep;
effectiveMass.M11 += softnessOverDt;
effectiveMass.M22 += softnessOverDt;
effectiveMass.M33 += softnessOverDt;
FPMatrix.Inverse(ref effectiveMass, out effectiveMass);
FPMatrix orientationDifference;
FPMatrix.Multiply(ref initialOrientation1, ref initialOrientation2, out orientationDifference);
FPMatrix.Transpose(ref orientationDifference, out orientationDifference);
FPMatrix q = orientationDifference * body2.invOrientation * body1.orientation;
FPVector axis;
FP x = q.M32 - q.M23;
FP y = q.M13 - q.M31;
FP z = q.M21 - q.M12;
FP r = FPMath.Sqrt(x * x + y * y + z * z);
FP t = q.M11 + q.M22 + q.M33;
FP angle = FP.Atan2(r, t - 1);
axis = new FPVector(x, y, z) * angle;
if (r != FP.Zero)
{
axis = axis * (FP.One / r);
}
bias = axis * biasFactor * (-FP.One / timestep);
// Apply previous frame solution as initial guess for satisfying the constraint.
if (!body1.IsStatic)
{
body1.angularVelocity += FPVector.Transform(accumulatedImpulse, body1.invInertiaWorld);
}
if (!body2.IsStatic)
{
body2.angularVelocity += FPVector.Transform(-FP.One * accumulatedImpulse, body2.invInertiaWorld);
}
}
public static FPVector2 MoveTowards(
FPVector2 current,
FPVector2 target,
pfloat maxDistanceDelta)
{
pfloat num1 = target.x - current.x;
pfloat num2 = target.y - current.y;
pfloat num4 = (num1 * num1 + num2 * num2);
if (num4 == 0f || maxDistanceDelta >= 0f && num4 <= maxDistanceDelta * maxDistanceDelta)
{
return(target);
}
var num5 = FPMath.Sqrt(num4);
return(new FPVector2(current.x + num1 / num5 * maxDistanceDelta, current.y + num2 / num5 * maxDistanceDelta));
}
public override FP ComputeSubmergedArea(ref FPVector2 normal, FP offset, ref Transform xf, out FPVector2 sc)
{
sc = FPVector2.zero;
FPVector2 p = MathUtils.Mul(ref xf, Position);
FP l = -(FPVector2.Dot(normal, p) - offset);
if (l < -Radius + Settings.Epsilon)
{
//Completely dry
return(0);
}
if (l > Radius)
{
//Completely wet
sc = p;
return(Settings.Pi * _2radius);
}
//Magic
FP l2 = l * l;
FP area = _2radius * (FP)((FPMath.Asin((l / Radius)) + FPMath.PiOver2) + l * FPMath.Sqrt(_2radius - l2));
// TODO - PORT
//FP com = -2.0f / 3.0f * (FP)Math.Pow(_2radius - l2, 1.5f) / area;
FP com = new FP(-2) / new FP(3) * (FP)Math.Pow((_2radius - l2).AsFloat(), 1.5f) / area;
sc.x = p.x + normal.x * com;
sc.y = p.y + normal.y * com;
return(area);
}
public static pfloat Angle(FPVector2 from, FPVector2 to)
{
var num = FPMath.Sqrt(from.sqrMagnitude * to.sqrMagnitude);
return((float)num < 1.0000000036274937E-15 ? pfloat.Zero : FPMath.ACos(FPMath.Clamp(FPVector2.Dot(from, to) / num, -pfloat.One, pfloat.One)) * (pfloat)57.29578f);
}
