public bool EncroachedUpon(DeterministicVector2 p)
{
if (p == A || p == B)
{
return(false);
}
var radius = (A - B).GetLengthSquared() / 2;
return((Midpoint() - p).GetLengthSquared() < radius);
}
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
});
}
public bool IsPointInTriangle(DeterministicVector2 p)
{
bool b1 = Sign(p, this.U, this.V) < 0;
bool b2 = Sign(p, this.V, this.W) < 0;
if (b1 != b2)
{
return(false);
}
bool b3 = Sign(p, this.W, this.U) < 0;
return(b2 == b3);
}
public override bool Equals(object obj)
{
if (obj == null)
{
return(false);
}
if (obj is DeterministicVector2)
{
DeterministicVector2 a = (DeterministicVector2)obj;
return((X == a.X) && (Y == a.Y));
}
else
{
return(false);
}
}
public NavigationEdge(DeterministicVector2 a, DeterministicVector2 b)
{
A = a;
B = b;
}
public bool HasPoint(DeterministicVector2 p)
{
return(A == p || B == p);
}
public NavigationTriangle(DeterministicVector2 u, DeterministicVector2 v, DeterministicVector2 w)
{
U = u;
V = v;
W = w;
}
private static DeterministicFloat Sign(DeterministicVector2 p1, DeterministicVector2 p2, DeterministicVector2 p3)
{
return((p1.X - p3.X) * (p2.Y - p3.Y) - (p2.X - p3.X) * (p1.Y - p3.Y));
}
public DeterministicFloat DotProduct(DeterministicVector2 b)
{
return(this.X * b.X + this.Y * b.Y);
}
public DeterministicFloat CrossProduct(DeterministicVector2 b)
{
return(this.X * b.Y - this.Y * b.X);
}
public DeterministicVector2(DeterministicVector2 pt)
{
this.X = pt.X;
this.Y = pt.Y;
}