public NavigationEdge GetBounding()
{
if (cachedBoundingBox == null)
{
DeterministicVector2 min = new DeterministicVector2(0, 0);
DeterministicVector2 max = new DeterministicVector2(0, 0);
if (this.Count > 0)
{
var p = this[0];
min.X = p.X;
max.X = p.X;
min.Y = p.Y;
max.Y = p.Y;
}
for (var i = 1; i < this.Count; i++)
{
var p = this[i];
min.X = DeterministicFloat.Min(p.X, min.X);
min.Y = DeterministicFloat.Min(p.Y, min.Y);
max.X = DeterministicFloat.Max(p.X, max.X);
max.Y = DeterministicFloat.Max(p.Y, max.Y);
}
cachedBoundingBox = new NavigationEdge(min, max);
}
return(cachedBoundingBox);
}
public DeterministicFloat GetBoundingDistance(DeterministicVector2 unitPosition)
{
var aabb = GetBounding();
DeterministicFloat wHalf = (aabb.B.X - aabb.A.X) / 2;
DeterministicFloat hHalf = (aabb.B.Y - aabb.A.Y) / 2;
DeterministicFloat dx = DeterministicFloat.Max(DeterministicFloat.Abs(unitPosition.X - (aabb.A.X + wHalf)) - wHalf, 0);
DeterministicFloat dy = DeterministicFloat.Max(DeterministicFloat.Abs(unitPosition.Y - (aabb.A.Y + hHalf)) - hHalf, 0);
return(DeterministicFloat.Sqrt(dx * dx + dy * dy));
}
public void Reset(IPool pool)
{
_prev = _next = null;
_anEdge = null;
_coords = Vec3.Zero;
_s = new DeterministicFloat(0);
_t = new DeterministicFloat(0);
_pqHandle = new PQHandle();
_n = 0;
_data = null;
}
public NavigationEdgeDistanceResult GetDistance(DeterministicVector2 pt)
{
DeterministicFloat dx = B.X - A.X;
DeterministicFloat dy = B.Y - A.Y;
var lenSquared = (dx * dx + dy * dy);
if (lenSquared == 0)
{
// It's a point not a line segment.
dx = pt.X - A.X;
dy = pt.Y - A.Y;
return(new NavigationEdgeDistanceResult()
{
ClosestPoint = A,
Distance = DeterministicFloat.Sqrt(dx * dx + dy * dy),
IsOnLine = false
});
}
// Calculate the t that minimizes the distance.
DeterministicFloat t = ((pt.X - A.X) * dx + (pt.Y - A.Y) * dy) / lenSquared;
DeterministicVector2 closest;
bool isOnLine = false;
// See if this represents one of the segment's
// end points or a point in the middle.
if (t < 0)
{
closest = new DeterministicVector2(A.X, A.Y);
}
else if (t > 1)
{
closest = new DeterministicVector2(B.X, B.Y);
}
else
{
isOnLine = true;
closest = new DeterministicVector2(A.X + t * dx, A.Y + t * dy);
}
var dcx = pt.X - closest.X;
var dcy = pt.Y - closest.Y;
return(new NavigationEdgeDistanceResult()
{
ClosestPoint = closest,
Distance = DeterministicFloat.Sqrt(dcx * dcx + dcy * dcy),
IsOnLine = isOnLine
});
}
/// <summary>
/// Return signed area of face.
/// </summary>
public static DeterministicFloat FaceArea(Face f)
{
DeterministicFloat area = new DeterministicFloat(0);
var e = f._anEdge;
do
{
area += (e._Org._s - e._Dst._s) * (e._Org._t + e._Dst._t);
e = e._Lnext;
} while (e != f._anEdge);
return(area);
}
public EdgeIntersectionResult CalculateIntersection(NavigationEdge other)
{
var result = new EdgeIntersectionResult();
var dx12 = this.B.X - this.A.X;
var dy12 = this.B.Y - this.A.Y;
var dx34 = other.B.X - other.A.X;
var dy34 = other.B.Y - other.A.Y;
var denominator = dy12 * dx34 - dx12 * dy34;
if (denominator == 0)
{
return(result);
}
// normally t1 is 0 -> 1 (or no hit), but for us it's 0 -> 1
var t1 = ((this.A.X - other.A.X) * dy34 + (other.A.Y - this.A.Y) * dx34) / denominator;
result.LinesIntersect = true;
// normally t1 is 0 -> 1 (or no hit), but for us it's 0 -> 1
var t2 = ((other.A.X - this.A.X) * dy12 + (this.A.Y - other.A.Y) * dx12) / -denominator;
// result.IntersectionPoint = new DeterministicVector2(this.A.X + dx12 * t1, this.A.Y + dy12 * t1);
result.SegmentsIntersect = t1 >= 0 && t1 <= 1 && t2 >= 0 && t2 <= 1;
result.Deltas = new DeterministicVector2(t1, t2);
if (t1 < 0)
{
t1 = new DeterministicFloat(0);
}
else if (t1 > 1)
{
t1 = new DeterministicFloat(1);
}
if (t2 < 0)
{
t1 = new DeterministicFloat(0);
}
else if (t2 > 1)
{
t2 = new DeterministicFloat(1);
}
// result.SegmentIntersection = new NavigationEdge(new DeterministicVector2(this.A.X + dx12 * t1, this.A.Y + dy12 * t1), new DeterministicVector2(other.A.X + dx34 * t2, other.A.Y + dy34 * t2));
return(result);
}
public static DeterministicFloat Interpolate(DeterministicFloat a, DeterministicFloat x, DeterministicFloat b, DeterministicFloat y)
{
if (a < 0)
{
a = new DeterministicFloat(0);
}
if (b < 0)
{
b = new DeterministicFloat(0);
}
return((a <= b) ? ((b == 0) ? ((x + y) / 2)
: (x + (y - x) * (a / (a + b))))
: (y + (x - y) * (b / (a + b))));
}
public AStarNode(DeterministicVector2 position, DeterministicVector2 destination, AStarNode parent = null)
{
this.Position = position;
this.destination = destination;
// Using the setter of Parent makes an endless loop!
this.parent = parent;
if (this.parent != null)
{
this.G = parent.G + (this.Position - parent.Position).ManhattanHeuristic();
}
ConstraintedEdgeNormal = new DeterministicVector2(0, 0);
}
public static int LongAxis(ref Vec3 v)
{
int i = 0;
if (DeterministicFloat.Abs(v.Y) > DeterministicFloat.Abs(v.X))
{
i = 1;
}
if (DeterministicFloat.Abs(v.Z) > DeterministicFloat.Abs(i == 0 ? v.X : v.Y))
{
i = 2;
}
return(i);
}
public DeterministicVector2 Steer(Unit unit, List <Unit> neighbors, List <NavigationEdge> path, Map map)
{
if (path == null || path.Count == 0)
{
return(new DeterministicVector2());
}
bool isLastMovementPossible = unit.LastSteering.GetLengthSquared() != 0;
DeterministicVector2 naiveNewPosition = unit.Position;
DeterministicVector2 lastSteering = unit.LastSteering;
if (isLastMovementPossible)
{
lastSteering = lastSteering.Normalize();
naiveNewPosition += lastSteering;
}
DeterministicFloat shortestDistance = new DeterministicFloat(long.MaxValue, false);
NavigationEdge nearestEdge = new NavigationEdge();
DeterministicVector2 closestPoint = new DeterministicVector2();
for (var i = 0; i < path.Count; i++)
{
var pathEdge = path[i];
var distanceResult = pathEdge.GetDistance(naiveNewPosition);
if (distanceResult.Distance < shortestDistance)
{
shortestDistance = distanceResult.Distance;
nearestEdge = pathEdge;
closestPoint = distanceResult.ClosestPoint;
if (i > 0)
{
unit.RecalcPathOnNextUpdate = true;
}
// naive positioning is on path, we don't need to steer
if (isLastMovementPossible && shortestDistance <= PathSize)
{
return(lastSteering);
}
}
}
var dir = nearestEdge.B - nearestEdge.A;
return(closestPoint + dir.Normalize() - unit.Position);
}
public DeterministicVector2 Steer(Unit unit, List <Unit> neighbors, List <NavigationEdge> path, Map map)
{
if (neighbors.Count == 0)
{
return(new DeterministicVector2());
}
var smallestDistanceFound = new DeterministicFloat(long.MaxValue, false);
var myNewPosition = unit.Position + unit.LastSteering;
DeterministicVector2 toMove = new DeterministicVector2();
foreach (var neighbor in neighbors)
{
// we ignore Units that go the same direction like we do
if (unit.LastSteering.DotProduct(neighbor.LastSteering) > new DeterministicFloat(0))
{
continue;
}
var neighborNewPosition = neighbor.Position + neighbor.LastSteering;
var distance = (myNewPosition - neighborNewPosition).GetLength();
var fullFunnelSize = unit.FunnelSize + neighbor.FunnelSize;
if (distance >= fullFunnelSize)
{
continue;
}
if (smallestDistanceFound < distance)
{
continue;
}
smallestDistanceFound = distance;
var meToNeighbour = neighborNewPosition - myNewPosition;
var steeringPerp = unit.LastSteering.PerpendicularClockwise();
var dotMeToNeighbourAndAvade = steeringPerp.DotProduct(meToNeighbour);
if (dotMeToNeighbourAndAvade < 0)
{
steeringPerp *= -1;
}
var toMoveDistance = fullFunnelSize * 2 - distance;
toMove = steeringPerp.Normalize() * toMoveDistance;
}
return(toMove);
}
private DeterministicFloat SignedArea(IList <ContourVertex> vertices)
{
DeterministicFloat area = new DeterministicFloat(0);
for (int i = 0; i < vertices.Count; i++)
{
var v0 = vertices[i];
var v1 = vertices[(i + 1) % vertices.Count];
area += v0.Position.X * v1.Position.Y;
area -= v0.Position.Y * v1.Position.X;
}
return(area);
}
public void Initialize(NavigationPolygon floor)
{
Bounding = floor.GetBounding();
Cells.Clear();
for (DeterministicFloat y = Bounding.A.Y; y < Bounding.B.Y; y++)
{
for (DeterministicFloat x = Bounding.A.X; x < Bounding.B.X; x++)
{
Cells.Add(new GridCell(x, y)
{
Type = GridCellType.Free
});
}
}
}
public Tess(IPool pool)
{
_normal = Vec3.Zero;
_bminX = _bminY = _bmaxX = _bmaxY = new DeterministicFloat(0);
_windingRule = WindingRule.EvenOdd;
_pool = pool;
if (_pool == null)
{
_pool = new NullPool();
}
_mesh = null;
_vertices = null;
_vertexCount = 0;
_elements = null;
_elementCount = 0;
}
public void PlaceStaticObjects(List <NavigationEdge> polygons)
{
var maxX = Bounding.B.X - Bounding.A.X;
foreach (var polygon in polygons)
{
var polyStartX = DeterministicFloat.Max(polygon.A.X, Bounding.A.X) - Bounding.A.X;
var polyStartY = DeterministicFloat.Max(polygon.A.Y, Bounding.A.Y) - Bounding.A.Y;
var polyLenX = DeterministicFloat.Min(polygon.B.X, Bounding.B.X) - Bounding.A.X;
var polyLenY = DeterministicFloat.Min(polygon.B.Y, Bounding.B.Y) - Bounding.A.Y;
for (DeterministicFloat x = polyStartX; x < polyLenX; x++)
{
for (DeterministicFloat y = polyStartY; y < polyLenY; y++)
{
Cells[(x + y * maxX).ToInt()].Type = GridCellType.Blocked;
}
}
}
}
private bool SwitchCellType(List <NavigationEdge> polygons, GridCellType expectedType, GridCellType resultType)
{
var maxX = Bounding.B.X - Bounding.A.X;
foreach (var polygon in polygons)
{
var polyStartX = DeterministicFloat.Max(polygon.A.X, Bounding.A.X) - Bounding.A.X;
var polyStartY = DeterministicFloat.Max(polygon.A.Y, Bounding.A.Y) - Bounding.A.Y;
var polyLenX = DeterministicFloat.Min(polygon.B.X, Bounding.B.X) - Bounding.A.X;
var polyLenY = DeterministicFloat.Min(polygon.B.Y, Bounding.B.Y) - Bounding.A.Y;
for (DeterministicFloat x = polyStartX; x < polyLenX; x++)
{
for (DeterministicFloat y = polyStartY; y < polyLenY; y++)
{
if (Cells[(x + y * maxX).ToInt()].Type != expectedType)
{
return(false);
}
}
}
}
foreach (var polygon in polygons)
{
var polyStartX = DeterministicFloat.Max(polygon.A.X, Bounding.A.X) - Bounding.A.X;
var polyStartY = DeterministicFloat.Max(polygon.A.Y, Bounding.A.Y) - Bounding.A.Y;
var polyLenX = DeterministicFloat.Min(polygon.B.X, Bounding.B.X) - Bounding.A.X;
var polyLenY = DeterministicFloat.Min(polygon.B.Y, Bounding.B.Y) - Bounding.A.Y;
for (DeterministicFloat x = polyStartX; x < polyLenX; x++)
{
for (DeterministicFloat y = polyStartY; y < polyLenY; y++)
{
Cells[(x + y * maxX).ToInt()].Type = resultType;
}
}
}
return(true);
}
public DeterministicFloat this[int index]
{
get
{
if (index == 0)
{
return(X);
}
if (index == 1)
{
return(Y);
}
if (index == 2)
{
return(Z);
}
throw new IndexOutOfRangeException();
}
set
{
if (index == 0)
{
X = value;
}
else if (index == 1)
{
Y = value;
}
else if (index == 2)
{
Z = value;
}
else
{
throw new IndexOutOfRangeException();
}
}
}
private void CheckOrientation()
{
// When we compute the normal automatically, we choose the orientation
// so that the sum of the signed areas of all contours is non-negative.
DeterministicFloat area = new DeterministicFloat(0);
for (var f = _mesh._fHead._next; f != _mesh._fHead; f = f._next)
{
if (f._anEdge._winding <= 0)
{
continue;
}
area += MeshUtils.FaceArea(f);
}
if (area < 0)
{
// Reverse the orientation by flipping all the t-coordinates
for (var v = _mesh._vHead._next; v != _mesh._vHead; v = v._next)
{
v._t = -v._t;
}
Vec3.Neg(ref _tUnit);
}
}
public DeterministicVector3(DeterministicFloat x, DeterministicFloat y, DeterministicFloat z)
{
X = x;
Y = y;
Z = z;
}
public static void Dot(ref Vec3 u, ref Vec3 v, out DeterministicFloat dot)
{
dot = u.X * v.X + u.Y * v.Y + u.Z * v.Z;
}
public DeterministicFloat GetLength()
{
return(DeterministicFloat.Sqrt(GetLengthSquared()));
}
public static DeterministicFloat VertL1dist(MeshUtils.Vertex u, MeshUtils.Vertex v)
{
return(DeterministicFloat.Abs(u._s - v._s) + DeterministicFloat.Abs(u._t - v._t));
}
public DeterministicVector2(int X, int Y)
{
this.X = new DeterministicFloat(X, true);
this.Y = new DeterministicFloat(Y, true);
}
private void OutputPolymesh(ElementType elementType, int polySize)
{
MeshUtils.Vertex v;
MeshUtils.Face f;
MeshUtils.Edge edge;
int maxFaceCount = 0;
int maxVertexCount = 0;
int faceVerts, i;
if (polySize < 3)
{
polySize = 3;
}
// Assume that the input data is triangles now.
// Try to merge as many polygons as possible
if (polySize > 3)
{
_mesh.MergeConvexFaces(_pool, polySize);
}
// Mark unused
for (v = _mesh._vHead._next; v != _mesh._vHead; v = v._next)
{
v._n = Undef;
}
// Create unique IDs for all vertices and faces.
for (f = _mesh._fHead._next; f != _mesh._fHead; f = f._next)
{
f._n = Undef;
if (!f._inside)
{
continue;
}
if (NoEmptyPolygons)
{
var area = MeshUtils.FaceArea(f);
if (DeterministicFloat.Abs(area) < DeterministicFloat.Epsilon)
{
continue;
}
}
edge = f._anEdge;
faceVerts = 0;
do
{
v = edge._Org;
if (v._n == Undef)
{
v._n = maxVertexCount;
maxVertexCount++;
}
faceVerts++;
edge = edge._Lnext;
}while (edge != f._anEdge);
Debug.Assert(faceVerts <= polySize);
f._n = maxFaceCount;
++maxFaceCount;
}
_elementCount = maxFaceCount;
if (elementType == ElementType.ConnectedPolygons)
{
maxFaceCount *= 2;
}
_elements = new int[maxFaceCount * polySize];
_vertexCount = maxVertexCount;
_vertices = new ContourVertex[_vertexCount];
// Output vertices.
for (v = _mesh._vHead._next; v != _mesh._vHead; v = v._next)
{
if (v._n != Undef)
{
// Store coordinate
_vertices[v._n].Position = v._coords;
_vertices[v._n].Data = v._data;
}
}
// Output indices.
int elementIndex = 0;
for (f = _mesh._fHead._next; f != _mesh._fHead; f = f._next)
{
if (!f._inside)
{
continue;
}
if (NoEmptyPolygons)
{
var area = MeshUtils.FaceArea(f);
if (DeterministicFloat.Abs(area) < DeterministicFloat.Epsilon)
{
continue;
}
}
// Store polygon
edge = f._anEdge;
faceVerts = 0;
do
{
v = edge._Org;
_elements[elementIndex++] = v._n;
faceVerts++;
edge = edge._Lnext;
} while (edge != f._anEdge);
// Fill unused.
for (i = faceVerts; i < polySize; ++i)
{
_elements[elementIndex++] = Undef;
}
// Store polygon connectivity
if (elementType == ElementType.ConnectedPolygons)
{
edge = f._anEdge;
do
{
_elements[elementIndex++] = GetNeighbourFace(edge);
edge = edge._Lnext;
} while (edge != f._anEdge);
// Fill unused.
for (i = faceVerts; i < polySize; ++i)
{
_elements[elementIndex++] = Undef;
}
}
}
}
private void ProjectPolygon()
{
var norm = _normal;
bool computedNormal = false;
if (norm.X == new DeterministicFloat(0) && norm.Y == new DeterministicFloat(0) && norm.Z == new DeterministicFloat(0))
{
ComputeNormal(ref norm);
_normal = norm;
computedNormal = true;
}
int i = Vec3.LongAxis(ref norm);
_sUnit[i] = new DeterministicFloat(0);
_sUnit[(i + 1) % 3] = SUnitX;
_sUnit[(i + 2) % 3] = SUnitY;
_tUnit[i] = new DeterministicFloat(0);
_tUnit[(i + 1) % 3] = norm[i] > new DeterministicFloat(0) ? -SUnitY : SUnitY;
_tUnit[(i + 2) % 3] = norm[i] > new DeterministicFloat(0) ? SUnitX : -SUnitX;
// Project the vertices onto the sweep plane
for (var v = _mesh._vHead._next; v != _mesh._vHead; v = v._next)
{
Vec3.Dot(ref v._coords, ref _sUnit, out v._s);
Vec3.Dot(ref v._coords, ref _tUnit, out v._t);
}
if (computedNormal)
{
CheckOrientation();
}
// Compute ST bounds.
bool first = true;
for (var v = _mesh._vHead._next; v != _mesh._vHead; v = v._next)
{
if (first)
{
_bminX = _bmaxX = v._s;
_bminY = _bmaxY = v._t;
first = false;
}
else
{
if (v._s < _bminX)
{
_bminX = v._s;
}
if (v._s > _bmaxX)
{
_bmaxX = v._s;
}
if (v._t < _bminY)
{
_bminY = v._t;
}
if (v._t > _bmaxY)
{
_bmaxY = v._t;
}
}
}
}
private void ComputeNormal(ref Vec3 norm)
{
var v = _mesh._vHead._next;
var minVal = new DeterministicFloat[3] {
v._coords.X, v._coords.Y, v._coords.Z
};
var minVert = new MeshUtils.Vertex[3] {
v, v, v
};
var maxVal = new DeterministicFloat[3] {
v._coords.X, v._coords.Y, v._coords.Z
};
var maxVert = new MeshUtils.Vertex[3] {
v, v, v
};
for (; v != _mesh._vHead; v = v._next)
{
if (v._coords.X < minVal[0])
{
minVal[0] = v._coords.X; minVert[0] = v;
}
if (v._coords.Y < minVal[1])
{
minVal[1] = v._coords.Y; minVert[1] = v;
}
if (v._coords.Z < minVal[2])
{
minVal[2] = v._coords.Z; minVert[2] = v;
}
if (v._coords.X > maxVal[0])
{
maxVal[0] = v._coords.X; maxVert[0] = v;
}
if (v._coords.Y > maxVal[1])
{
maxVal[1] = v._coords.Y; maxVert[1] = v;
}
if (v._coords.Z > maxVal[2])
{
maxVal[2] = v._coords.Z; maxVert[2] = v;
}
}
// Find two vertices separated by at least 1/sqrt(3) of the maximum
// distance between any two vertices
int i = 0;
if (maxVal[1] - minVal[1] > maxVal[0] - minVal[0])
{
i = 1;
}
if (maxVal[2] - minVal[2] > maxVal[i] - minVal[i])
{
i = 2;
}
if (minVal[i] >= maxVal[i])
{
// All vertices are the same -- normal doesn't matter
norm = new Vec3(new DeterministicFloat(0), new DeterministicFloat(0), new DeterministicFloat(1));
return;
}
// Look for a third vertex which forms the triangle with maximum area
// (Length of normal == twice the triangle area)
DeterministicFloat maxLen2 = new DeterministicFloat(0), tLen2;
var v1 = minVert[i];
var v2 = maxVert[i];
Vec3 d1, d2, tNorm;
Vec3.Sub(ref v1._coords, ref v2._coords, out d1);
for (v = _mesh._vHead._next; v != _mesh._vHead; v = v._next)
{
Vec3.Sub(ref v._coords, ref v2._coords, out d2);
tNorm.X = d1.Y * d2.Z - d1.Z * d2.Y;
tNorm.Y = d1.Z * d2.X - d1.X * d2.Z;
tNorm.Z = d1.X * d2.Y - d1.Y * d2.X;
tLen2 = tNorm.X * tNorm.X + tNorm.Y * tNorm.Y + tNorm.Z * tNorm.Z;
if (tLen2 > maxLen2)
{
maxLen2 = tLen2;
norm = tNorm;
}
}
if (maxLen2 <= new DeterministicFloat(0))
{
// All points lie on a single line -- any decent normal will do
norm = Vec3.Zero;
i = Vec3.LongAxis(ref d1);
norm[i] = new DeterministicFloat(1);
}
}
public DeterministicVector2(DeterministicFloat X, DeterministicFloat Y)
{
this.X = X;
this.Y = Y;
}
public DeterministicVector2(DeterministicVector2 pt)
{
this.X = pt.X;
this.Y = pt.Y;
}
public DeterministicFloat ManhattanHeuristic()
{
return(DeterministicFloat.Abs(X) + DeterministicFloat.Abs(Y));
}
public GridCell(DeterministicFloat x, DeterministicFloat y)
{
Type = GridCellType.Blocked;
X = x;
Y = y;
}
