public static float DistanceBetweenPointAndLineSegment(ref FVector2 point, ref FVector2 lineEndPoint1,
ref FVector2 lineEndPoint2)
{
FVector2 v = FVector2.Subtract(lineEndPoint2, lineEndPoint1);
FVector2 w = FVector2.Subtract(point, lineEndPoint1);
float c1 = FVector2.Dot(w, v);
if (c1 <= 0)
{
return(DistanceBetweenPointAndPoint(ref point, ref lineEndPoint1));
}
float c2 = FVector2.Dot(v, v);
if (c2 <= c1)
{
return(DistanceBetweenPointAndPoint(ref point, ref lineEndPoint2));
}
float b = c1 / c2;
FVector2 pointOnLine = FVector2.Add(lineEndPoint1, FVector2.Multiply(v, b));
return(DistanceBetweenPointAndPoint(ref point, ref pointOnLine));
}
public PolyNode GetRightestConnection(PolyNode incoming)
{
if (NConnected == 0)
{
Debug.Assert(false); // This means the connection graph is inconsistent
}
if (NConnected == 1)
{
//b2Assert(false);
// Because of the possibility of collapsing nearby points,
// we may end up with "spider legs" dangling off of a region.
// The correct behavior here is to turn around.
return(incoming);
}
FVector2 inDir = Position - incoming.Position;
float inLength = inDir.Length();
inDir.Normalize();
Debug.Assert(inLength > Box2D.Settings.b2_epsilon);
PolyNode result = null;
for (int i = 0; i < NConnected; ++i)
{
if (Connected[i] == incoming)
{
continue;
}
FVector2 testDir = Connected[i].Position - Position;
float testLengthSqr = testDir.LengthSquared();
testDir.Normalize();
Debug.Assert(testLengthSqr >= Box2D.Settings.b2_epsilon * Box2D.Settings.b2_epsilon);
float myCos = FVector2.Dot(inDir, testDir);
float mySin = MathUtils.Cross(inDir, testDir);
if (result != null)
{
FVector2 resultDir = result.Position - Position;
resultDir.Normalize();
float resCos = FVector2.Dot(inDir, resultDir);
float resSin = MathUtils.Cross(inDir, resultDir);
if (IsRighter(mySin, myCos, resSin, resCos))
{
result = Connected[i];
}
}
else
{
result = Connected[i];
}
}
Debug.Assert(result != null);
return(result);
}