public static Vector3 LineLineIntersection(RoadSegment.Bound boundA, RoadSegment.Bound boundB)
{
var A = boundA.BorderPointA;
var B = boundA.BorderPointB;
var C = boundB.BorderPointA;
var D = boundB.BorderPointB;
return(LineLineIntersection(A, B, C, D));
}
private static void SetIntersectionPoint(RoadSegment.Bound boundA, RoadSegment.Bound boundB)
{
var point = LineLineIntersection(boundA, boundB);
if (CheckPoint(point, boundA) && CheckPoint(point, boundB))
{
boundA.BoundIntersectionPoints.Add(point);
boundB.BoundIntersectionPoints.Add(point);
}
}
public static bool CheckPoint(Vector3 pos, RoadSegment.Bound bound)
{
var dirSegment = bound.BorderPointB - bound.BorderPointA;
var dirToPos = pos - bound.BorderPointA;
var segmentMagn = dirSegment.magnitude;
var segmentPosMagn = dirToPos.magnitude;
var isPosOnSegment = segmentPosMagn / segmentMagn < 1 && Vector3.Dot(dirSegment, dirToPos) > 0;
return(isPosOnSegment);
}
private bool CheckBoundDirection(Node outerNode, RoadSegment.Bound bound, bool isLeftBound)
{
var distA = Vector3.SqrMagnitude(outerNode.Position - bound.BorderPointA);
var distB = Vector3.SqrMagnitude(outerNode.Position - bound.BorderPointB);
var outerNodeSideBound = distA < distB ? bound.BorderPointA : bound.BorderPointB;
var dir = outerNodeSideBound - outerNode.Position;
var dotProd = Vector3.Dot(Vector3.up, Vector3.Cross(dir, Node.Position - outerNode.Position));
var isAligned = dotProd * (isLeftBound ? 1 : -1) > 0;
return(isAligned);
}