public void TestPointHashing()
{
GeometryCollection geometry = new GeometryCollection();
geometry.Add(new Vector2(0, 0));
geometry.Add(new Vector2(0, 0));
Assert.AreEqual(1, geometry.GetPointsAsArray().Length);
}
public void TestLinesAttaching()
{
GeometryCollection geometry = new GeometryCollection();
geometry.Add(new Line(new Vector2(-1, 0), new Vector2(0, 0)));
geometry.Add(new Line(new Vector2(0, 0), new Vector2(1, 0)));
Assert.AreEqual(1, geometry.LinesAttachedTo(new Vector2(-1, 0)).Count());
Assert.AreEqual(2, geometry.LinesAttachedTo(new Vector2(0, 0)).Count());
Assert.AreEqual(1, geometry.LinesAttachedTo(new Vector2(1, 0)).Count());
}
public void TestDisconnectReturnsNull()
{
GeometryCollection geometry = new GeometryCollection();
Line line = new Line(new Vector2(-1, 0), new Vector2(1, 0));
geometry.Add(line);
geometry.Add(new Vector2(2, 0));
BoundParticle particle = new BoundParticle(line, false, 0.5, 0, 0);
Path path = PathFinding.PathTo(particle, new Vector2(2, 0), geometry, 0.1f, 0.1f);
Assert.IsNull(path);
}
public void TestLineHashing()
{
GeometryCollection geometry = new GeometryCollection();
geometry.Add(new Line(new Vector2(-1, 0), new Vector2(1, 0)));
geometry.Add(new Line(new Vector2(-1, 0), new Vector2(1, 0)));
geometry.Add(new Line(new Vector2(1, 0), new Vector2(-1, 0)));
geometry.Add(new Line(new Vector2(1, 0), new Vector2(-1, 0)));
Assert.AreEqual(2, geometry.GetPointsAsArray().Length);
Assert.AreEqual(2, geometry.GetLinesAsIndexArray().Length);
}
private static Boolean ContainsAnyState(Dictionary<PathCriticalPoint, PointFunction> dict, Point p, GeometryCollection geom, int key)
{
foreach(var line in geom.LinesAttachedTo(p))
{
PathCriticalPoint state = new PathCriticalPoint(line, line.p1.Equals(p));
if(dict.ContainsKey(state) && dict[state].timings.ContainsKey(key))
{
return true;
}
}
return false;
}
public void TestBowlSwing()
{
GeometryCollection geometry = new GeometryCollection();
Line line = new Line(new Vector2(1, 0), new Vector2(-1, 0));
geometry.Add(line);
geometry.Add(new Line(new Vector2(-2, 1), new Vector2(-1, 0)));
geometry.Add(new Line(new Vector2(2, 1), new Vector2(1, 0)));
// Shape looks like \_._/*
BoundParticle particle = new BoundParticle(line, false, 0.5, 0, 0);
Path path = PathFinding.PathTo(particle, new Vector2(2, 1), geometry, 0.1f, -0.2f);
Assert.IsNotNull(path);
}
public void TestBowlSlowdown()
{
GeometryCollection geometry = new GeometryCollection();
Line line = new Line(new Vector2(1, 0), new Vector2(-1, 0));
geometry.Add(line);
geometry.Add(new Line(new Vector2(-2, 1), new Vector2(-1, 0)));
geometry.Add(new Line(new Vector2(2, 1), new Vector2(1, 0)));
// Shape looks like \_._/*
BoundParticle particle = new BoundParticle(line, false, 0.5, 0, 0);
Path path = PathFinding.PathTo(particle, new Vector2(2, 1), geometry, 0.1f, -0.1f);
// Exact answer: √(80+120√2)-√40
AssertExtra.AreApproximate(9.4775213623, path.totalTime);
}
private static Path BestOfAnyState(Dictionary<PathCriticalPoint, PointFunction> dict, Point p, GeometryCollection geom, int key)
{
Path best = null;
foreach (var line in geom.LinesAttachedTo(p))
{
PathCriticalPoint state = new PathCriticalPoint(line, line.p1.Equals(p));
if (dict.ContainsKey(state) && dict[state].timings.ContainsKey(key))
{
if(best==null || dict[state].timings[key].totalTime>best.totalTime)
{
best = dict[state].timings[key];
}
}
}
return best;
}
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 void TestAngledImpacts()
{
// Try all four rotations around the origin
foreach(int xSign in new []{-1, 1})
{
foreach (int ySign in new[] { -1, 1 })
{
GeometryCollection geometry = new GeometryCollection();
Line line = new Line(new Vector2(0, 0), new Vector2(xSign, ySign));
geometry.Add(line);
// position just 0.5 above the line
Particle p = new Particle(new Vector2(0.5f * xSign, (ySign + 1) / 2), new Vector2(0, -1), new Vector2(0, -0.1f));
BoundParticle impact = geometry.FirstCollision(p);
Assert.AreEqual<Line>(line, impact.boundTo);
AssertExtra.AreApproximate(0.5, impact.g);
AssertExtra.AreApproximate(-ySign * 0.5, impact.gv);
AssertExtra.AreApproximate(-ySign * 0.5 / 10, impact.ga);
}
}
}
public void TestMultipleFlatLines()
{
int n = 10; // half the total number of points
GeometryCollection geometry = new GeometryCollection();
Line line = new Line(new Vector2(-1, 0), new Vector2(1, 0));
geometry.Add(line);
MathExp.Geometry.Point target = null;
for(int i=1; i<n; i++)
{
MathExp.Geometry.Point rightMostPoint = new Vector2(i + 1, 0);
Line leftLine = new Line(new Vector2(i, 0), rightMostPoint);
Line rightLine = new Line(new Vector2(-i, 0), new Vector2(-i - 1, 0));
geometry.Add(leftLine);
geometry.Add(rightLine);
target = rightMostPoint;
}
BoundParticle particle = new BoundParticle(line, false, 0.5, 0, 0);
Path path = PathFinding.PathTo(particle, target, geometry, 0.1f, 0.1f);
AssertExtra.AreApproximate(path.totalTime, 2 * Math.Sqrt(10 * n));
for (float t = 0; t < path.totalTime; t += 0.01f)
{
Vector2 pos = path.absolutePosAt(t);
AssertExtra.AreApproximate(0, pos.Y);
if (t < Math.Sqrt(10*n))
{
// when speeding up to approach
AssertExtra.AreApproximate(0.1 * t * t / 2, pos.X);
}
else
{
// when slowing down to stop
double g = t - Math.Sqrt(10*n);
AssertExtra.AreApproximate(-0.1 * g * g / 2 + Math.Sqrt(10*n) * 0.1 * g + 0.5*n, pos.X);
}
}
}
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);
}
}
}
// Load
public LineEnvironment(string file)
{
if (File.Exists(file)) {
Stream stream = new FileStream(file, FileMode.Open, FileAccess.Read, FileShare.None);
using (BinaryReader reader = new BinaryReader(stream))
{
geometry = new GeometryCollection(reader.ReadVertices(Color.White).ToList(), reader.ReadShorts().ToList());
}
} else
{
geometry = new GeometryCollection();
}
mouseListener = new MouseListener();
mouseListener.LeftButtonDrag = (start, end) =>
{
selected.Position = end;
};
mouseListener.RightButtonDrag = (start, end) =>
{
var selected2 = geometry.SnapToClosePoint(end);
drawnGeometry = new GeometryCollection();
if(selected2 == null)
{
drawnGeometry.Add(selected, new VertexPositionColor(end, Color.Red));
}else
{
drawnGeometry.Add(selected, selected2);
}
};
mouseListener.RightButtonRelease = (pos) =>
{
drawnGeometry = new GeometryCollection();
if (selected.isNull)
{
geometry.Add(new VertexPositionColor(pos, Color.White));
}else
{
var selected2 = geometry.SnapToClosePoint(pos);
if(selected2 == (MathExp.Geometry.Point)selected)
{
geometry.Remove(selected);
}
else
{
geometry.Add(selected, selected2);
}
}
};
mouseListener.Move = (start, end) =>
{
selected.Color = Color.White;
selected = geometry.SnapToClosePoint(end);
selected.Color = Color.Red;
};
mouseListener.LeftButtonRelease = (pos) =>
{
if (selected.isNull)
{
player.position = new Vector2((float)Math.Round(pos.X/10)*10, (float)Math.Round(pos.Y/10)*10);
player.velocity = Vector2.Zero;
isBound = false;
}
};
}
public void TestSingleFlatLine()
{
GeometryCollection geometry = new GeometryCollection();
Line line = new Line(new Vector2(-1, 0), new Vector2(1, 0));
geometry.Add(line);
BoundParticle particle = new BoundParticle(line, false, 0.5, 0, 0);
Path path = PathFinding.PathTo(particle, new Vector2(1, 0), geometry, 0.1f, 0.1f);
AssertExtra.AreApproximate(2 * Math.Sqrt(10), path.totalTime);
for (float t = 0; t < path.totalTime; t += 0.01f)
{
Vector2 pos = path.absolutePosAt(t);
AssertExtra.AreApproximate(0, pos.Y);
if (t < Math.Sqrt(10))
{
// when speeding up to approach
AssertExtra.AreApproximate(0.1 * t * t / 2, pos.X);
}else
{
// when slowing down to stop
double g = t - Math.Sqrt(10);
AssertExtra.AreApproximate(-0.1 * g * g / 2 + Math.Sqrt(10)*0.1*g+0.5, pos.X);
}
}
}