/// <summary>
/// Вычисление линии пересечения двух плоскостей.
/// </summary>
/// <param name="p1">Первая точка, определяющая 1-ю плоскость.</param>
/// <param name="p2">Вторая точка, определяющая 1-ю плоскость.</param>
/// <param name="p3">Третья точка, определяющая 1-ю плоскость.</param>
/// <param name="p4">Первая точка, определяющая 2-ю плоскость.</param>
/// <param name="p5">Вторая точка, определяющая 2-ю плоскость.</param>
/// <param name="p6">Третья точка, определяющая 2-ю плоскость.</param>
/// <param name="res">Результирующая прямая.</param>
/// <param name="threshold">Допуск определения параллельности плоскостей.</param>
/// <returns>TRUE, если линия пересечения найдена.</returns>
public static bool FindIntersection(IXYZ p1, IXYZ p2, IXYZ p3,
IXYZ p4, IXYZ p5, IXYZ p6,
out Line3d res, double threshold)
{
Vector3d n1 = (p2.ToVector3d() - p1.ToVector3d()) ^ (p3.ToVector3d() - p1.ToVector3d());
Vector3d n2 = (p5.ToVector3d() - p4.ToVector3d()) ^ (p6.ToVector3d() - p4.ToVector3d());
if (Vector3d.AreParallel(n1, n2, threshold))
{
res = null;
return(false);
}
else
{
//Vector3d d = n1 ^ n2 ^ n1;
//Vector3d dp1 = p1.ToVector3d();
//Vector3d dp2 = p1.ToVector3d() + d;
//FindIntersection(p4, p5, p6, dp1.ToPoint3d(), dp2.ToPoint3d(), out Point3d ip1, out double t, threshold);
//Point3d ip2 = ip1 + (n1 ^ n2);
double det = (n1 % n1) * (n2 % n2) - Math.Pow(n1 % n2, 2);
double d1 = n1 % p1.ToVector3d();
double d2 = n2 % p4.ToVector3d();
double c1 = (d1 * (n2 % n2) - d2 * (n1 % n2)) / det;
double c2 = (d2 * (n1 % n1) - d1 * (n1 % n2)) / det;
Vector3d ip1 = c1 * n1 + c2 * n2;
Vector3d ip2 = c1 * n1 + c2 * n2 + (n1 ^ n2);
res = new Line3d(ip1.ToPoint3d(), ip2.ToPoint3d());
return(true);
}
}
/// <summary>
/// Вычисление пересечения линии и сферы.
/// </summary>
/// <param name="p1">Начальная точка линии.</param>
/// <param name="p2">Конечная точка линии.</param>
/// <param name="sc">Центр сферы.</param>
/// <param name="r">Радиус сферы.</param>
/// <param name="mu1">Первый результирующий параметр.</param>
/// <param name="mu2">Второй результирующий параметр.</param>
/// <remarks>
/// Отрезок определяется от p1 до p2.
/// Сфера определяется радиусом r с центром в sc.
/// Есть потенциально две точки пересечения, заданные
/// p = p1 + mu1 (p2 - p1)
/// p = p1 + mu2 (p2 - p1)
/// </remarks>
/// <returns>FALSE, если луч не пересекает сферу.</returns>
private bool RaySphere(IXYZ p1, IXYZ p2, IXYZ sc, double r, out double mu1, out double mu2)
{
double a, b, c, bb4ac;
Vector3d dp = p2.ToVector3d() - p1.ToVector3d();
a = dp.Norma;
b = 2 * (dp.Vx * (p1.X - sc.X) + dp.Vy * (p1.Y - sc.Y) + dp.Vz * (p1.Z - sc.Z));
c = sc.ToVector3d().Norma;
c += p1.ToVector3d().Norma;
c -= 2 * (sc.X * p1.X + sc.Y * p1.Y + sc.Z * p1.Z);
c -= r * r;
bb4ac = b * b - 4 * a * c;
if (Math.Abs(a) < 1e-12 || bb4ac < 0)
{
mu1 = double.NaN;
mu2 = double.NaN;
return(false);
}
else if (bb4ac > 0 && bb4ac <= 1e-12)
{
mu1 = -b / (2 * a);
mu2 = double.NaN;
return(false);
}
mu1 = (-b + Math.Sqrt(bb4ac)) / (2 * a);
mu2 = (-b - Math.Sqrt(bb4ac)) / (2 * a);
return(true);
}
/// <summary>
/// Вычисляет минимальное расстояние между точкой и линией.
/// </summary>
/// <param name="p">Точка.</param>
/// <param name="origin">Точка, определяющая линию.</param>
/// <param name="dir">Направляющий вектор линии.</param>
/// <returns>Минимальное расстояние между точкой и линией.</returns>
public static double PointLineDistance(IXYZ p, IXYZ origin, Vector3d dir)
{
double t = dir % (p.ToVector3d() - origin.ToVector3d());
Vector3d pPrime = origin.ToVector3d() + dir * t;
Vector3d vec = p.ToVector3d() - pPrime;
double distanceSquared = vec.Norma;
return(Math.Sqrt(distanceSquared));
}
/// <summary>
/// Проверяет, находится ли точка внутри области, определяемой линейным сегментом.
/// </summary>
/// <param name="p">Точка.</param>
/// <param name="start">Начальная точка сегмента.</param>
/// <param name="end">Конечная точка сегмента.</param>
/// <returns>
/// 0, если точка находится внутри сегмента,
/// 1, если точка находится после конечной точки, и
/// -1, если точка находится перед начальной точкой.</returns>
/// <remarks>
/// Для целей тестирования точка считается внутри сегмента,
/// если она попадает в область от начала до конца сегмента, которая простирается бесконечно перпендикулярно его направлению.
/// Позже, если необходимо, вы можете использовать метод PointLineDistance для определеня расстояния от точки до сегмента.
/// Если это расстояние равно нулю, то точка находится вдоль линии, определяемой начальной и конечной точками.
/// </remarks>
public static int PointInSegment(IXYZ p, IXYZ start, IXYZ end)
{
Vector3d dir = end.ToVector3d() - start.ToVector3d();
Vector3d pPrime = p.ToVector3d() - start.ToVector3d();
double t = dir % pPrime;
if (t < 0)
{
return(-1);
}
double dot = dir % dir;
if (t > dot)
{
return(1);
}
return(0);
}
/// <summary>
/// Проверяет, находится ли точка внутри области, определяемой линейным сегментом.
/// </summary>
/// <param name="p">Точка.</param>
/// <param name="start">Начальная точка сегмента.</param>
/// <param name="end">Конечная точка сегмента.</param>
/// <returns>
/// 0, если точка находится внутри сегмента,
/// 1, если точка находится после конечной точки, и
/// -1, если точка находится перед начальной точкой.</returns>
/// <remarks>
/// Для целей тестирования точка считается внутри сегмента,
/// если она попадает в область от начала до конца сегмента, которая простирается бесконечно перпендикулярно его направлению.
/// Позже, если необходимо, вы можете использовать метод PointLineDistance для определеня расстояния от точки до сегмента.
/// Если это расстояние равно нулю, то точка находится вдоль линии, определяемой начальной и конечной точками.
/// </remarks>
public static int PointInSegmentNoBounds(IXYZ p, IXYZ start, IXYZ end)
{
Vector3d dir = end.ToVector3d() - start.ToVector3d();
Vector3d pPrime = p.ToVector3d() - start.ToVector3d();
double t = dir % pPrime;
if (t < 0 || IsZero(t))
{
return(-1);
}
double dot = dir % dir;
if (t > dot || IsEqual(t, dot))
{
return(1);
}
return(0);
}
public Circle2d(IXYZ p1, IXYZ p2, IXYZ p3)
{
double ma = (p2.Y - p1.Y) / (p2.X - p1.X);
double mb = (p3.Y - p2.Y) / (p3.X - p2.X);
double x = (ma * mb * (p1.Y - p3.Y) + mb * (p2.X + p1.X) - ma * (p3.X + p2.X)) / (2 * (mb - ma));
double y = 1 / ma * (x - 0.5 * (p2.X + p1.X)) + 0.5 * (p2.Y + p1.Y);
Center = new Point3d(x, y);
Radius = (p1.ToVector3d() - Center.ToVector3d()).Norma;
Length = 2 * Radius * Math.PI;
}
/// <summary>
/// Вычисление точки пересечения прямой с плоскостью.
/// </summary>
/// <param name="p1">Первая точка, определяющая плоскость.</param>
/// <param name="p2">Вторая точка, определяющая плоскость.</param>
/// <param name="p3">Третья точка, определяющая плоскость.</param>
/// <param name="p4">Первая точка, определяющая прямую.</param>
/// <param name="p5">Вторая точка, определяющая прямую.</param>
/// <param name="res">Результирующая точка пересечения.</param>
/// <param name="t">Параметр, результирующей точки пересечения</param>
/// <param name="threshold">Допуск.</param>
/// <returns>TRUE, если точка пересечения найдена.</returns>
public static bool FindIntersection(IXYZ p1, IXYZ p2, IXYZ p3,
IXYZ p4, IXYZ p5,
out Point3d res, out double t, double threshold)
{
Vector3d normal = (p2.ToVector3d() - p1.ToVector3d()) ^ (p3.ToVector3d() - p1.ToVector3d());
double denom = normal % (p5.ToVector3d() - p4.ToVector3d());
double nom = normal % (p1.ToVector3d() - p4.ToVector3d());
if (IsZero(denom, threshold))
{
res = null;
t = double.NaN;
return(false);
}
else
{
t = nom / denom;
res = new Point3d(p4.ToVector3d() + (p5.ToVector3d() - p4.ToVector3d()) * t);
return(true);
}
}
/// <summary>
/// Transforms a point between coordinate systems.
/// </summary>
/// <param name="point">Point to transform.</param>
/// <param name="zAxis">Object normal vector.</param>
/// <param name="from">Point coordinate system.</param>
/// <param name="to">Coordinate system of the transformed point.</param>
/// <returns>Transformed point.</returns>
public static Point3d Transform(IXYZ point, Vector3d zAxis, CoordinateSystem from, CoordinateSystem to)
{
// if the normal is (0,0,1) no transformation is needed the transformation matrix is the identity
if (zAxis.Equals(Vector3d.UnitZ))
{
return(new Point3d(point));
}
Matrix trans = ArbitraryAxis(zAxis);
if (from == CoordinateSystem.World && to == CoordinateSystem.Object)
{
trans = trans.Transpose();
return((trans * point.ToVector3d()).ToPoint3d());
}
if (from == CoordinateSystem.Object && to == CoordinateSystem.World)
{
return((trans * point.ToVector3d()).ToPoint3d());
}
return(new Point3d(point));
}
/// <summary>
/// Вычисляет отрезок линии пересечения между 2 линиями (не отрезками).
/// </summary>
/// <returns>Возвращает FALSE, если решение не найдено.</returns>
public static bool FindIntersection(IXYZ line1Point1, IXYZ line1Point2,
IXYZ line2Point1, IXYZ line2Point2,
out Point3d resultSegmentPoint1, out Point3d resultSegmentPoint2,
double threshold = 1e-12)
{
// Algorithm is ported from the C algorithm of
// Paul Bourke at http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/
resultSegmentPoint1 = null;
resultSegmentPoint2 = null;
Vector3d p1 = line1Point1.ToVector3d();
Vector3d p2 = line1Point2.ToVector3d();
Vector3d p3 = line2Point1.ToVector3d();
Vector3d p4 = line2Point2.ToVector3d();
Vector3d p13 = p1 - p3;
Vector3d p43 = p4 - p3;
if (p43.Norma < threshold)
{
return(false);
}
Vector3d p21 = p2 - p1;
if (p21.Norma < threshold)
{
return(false);
}
double d1343 = p13 % p43;
double d4321 = p43 % p21;
double d1321 = p13 % p21;
double d4343 = p43 % p43;
double d2121 = p21 % p21;
double denom = d2121 * d4343 - d4321 * d4321;
if (Math.Abs(denom) < threshold)
{
return(false);
}
double numer = d1343 * d4321 - d1321 * d4343;
double mua = numer / denom;
double mub = (d1343 + d4321 * (mua)) / d4343;
resultSegmentPoint1 = (p1 + mua * p21).ToPoint3d();
resultSegmentPoint2 = (p3 + mub * p43).ToPoint3d();
return(true);
}
/// <summary>
/// Вычисление пересечения линии и сферы.
/// </summary>
/// <param name="p1">Начальная точка линии.</param>
/// <param name="p2">Конечная точка линии.</param>
/// <param name="sc">Центр сферы.</param>
/// <param name="r">Радиус сферы.</param>
/// <param name="res1">Первая точка пересечения.</param>
/// <param name="res2">Вторая точка пересечения.</param>
/// <param name="threshold">Допуск.</param>
/// <remarks>
/// Отрезок определяется от p1 до p2.
/// Сфера определяется радиусом r с центром в sc.
/// Есть потенциально две точки пересечения.
/// </remarks>
/// <returns>FALSE, если линия не пересекает сферу.</returns>
public static bool FindIntersection(IXYZ p1, IXYZ p2,
IXYZ sc, double r,
out Point3d res1, out Point3d res2,
double threshold)
{
double a, b, c, bb4ac, mu1, mu2;
Vector3d dp = p2.ToVector3d() - p1.ToVector3d();
a = dp.Norma;
b = 2 * (dp.Vx * (p1.X - sc.X) + dp.Vy * (p1.Y - sc.Y) + dp.Vz * (p1.Z - sc.Z));
c = sc.ToVector3d().Norma;
c += p1.ToVector3d().Norma;
c -= 2 * (sc.X * p1.X + sc.Y * p1.Y + sc.Z * p1.Z);
c -= r * r;
bb4ac = b * b - 4 * a * c;
if (Math.Abs(a) < threshold || bb4ac < 0)
{
res1 = null;
res2 = null;
return(false);
}
else if (bb4ac > 0 && bb4ac <= threshold)
{
mu1 = -b / (2 * a);
res1 = (p1.ToVector3d() + (p2.ToVector3d() - p1.ToVector3d()) * mu1).ToPoint3d();
res2 = null;
return(false);
}
mu1 = (-b + Math.Sqrt(bb4ac)) / (2 * a);
mu2 = (-b - Math.Sqrt(bb4ac)) / (2 * a);
res1 = (p1.ToVector3d() + (p2.ToVector3d() - p1.ToVector3d()) * mu1).ToPoint3d();
res2 = (p1.ToVector3d() + (p2.ToVector3d() - p1.ToVector3d()) * mu2).ToPoint3d();
return(true);
}
/// <summary>
/// Проверяет, совпадают ли координаты точечных объектов.
/// </summary>
/// <param name="a">Координаты точечного объекта.</param>
/// <param name="b">Координаты точечного объекта.</param>
/// <param name="threshold">Tolerance.</param>
/// <returns>True if its close to one or false in any other case.</returns>
public static bool IsEqual(IXYZ a, IXYZ b, double threshold)
{
return(IsZero((a.ToVector3d() - b.ToVector3d()).ToPoint3d(), threshold));
}