static Vector ComputeIntersection(Vector S, Vector E, AffineManifold clipEdge)
{
var SE = AffineManifold.FromPoints(S, E);
var I = AffineManifold.Intersect2D(SE, clipEdge);
//var D = S - E;
//D.Normalize();
//var N1 = new Vector(D.y, -D.x);
//double inner = N1 * clipEdge.Normal;
Debug.Assert(!double.IsNaN(I.x));
Debug.Assert(!double.IsInfinity(I.x));
Debug.Assert(!double.IsNaN(I.y));
Debug.Assert(!double.IsInfinity(I.y));
return(I);
}
/// <summary>
/// Intersection of line <paramref name="S1"/>--<paramref name="S2"/> and <paramref name="E1"/>--<paramref name="E2"/>
/// </summary>
/// <param name="S1"></param>
/// <param name="S2"></param>
/// <param name="E1"></param>
/// <param name="E2"></param>
/// <param name="alpha1">
/// coordinate of <paramref name="I"/> on the line <paramref name="S1"/>--<paramref name="S2"/>
/// </param>
/// <param name="alpha2">
/// coordinate of <paramref name="I"/> on the line <paramref name="E1"/>--<paramref name="E2"/>
/// </param>
/// <param name="I"></param>
/// <returns></returns>
public static bool ComputeIntersection(Vector S1, Vector S2, Vector E1, Vector E2, out double alpha1, out double alpha2, out Vector I)
{
if (S1.Dim != 2)
{
throw new ArgumentException("spatial dimension mismatch.");
}
if (S2.Dim != 2)
{
throw new ArgumentException("spatial dimension mismatch.");
}
if (E1.Dim != 2)
{
throw new ArgumentException("spatial dimension mismatch.");
}
if (E2.Dim != 2)
{
throw new ArgumentException("spatial dimension mismatch.");
}
Vector S12 = S2 - S1;
Vector E12 = E2 - E1;
var P_S12 = AffineManifold.FromPoints(S1, S2);
var P_E12 = AffineManifold.FromPoints(E1, E2);
double parallel = S12[0] * E12[1] - S12[1] * E12[0];
double relParallel = parallel * parallel / (S12.AbsSquare() * E12.AbsSquare());
if (Math.Abs(relParallel) <= 1e-20)
{
alpha1 = P_S12.PointDistance(E1);
alpha1 /= E12.Abs();
alpha2 = double.PositiveInfinity;
I = new Vector(double.PositiveInfinity, double.PositiveInfinity);
return(false);
}
//S12.Normalize();
//E12.Normalize();
I = AffineManifold.Intersect2D(P_S12, P_E12);
Vector IS1 = I - S2;
Vector IE1 = I - E2;
Vector IS2 = I - S1;
Vector IE2 = I - E1;
Vector IS;
bool flip_1;
if (IS1.AbsSquare() > IS2.AbsSquare())
{
IS = IS1;
flip_1 = true;
}
else
{
IS = IS2;
flip_1 = false;
}
Vector IE;
bool flip_2;
if (IE1.AbsSquare() > IE2.AbsSquare())
{
IE = IE1;
flip_2 = true;
}
else
{
IE = IE2;
flip_2 = false;
}
Debug.Assert((S12.AngleTo(IS).Abs() <= 1.0e-5) || ((S12.AngleTo(IS).Abs() - Math.PI).Abs() <= 1.0e-5));
Debug.Assert((E12.AngleTo(IE).Abs() <= 1.0e-5) || ((E12.AngleTo(IE).Abs() - Math.PI).Abs() <= 1.0e-5));
alpha1 = (S12 * IS) / S12.AbsSquare();
alpha2 = (E12 * IE) / E12.AbsSquare();
if (flip_1)
{
alpha1 = 1 + alpha1;
}
if (flip_2)
{
alpha2 = 1 + alpha2;
}
return(true);
}