Skip to content

Commit

Permalink
Take methods from PointMath
Browse files Browse the repository at this point in the history
  • Loading branch information
@ApmeM
ApmeM committed Jul 21, 2023
1 parent fc60e92 commit eae7d07
Show file tree
Hide file tree
Showing 7 changed files with 148 additions and 159 deletions.
67 changes: 2 additions & 65 deletions BrainAI.Tests/PointMathTest.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,20 +7,6 @@
[TestFixture]
public class PointMathTest
{
[Test]
public void DoubledTriangleSquareBy3Dots_CW()
{
var result = PointMath.DoubledTriangleSquareBy3Dots(new Point(1, 0), new Point(0, 0), new Point(0, 1));
Assert.AreEqual(-1, result);
}

[Test]
public void DoubledTriangleSquareBy3Dots_CCW()
{
var result = PointMath.DoubledTriangleSquareBy3Dots(new Point(0, 1), new Point(0, 0), new Point(1, 0));
Assert.AreEqual(1, result);
}

[Test]
public void IsDirectionInsidePolygon_InsidePoligon()
{
Expand Down Expand Up @@ -60,58 +46,9 @@ public void IsDirectionInsidePolygon_InsideRect()
}

[Test]
public void CompareVectorsTest_EqualZeroVectors()
{
Assert.AreEqual(0, PointMath.CompareVectors(new Point(15, 15), new Point(15, 15), new Point(15, 15)));
Assert.AreEqual(1, PointMath.CompareVectors(new Point(15, 15), new Point(15, 15), new Point(-15, 15)));
Assert.AreEqual(-1, PointMath.CompareVectors(new Point(-15, 15), new Point(15, 15), new Point(15, 15)));
}

[Test]
public void CompareVectorsTest_ZeroY_CompareDirectionOfX()
{
Assert.AreEqual(-1, PointMath.CompareVectors(new Point(-15, 15), new Point(15, 15), new Point(20, 15)));
Assert.AreEqual(1, PointMath.CompareVectors(new Point(20, 15), new Point(15, 15), new Point(-15, 15)));
Assert.AreEqual(0, PointMath.CompareVectors(new Point(-15, 15), new Point(15, 15), new Point(-10, 15)));
}

[Test]
public void CompareVectorsTest_VectorsInDifferentHalfSphere_TopIsFirst()
{
Assert.AreEqual(-1, PointMath.CompareVectors(new Point(-16, -40), new Point(15, 15), new Point(17, 40)));
Assert.AreEqual(1, PointMath.CompareVectors(new Point(-19, 20), new Point(15, 15), new Point(18, -20)));
}

[Test]
public void CompareVectorsTest_VectorsInSameDirection_Equal()
{
Assert.AreEqual(0, PointMath.CompareVectors(new Point(10, 0), new Point(0, 0), new Point(9, 0)));
Assert.AreEqual(0, PointMath.CompareVectors(new Point(-10, -10), new Point(0, 0), new Point(-12, -12)));
Assert.AreEqual(0, PointMath.CompareVectors(new Point(-15, 15), new Point(15, 15), new Point(-10, 15)));
Assert.AreEqual(0, PointMath.CompareVectors(new Point(20, 15), new Point(15, 15), new Point(25, 15)));
Assert.AreEqual(0, PointMath.CompareVectors(new Point(15, -15), new Point(15, 15), new Point(15, -10)));
Assert.AreEqual(0, PointMath.CompareVectors(new Point(15, 20), new Point(15, 15), new Point(15, 25)));
}

[Test]
public void CompareVectorsTest_VectorsTopHalfSphere_RightIsFirst()
{
Assert.AreEqual(1, PointMath.CompareVectors(new Point(10, 0), new Point(0, 0), new Point(11, 1)));
Assert.AreEqual(-1, PointMath.CompareVectors(new Point(2, 3), new Point(0, 0), new Point(1, 1)));
}

[Test]
public void CompareVectorsTest_VectorsBottomHalfSphere_LeftIsFirst()
{
Assert.AreEqual(1, PointMath.CompareVectors(new Point(10, -50), new Point(0, 0), new Point(11, -20)));
Assert.AreEqual(-1, PointMath.CompareVectors(new Point(10, -50), new Point(0, 0), new Point(-11, -20)));
}

[Test]
public void CompareVectorsTest_OneVectorDirectedToZero()
public void FindEndPoint_ToCCW()
{
Assert.AreEqual(1, PointMath.CompareVectors(new Point(-2, 0), new Point(0, 0), new Point(10, -18)));
Assert.AreEqual(-1, PointMath.CompareVectors(new Point(10, -18), new Point(0, 0), new Point(-2, 0)));
Assert.AreEqual((0, new Point(10,30)), PointMath.FindEndPoint(new Point(15, 15), (0, new Point(10, 10)), new Point(0, 10), new Point(10, 30)));
}
}
}
2 changes: 1 addition & 1 deletion BrainAI/Pathfinding/Graphs/EdgesPointGraph.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ public class EdgesPointGraph : EdgesGraph<Point>, IAstarGraph<Point>
{
public int Heuristic(Point node, Point goal)
{
return (Math.Abs(node.X - goal.X) + Math.Abs(node.Y - goal.Y)) * this.DefaultWeight;
return (node - goal).ManhattanLength * this.DefaultWeight;
}
}
}
Expand Down
2 changes: 1 addition & 1 deletion BrainAI/Pathfinding/Graphs/GridGraph.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ public int Cost(Point from, Point to)

public int Heuristic(Point node, Point goal)
{
return Math.Abs(node.X - goal.X) + Math.Abs(node.Y - goal.Y);
return (node - goal).ManhattanLength * this.DefaultWeight;
}

private bool IsNodeInBounds(Point node)
Expand Down
6 changes: 3 additions & 3 deletions BrainAI/Pathfinding/Graphs/StrightEdgeGraph.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,10 +35,10 @@ public StrightEdgeGraph()
{
sortByAngleFromPoint = (((int, Point) first, (int, Point) second) =>
{
var result = PointMath.CompareVectors(first.Item2, wrapper.p, second.Item2);
var result = (first.Item2-wrapper.p).CompareTo(second.Item2-wrapper.p);
if (result == 0)
{
result = Math.Sign(PointMath.DistanceSquare(wrapper.p, first.Item2) - PointMath.DistanceSquare(wrapper.p, second.Item2));
result = Math.Sign((wrapper.p - first.Item2).LengthQuad - (wrapper.p - second.Item2).LengthQuad);
}
return result;

Expand Down Expand Up @@ -95,7 +95,7 @@ private void ApplyChanges()

obstacleConnections[(obstacle, point.Value)] = (pointPrev.Value, pointNext);

var triangleSquare = PointMath.DoubledTriangleSquareBy3Dots(point.Value, pointPrev.Value, pointNext);
var triangleSquare = (pointNext-pointPrev.Value).Cross(point.Value-pointPrev.Value);
totalAreaX2 += triangleSquare;

if (triangleSquare > 0)
Expand Down
56 changes: 24 additions & 32 deletions BrainAI/Pathfinding/Graphs/Utils/GridToStrightEdgeConverter.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,36 +27,28 @@ public void BuildGraph(GridGraph graph, StrightEdgeGraph result, int scale = 1)

list.Clear();

var p0 = list.AddLast(new Point(wall.X, wall.Y));
var p1 = list.AddLast(new Point(wall.X + 1, wall.Y));
var p2 = list.AddLast(new Point(wall.X + 1, wall.Y + 1));
var p3 = list.AddLast(new Point(wall.X, wall.Y + 1));
var p0 = list.AddLast(wall);
var p1 = list.AddLast(wall + Point.Right);
var p2 = list.AddLast(wall + Point.Right + Point.Down);
var p3 = list.AddLast(wall + Point.Down);

DFS(graph, visited, list, wall, new Point(1, 0), p1);
DFS(graph, visited, list, wall, new Point(-1, 0), p3);
DFS(graph, visited, list, wall, new Point(0, +1), p2);
DFS(graph, visited, list, wall, new Point(0, -1), p0);
DFS(graph, visited, list, wall, Point.Right, p1);
DFS(graph, visited, list, wall, Point.Left, p3);
DFS(graph, visited, list, wall, Point.Down, p2);
DFS(graph, visited, list, wall, Point.Up, p0);

Cleanup(list);

foreach (var point in list)
{
var newPoint = new Point(point.X * scale, point.Y * scale);
var newPoint = point * scale;
result.AddPoint(obstacle, newPoint);
}

obstacle++;
}
}

private static void Scale(List<Point> list, int scale)
{
for (int i = 0; i < list.Count; i++)
{
list[i] = new Point(list[i].X * scale, list[i].Y * scale);
}
}

private static void Cleanup(LinkedList<Point> list)
{
var node = list.First;
Expand All @@ -82,8 +74,8 @@ private static void Cleanup(LinkedList<Point> list)
var next = (node.Next ?? list.First).Value;
var cur = node.Value;

var dir1 = new Point(cur.X - prev.X, cur.Y - prev.Y);
var dir2 = new Point(next.X - cur.X, next.Y - cur.Y);
var dir1 = cur - prev;
var dir2 = next - cur;
if (
Math.Sign(dir1.X) == Math.Sign(dir2.X) &&
Math.Sign(dir1.Y) == Math.Sign(dir2.Y)
Expand All @@ -103,7 +95,7 @@ private static void Cleanup(LinkedList<Point> list)

private static void DFS(GridGraph graph, HashSet<Point> visited, LinkedList<Point> list, Point p, Point direction, LinkedListNode<Point> afterNode)
{
var wall = new Point(p.X + direction.X, p.Y + direction.Y);
var wall = p + direction;

if (!graph.Walls.Contains(wall) ||
visited.Contains(wall))
Expand All @@ -122,37 +114,37 @@ private static void DFS(GridGraph graph, HashSet<Point> visited, LinkedList<Poin
{
p0 = afterNode;
p3 = afterNode.Next;
p2 = list.AddAfter(afterNode, new Point(wall.X + 1, wall.Y + 1));
p1 = list.AddAfter(afterNode, new Point(wall.X + 1, wall.Y));
p2 = list.AddAfter(afterNode, wall + Point.Right + Point.Down);
p1 = list.AddAfter(afterNode, wall + Point.Right);
}
else if (direction.X < 0)
{
p2 = afterNode;
p1 = afterNode.Next;
p0 = list.AddAfter(afterNode, new Point(wall.X, wall.Y));
p3 = list.AddAfter(afterNode, new Point(wall.X, wall.Y + 1));
p0 = list.AddAfter(afterNode, wall);
p3 = list.AddAfter(afterNode, wall + Point.Down);
}
else if (direction.Y > 0)
{

p0 = afterNode.Next;
p1 = afterNode;
p3 = list.AddAfter(afterNode, new Point(wall.X, wall.Y + 1));
p2 = list.AddAfter(afterNode, new Point(wall.X + 1, wall.Y + 1));
p3 = list.AddAfter(afterNode, wall + Point.Down);
p2 = list.AddAfter(afterNode, wall + Point.Right + Point.Down);

}
else
{
p3 = afterNode;
p2 = afterNode.Next;
p1 = list.AddAfter(afterNode, new Point(wall.X + 1, wall.Y));
p0 = list.AddAfter(afterNode, new Point(wall.X, wall.Y));
p1 = list.AddAfter(afterNode, wall + Point.Right);
p0 = list.AddAfter(afterNode, wall);
}

DFS(graph, visited, list, wall, new Point(1, 0), p1);
DFS(graph, visited, list, wall, new Point(-1, 0), p3);
DFS(graph, visited, list, wall, new Point(0, +1), p2);
DFS(graph, visited, list, wall, new Point(0, -1), p0);
DFS(graph, visited, list, wall, Point.Right, p1);
DFS(graph, visited, list, wall, Point.Left, p3);
DFS(graph, visited, list, wall, Point.Down, p2);
DFS(graph, visited, list, wall, Point.Up, p0);
}
}
}
114 changes: 99 additions & 15 deletions BrainAI/Pathfinding/Point.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,39 +2,123 @@

namespace BrainAI.Pathfinding
{
public struct Point : IEquatable<Point>
public struct Point : IEquatable<Point>, IComparable<Point>
{
public static Point Zero = new Point(0, 0);
public static Point Left = new Point(-1, 0);
public static Point Right = new Point(1, 0);
public static Point Up = new Point(0, -1);
public static Point Down = new Point(0, 1);

public int X;
public int Y;

public Point(int x, int y)
{
X = x;
Y = y;
this.X = x;
this.Y = y;
}

public int ManhattanLength => Math.Abs(this.X) + Math.Abs(this.Y);
public int Length => (int)Math.Sqrt(this.LengthQuad);

public int LengthQuad => this.X * this.X + this.Y * this.Y;

public Point Normalize() => this / this.Length;

public int Angle => (int)Math.Atan2(this.Y, this.X);

public static Point operator +(Point a, Point b)
=> new Point(a.X + b.X, a.Y + b.Y);

public static Point operator -(Point a, Point b)
=> new Point(a.X - b.X, a.Y - b.Y);

public static Point operator -(Point a)
=> new Point(-a.X, -a.Y);

public static Point operator *(Point a, int b)
=> new Point(a.X * b, a.Y * b);

public static Point operator *(int b, Point a)
=> new Point(a.X * b, a.Y * b);

public static Point operator /(Point a, int b)
=> new Point(a.X / b, a.Y / b);

public static Point operator /(int b, Point a)
=> new Point(a.X / b, a.Y / b);

public static bool operator ==(Point a, Point b)
=> a.X == b.X && a.Y == b.Y;

public static bool operator !=(Point a, Point b)
=> !(a == b);

public int Dot(Point p2)
{
return this.X * p2.X + this.Y * p2.Y;
}

public int X { get; set; }
public int Y { get; set; }
// In case points set in CCW order, the value will be positive, otherwice - negative.
// Method can be used as a dotprod between vectors this and perpendicular to p2
// The overal result value is equl to doubled triangle size between those 2 points and point (0,0)
public int Cross(Point p2)
{
return this.X * p2.Y - this.Y * p2.X;
}

public override string ToString()
public Point Rotate(int angle)
{
return $"({X} x {Y})";
var cos = (int)Math.Cos(angle);
var sin = (int)Math.Sin(angle);

return new Point(cos * this.X - sin * this.Y, sin * this.X + cos * this.Y);
}

public override int GetHashCode()
public int AngleToVector(Point vec2)
{
return X.GetHashCode() ^ Y.GetHashCode();
return (int)Math.Acos((this.X * vec2.X + this.Y * vec2.Y) / (this.Length * vec2.Length));
}

public override bool Equals(object obj)
{
return (obj is Point other) && this.Equals(other);
return obj is Point vector && this.Equals(vector);
}

public bool Equals(Point obj)
public override int GetHashCode()
{
return this.X == obj.X && this.Y == obj.Y;
int hashCode = -1274299002;
hashCode = hashCode * -1521134295 + X.GetHashCode();
hashCode = hashCode * -1521134295 + Y.GetHashCode();
return hashCode;
}

public static bool operator ==(Point a, Point b) => a.X == b.X && a.Y == b.Y;
public bool Equals(Point vector)
{
return X == vector.X &&
Y == vector.Y;
}

public static bool operator !=(Point a, Point b) => a.X != b.X || a.Y != b.Y;
/// Comparison is done by angle in CCW order from 0 to 2PI.
public int CompareTo(Point other)
{
if (this.Y == 0 && other.Y == 0)
{
return Math.Sign(Math.Sign(this.X) - Math.Sign(other.X));
}

if ((this.Y >= 0) ^ (other.Y >= 0))
{
if (this.Y >= 0)
return 1;
else
return -1;
}
else
{
return Math.Sign((other - this).Cross(-this));
}
}
}
}
}
Loading

0 comments on commit eae7d07

Please sign in to comment.