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]);
}
}
}
}
}
