public bool IntersectsWith(DoubleRay2D ray, out Vector <double> intersection)
{
bool intersect = false;
intersection = new DenseVector(2);
double[] coeffs1 = GetLineCoeficients();
double[] coeffs2 = ray.GetLineCoeficients();
double det = coeffs1[0] * coeffs2[1] - coeffs2[0] * coeffs1[1];
if (Math.Abs(det) >= 1e-6)
{
double x = (coeffs2[1] * coeffs1[2] - coeffs1[1] * coeffs2[2]) / det;
double y = (coeffs1[0] * coeffs2[2] - coeffs2[0] * coeffs1[2]) / det;
intersection = new DenseVector(2)
{
[0] = x,
[1] = y
};
intersect = true;
}
return(intersect);
}
// CONSTRUCTORS
public Ray2D(Vector <double> origin, Vector <double> direction)
{
doubleRay = new DoubleRay2D()
{
Origin = origin,
Direction = direction,
};
}
public bool IntersectsWith(Ray2D ray, out Vector <double> intersectionPoint)
{
DoubleRay2D doubleRay2 = ray.GetDoubleRay();
bool thereIsIntersection = doubleRay2.IntersectsWith(doubleRay, out intersectionPoint);
return(thereIsIntersection &&
GetDistanceAtPoint(intersectionPoint) >= 0 &&
ray.GetDistanceAtPoint(intersectionPoint) >= 0);
}
public bool ContainsPoint(Vector <double> p)
{
double[] c1 = GetLineCoeficients();
Vector <double> dr = new DenseVector(2);
dr[0] = p[0] - Origin[0];
dr[1] = p[1] - Origin[1];
DoubleRay2D dr2D = new DoubleRay2D()
{
Origin = Origin,
direction = dr,
};
double[] c2 = dr2D.GetLineCoeficients();
double error = c2.Select((t, index) => Math.Pow(c1[index] - t, 2)).Sum();
error = Math.Sqrt(error);
return(error <= 1e-6);
}