public static float2 ClosestApproach(PhysicalState subject, PhysicalState target, float timeIncrement = 1f, int maxSteps = 3600)
{
var closestDistance = math.distance(subject.Position, target.Position);
var closestDistanceTime = 0f;
var totalTime = 0f;
var steps = 0;
while (steps < maxSteps && closestDistance > float.Epsilon)
{
steps += 1;
subject = PredictFutureState(subject, timeIncrement);
target = PredictFutureState(target, timeIncrement);
totalTime += timeIncrement;
var dist = math.distance(subject.Position, target.Position);
if (dist < closestDistance)
{
closestDistance = dist;
closestDistanceTime = totalTime;
}
}
return(new float2(closestDistance, closestDistanceTime));
}
public static PhysicalState PredictFutureState(PhysicalState state, float deltaTime)
{
return(new PhysicalState
{
Position = state.Position + Displacement(state.Velocity, state.Acceleration, deltaTime),
Velocity = state.Velocity + deltaTime * state.Acceleration,
Acceleration = state.Acceleration
});
}
public static InterceptData AccelerationToIntercept(PhysicalState subject, PhysicalState target, float accMag, float leadTimeIncrement = 1f, int maxSteps = 3600)
{
var leadTime = 0f;
var steps = 0;
var intercept = new InterceptData
{
Acceleration = float3.zero,
ClosestDistance = math.distance(subject.Position, target.Position),
TimeToIntercept = 0,
SubjectPos = subject.Position,
TargetPos = target.Position
};
while (steps < maxSteps && intercept.ClosestDistance > float.Epsilon)
{
steps += 1;
var targetPoint = PredictFutureState(target, leadTime).Position;
var acc = math.normalize(targetPoint - subject.Position) * accMag;
var ca = ClosestApproach(
new PhysicalState()
{
Position = subject.Position,
Velocity = subject.Velocity,
Acceleration = acc
},
target
);
var cd = ca.x;
var tti = ca.y;
if (cd < intercept.ClosestDistance)
{
intercept = new InterceptData
{
Acceleration = acc,
ClosestDistance = cd,
TimeToIntercept = tti,
SubjectPos = PredictFutureState(new PhysicalState()
{
Position = subject.Position,
Velocity = subject.Velocity,
Acceleration = acc
}, tti).Position,
TargetPos = PredictFutureState(target, tti).Position
};
}
leadTime += leadTimeIncrement;
}
return(intercept);
}