public static Path PathTo(BoundParticle start, Point end, GeometryCollection geometry, float accel, float gravity)
{
// construct a new critical point
Point newPoint = start.position();
Line line1 = new Line(newPoint, start.boundTo.p1);
Line line2 = new Line(newPoint, start.boundTo.p2);
geometry.Add(line1);
geometry.Add(line2);
geometry.Remove(start.boundTo);
Queue<PathCriticalPoint> bfs = new Queue<PathCriticalPoint>();
// map time function for two attached points
Dictionary<PathCriticalPoint, PointFunction> dict = new Dictionary<PathCriticalPoint, PointFunction>();
PathCriticalPoint state1 = new PathCriticalPoint(line1, true);
PathCriticalPoint state2 = new PathCriticalPoint(line2, true);
dict[state1] = new PointFunction(0);
dict[state2] = new PointFunction(0);
bfs.Enqueue(state1);
bfs.Enqueue(state2);
// bfs will simply search through steps, and will not continue a branch if it doesn't improve anything
while (bfs.Count > 0)
{
var head = bfs.Dequeue();
// TODO: currently not going back to previously visited points, as the logic isn't correct for it
var currPoint = head.firstPoint ? head.line.p1 : head.line.p2;
foreach (var nextLine in geometry.LinesAttachedTo(currPoint).Where(l => !l.Equals(head.line)))
{
var nextPoint = (nextLine.p1.Equals(currPoint)) ? nextLine.p2 : nextLine.p1;
var nextState = new PathCriticalPoint(nextLine, nextLine.p1.Equals(nextPoint));
PointFunction timings = new PointFunction(currPoint, nextPoint, dict[head], accel, gravity);
WorkWithTimings(end, geometry, bfs, dict, nextState, timings);
// also attempt to return along that path
var nextState2 = new PathCriticalPoint(nextLine, nextLine.p1.Equals(currPoint));
PointFunction timings2 = PointFunction.Return(currPoint, nextPoint, dict[head], accel, gravity);
WorkWithTimings(end, geometry, bfs, dict, nextState2, timings2);
}
}
Path answer = null;
if (ContainsAnyState(dict, end, geometry, 0))
{
answer = BestOfAnyState(dict, end, geometry, 0);
}
geometry.Remove(newPoint);
geometry.Add(start.boundTo);
return answer;
}
public PointFunction(Geometry.Point p1, Geometry.Point p2, PointFunction old, float accel, float gravity)
{
Vector2 path = ((Vector2)p2-(Vector2)p1);
Vector2 normalized = path;
normalized.Normalize();
float g = gravity * path.Y / path.Length();
// r and l have been specifically framed this way, and affect several equations
float r = g + accel;
float l = -g + accel;
float d = path.Length();
foreach(var pair in old.timings)
{
// is positive if it assists acceleration towards p2 (aka right)
float b = FromKey(pair.Key);
// collisions reduce speed
Vector2 prevPath = (Vector2)p1 - pair.Value.posAt(0);
if (prevPath != Vector2.Zero)
{
b *= Math.Abs(Vector2.Dot(path, prevPath)) / (path.Length() * prevPath.Length());
}
Parabola fullSpeedPath = new Parabola(r, b, 0);
Parabola fullRetreatPath = new Parabola(-l, b, 0);
Paraboloid speedThenRetreat = fullSpeedPath.FollowedBy(fullRetreatPath);
int lowerBound = ToKey(fullRetreatPath.SpeedAt(d, 0));
int upperBound = ToKey(fullSpeedPath.SpeedAt(d, 0));
for (int i = lowerBound; i <= upperBound; i++)
{
float f = FromKey(i);
float newPartialTime = speedThenRetreat.SpecialShit(d, f, -1);
if (newPartialTime >= 0)
{
float newT = newPartialTime + pair.Value.totalTime;
if (!timings.ContainsKey(i) || timings[i].totalTime > newT)
{
float rT = (l * newPartialTime + f - b) / (l + r);
float lT = newPartialTime - rT;
timings[i] = new Path(newT-lT, p1, b * normalized, r * normalized, pair.Value);
timings[i] = new Path(newT, timings[i].posAt(rT), timings[i].vAt(rT), -l * normalized, timings[i]);
}
}
}
}
}
private static void WorkWithTimings(Point end, GeometryCollection geometry, Queue<PathCriticalPoint> bfs, Dictionary<PathCriticalPoint, PointFunction> dict, PathCriticalPoint nextState, PointFunction timings)
{
if (dict.ContainsKey(nextState))
{
double lowestChange = dict[nextState].improved(timings);
if (!double.IsInfinity(lowestChange))
{
if (!ContainsAnyState(dict, end, geometry, 0) || lowestChange < BestOfAnyState(dict, end, geometry, 0).totalTime)
{
bfs.Enqueue(nextState);
}
}
}
else
{
double lowestChange = (timings.timings.Count > 0) ? timings.timings.OrderBy(p => p.Value.totalTime).First().Value.totalTime : double.PositiveInfinity;
dict[nextState] = timings;
if (!ContainsAnyState(dict, end, geometry, 0) || lowestChange < BestOfAnyState(dict, end, geometry, 0).totalTime)
{
bfs.Enqueue(nextState);
}
}
}
internal static PointFunction Return(Geometry.Point p1, Geometry.Point p2, PointFunction old, float accel, float gravity)
{
// either you accelerate fully away and then return to get a higher speed, or you try to backpedel and then brace yourself to get a lower speed
PointFunction answer = new PointFunction();
Vector2 path = ((Vector2)p2 - (Vector2)p1);
Vector2 normalized = path;
normalized.Normalize();
float g = gravity * path.Y / path.Length();
// r and l have been specifically framed this way, and affect several equations
float r = g + accel;
float l = -g + accel;
float d = (p1.v.Position - p2.v.Position).Length();
foreach (var pair in old.timings)
{
// is positive if it assists acceleration towards p2 (aka right)
float b = FromKey(pair.Key);
// collisions reduce speed
Vector2 l1 = (Vector2)p2 - (Vector2)p1;
Vector2 l2 = (Vector2)p1 - pair.Value.posAt(0);
if (l2 != Vector2.Zero)
{
b *= Math.Abs(Vector2.Dot(l1, l2)) / (l1.Length() * l2.Length());
}
// old
/*int lowerBound = ToKey(b);
int upperBound = ToKey(Math.Sqrt(Math.Max(b * b + 2 * r * d, 0)));
float newPartialTime = 2 * b / l;
if (true)
{
float newT = newPartialTime + pair.Value.totalTime;
int i = ToKey(b);
if (!answer.timings.ContainsKey(i) || answer.timings[i].totalTime > newT)
{
answer.timings[i] = new Path(newT, p1, b * normalized, -l * normalized, pair.Value);
}
}*/
// if trying to speed up first
Parabola fullSpeedPath = new Parabola(r, b, 0);
Parabola fullRetreatPath = new Parabola(-l, b, 0);
Paraboloid speedThenRetreat = fullSpeedPath.FollowedBy(fullRetreatPath);
int lowerBound = ToKey(b);
int upperBound = ToKey(Math.Sqrt(2*l*d));
//int upperBound = ToKey();
for (int i = lowerBound; i <= upperBound; i++)
{
float f = FromKey(i);
float newPartialTime = speedThenRetreat.SpecialShit(0, -f, -1);
if (newPartialTime >= 0)
{
float newT = newPartialTime + pair.Value.totalTime;
if (!answer.timings.ContainsKey(i) || answer.timings[i].totalTime > newT)
{
//float rT = newPartialTime * 0.27f;
//float rT = (l * newPartialTime + f - b) / (l + r);
//f=b+rt-lg
//T=t+g
//lT+f=b+rt+lt
//t=(lT+f-b)/(l+r)
float rT = (l * newPartialTime - f - b) / (l + r);
float lT = newPartialTime - rT;
answer.timings[i] = new Path(newT - lT, p1, b * normalized, r * normalized, pair.Value);
answer.timings[i] = new Path(newT, answer.timings[i].posAt(rT), answer.timings[i].vAt(rT), -l * normalized, answer.timings[i]);
}
}
}
}
return answer;
}
internal double improved(PointFunction newTimings)
{
double improved = double.PositiveInfinity;
foreach (var pair in newTimings.timings)
{
if (!timings.ContainsKey(pair.Key) || timings[pair.Key].totalTime > pair.Value.totalTime)
{
timings[pair.Key] = pair.Value;
improved = Math.Min(pair.Value.totalTime, improved);
}
}
return improved;
}
