/// <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);
}
}
/*
* Determine whether or not the line segment p1,p2
* Intersects the 3 vertex facet bounded by pa,pb,pc
* Return true/false and the intersection point p
*
* The equation of the line is p = p1 + mu (p2 - p1)
* The equation of the plane is a x + b y + c z + d = 0
* n.X x + n.Y y + n.Z z + d = 0
*/
private int LineFacet(IXYZ p1, IXYZ p2, IXYZ pa, IXYZ pb, IXYZ pc, IXYZ p)
{
double d;
double a1;
double a2;
double a3;
double total;
double denom;
double mu;
IXYZ n = new Point3d();
IXYZ pa1 = new Point3d();
IXYZ pa2 = new Point3d();
IXYZ pa3 = new Point3d();
/* Расчет параметров плоскости */
n.X = (pb.Y - pa.Y) * (pc.Z - pa.Z) - (pb.Z - pa.Z) * (pc.Y - pa.Y);
n.Y = (pb.Z - pa.Z) * (pc.X - pa.X) - (pb.X - pa.X) * (pc.Z - pa.Z);
n.Z = (pb.X - pa.X) * (pc.Y - pa.Y) - (pb.Y - pa.Y) * (pc.X - pa.X);
Normalise(n);
d = -n.X * pa.X - n.Y * pa.Y - n.Z * pa.Z;
/* Вычислить положение на линии, пересекающей плоскость */
denom = n.X * (p2.X - p1.X) + n.Y * (p2.Y - p1.Y) + n.Z * (p2.Z - p1.Z);
if (ABS(denom) < EPS) // Прямая и плоскость не пересекаются
{
return(0);
}
mu = -(d + n.X * p1.X + n.Y * p1.Y + n.Z * p1.Z) / denom;
p.X = p1.X + mu * (p2.X - p1.X);
p.Y = p1.Y + mu * (p2.Y - p1.Y);
p.Z = p1.Z + mu * (p2.Z - p1.Z);
if (mu < 0 || mu > 1) // Пересечение находится не на отрезке, задающем прямую
{
return(0);
}
/* Определение, ограничена ли точка пересечения параметрами pa, pb, pc */
pa1.X = pa.X - p.X;
pa1.Y = pa.Y - p.Y;
pa1.Z = pa.Z - p.Z;
Normalise(pa1);
pa2.X = pb.X - p.X;
pa2.Y = pb.Y - p.Y;
pa2.Z = pb.Z - p.Z;
Normalise(pa2);
pa3.X = pc.X - p.X;
pa3.Y = pc.Y - p.Y;
pa3.Z = pc.Z - p.Z;
Normalise(pa3);
a1 = pa1.X * pa2.X + pa1.Y * pa2.Y + pa1.Z * pa2.Z;
a2 = pa2.X * pa3.X + pa2.Y * pa3.Y + pa2.Z * pa3.Z;
a3 = pa3.X * pa1.X + pa3.Y * pa1.Y + pa3.Z * pa1.Z;
total = (Math.Acos(a1) + Math.Acos(a2) + Math.Acos(a3)) * RTOD; //Комментарии. Функция RTOD( ) преобразует значение числового выражения, заданное в радианах, в эквивалентное значение в градусах.
if (Math.Abs(total - 360) > EPS)
{
return(0);
}
return(1);
}
/// <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);
}
private IXYZ Nearest(KdNode node, RectHV rect, double x, double y, IXYZ candidate)
{
if (node == null)
{
return(candidate);
}
double dqn = 0;
double drq = 0;
var query = new Point2d(x, y);
var nearest = candidate;
if (nearest != null)
{
dqn = query.DistanceSquaredTo(nearest);
drq = rect.DistanceSquaredTo(query);
}
if (nearest == null || dqn < drq)
{
var point = new Point2d(node.X, node.Y);
if (nearest == null || dqn > query.DistanceSquaredTo(point))
{
nearest = point;
}
}
var left = LeftRect(rect, node);
var right = RightRect(rect, node);
if (node.Vertical)
{
if (x < node.X)
{
nearest = Nearest(node.Left, left, x, y, nearest);
nearest = Nearest(node.Right, right, x, y, nearest);
}
else
{
nearest = Nearest(node.Right, right, x, y, nearest);
nearest = Nearest(node.Left, left, x, y, nearest);
}
}
else
{
if (y < node.Y)
{
nearest = Nearest(node.Left, left, x, y, nearest);
nearest = Nearest(node.Right, right, x, y, nearest);
}
else
{
nearest = Nearest(node.Right, right, x, y, nearest);
nearest = Nearest(node.Left, left, x, y, nearest);
}
}
return(nearest);
}
/// <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="xyz"></param>
/// <returns></returns>
public double Distance(IXYZ xyz)
{
// if (Dimension == 2) return base.Distance(xyz);
double dx = xyz.X - X;
double dy = xyz.Y - Y;
double dz = xyz.Z - Z;
return(Math.Sqrt(dx * dx + dy * dy + dz * dz));
}
public static double [] XyzToGeodeticCoord(IXYZ xyz, Geo.Referencing.IEllipsoid ellipsiod = null)
{
double lon, lat, height;
if (ellipsiod == null)
{
ellipsiod = Geo.Referencing.Ellipsoid.CGCS2000;
}
XyzToGeodeticCoord(xyz.X, xyz.Y, xyz.Z, out lon, out lat, out height, ellipsiod.SemiMajorAxis, ellipsiod.InverseFlattening);
return(new double[] { lon, lat, height });
}
/// <summary>
/// 由空间直角坐标转换为椭球坐标。默认角度单位为度。
/// </summary>
/// <param name="xyz"></param>
/// <param name="ellipsoid"></param>
/// <param name="angeUnit"></param>
/// <returns></returns>
public static GeoCoord XyzToGeoCoord(IXYZ xyz, Geo.Referencing.Ellipsoid ellipsoid, AngleUnit angeUnit = AngleUnit.Degree)
{
double x = xyz.X;
double y = xyz.Y;
double z = xyz.Z;
double a = ellipsoid.SemiMajorAxis;
double e = ellipsoid.FirstEccentricity;
return(XyzToGeoCoord(x, y, z, a, e, angeUnit));
}
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="pos">空间直角坐标</param>
/// <param name="date">儒略日</param>
/// <param name="ellipsoid">参考椭球</param>
/// <param name="unit">角度单位</param>
/// <returns></returns>
public static GeoCoord XyzToGeoCoord(IXYZ pos, Julian date, Geo.Referencing.Ellipsoid ellipsoid = null, AngleUnit unit = AngleUnit.Degree)
{
if (ellipsoid == null)
{
ellipsoid = Geo.Referencing.Ellipsoid.WGS84;
}
double f = ellipsoid.Flattening;
double a = ellipsoid.SemiMajorAxis;
double TwoPi = 2 * CoordConsts.PI;
double x = pos.X;
double y = pos.Y;
double z = pos.Z;
double theta = (GeoMath.AcTan(pos.Y, pos.X) - date.GetGreenwichMeanSiderealTime()) % TwoPi;
theta = theta % TwoPi;
if (theta < 0.0)
{
// "wrap" negative modulo
theta += TwoPi;
}
double r = Math.Sqrt(x * x + y * y);
double e2 = f * (2.0 - f);
double lat = GeoMath.AcTan(z, r);
const double DELTA = 1.0e-07;
double phi;
double c;
do
{
phi = lat;
c = 1.0 / Math.Sqrt(1.0 - e2 * GeoMath.Sqr(Sin(phi)));
lat = GeoMath.AcTan(pos.Z + a * c * e2 * Sin(phi), r);
}while (Math.Abs(lat - phi) > DELTA);
double Altitude = (r / Cos(lat)) - a * c;
if (unit == AngleUnit.Degree)
{
lat *= AngularConvert.RadToDegMultiplier;
theta *= AngularConvert.RadToDegMultiplier;
}
double Lat = lat;
double Lon = theta;
return(new GeoCoord(Lon, Lat, Altitude, unit));
}
/// <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="old"></param>
/// <param name="info"></param>
/// <returns></returns>
///坐标转换时都是转XYZ，然后再将XYZ转换成BLH和xy
/// WGS-84 --> BJZ54
///-15.415, 157.025, 94.74, 0.312, 0.08, 0.102, -1.465e-6 /*全国*/
///-14.756, 145.798, 100.886, 0.618, 0.255, 0.302, -0.439e-6 /*东南局部*/
/// BJZ54 --> DXZ88
///16.5, -152.9, -91.8, -0.226, -0.003, 0.0, 1.22e-6
public static IXYZ BursaTransform(IXYZ old, IBursaTransParams p)
{
//double[] result = info.Transform(new double[] { old.X, old.Y, old.Z });
// return XYZ.Parse(result);
double X, Y, Z;
double scale = (1 + p.Scale_m);
X = p.Dx + old.X * scale + (old.Y * p.Ez - old.Z * p.Ey) * SEC_TO_RAD;
Y = p.Dy + old.Y * scale + (old.Z * p.Ex - old.X * p.Ez) * SEC_TO_RAD;
Z = p.Dz + old.Z * scale + (old.X * p.Ey - old.Y * p.Ex) * SEC_TO_RAD;
return(new Xyz(X, Y, Z));
}
public static bool Contains(IXYZ[] points, IXYZ p)
{
bool result = false;
for (int i = 0; i < points.Length - 1; i++)
{
if ((((points[i + 1].Y <= p.Y) && (p.Y < points[i].Y)) || ((points[i].Y <= p.Y) &&
(p.Y < points[i + 1].Y))) &&
(p.X < (points[i].X - points[i + 1].X) * (p.Y - points[i + 1].Y) / (points[i].Y - points[i + 1].Y) + points[i + 1].X))
{
result = !result;
}
}
return(result);
}
/// <summary>
/// 只有基准不同的 XYZ 坐标转换,采用布尔沙 7 参数模型。
/// 以WGS84转换参数作为中间参数，共转换 2 次
/// </summary>
/// <param name="oldCoord">待转坐标，只取其数字部分，参考系取自属性本对象的TargetCrs属性 </param>
/// <returns></returns>
public override ICoordinate MatchedTrans(ICoordinate oldCoord)
{
BursaTransParams BursaTransParams = ((IGeodeticDatum)SourceCrs.Datum).TransParamsToWgs84;
IXYZ temp = GeodeticUtils.BursaTransform((IXYZ)oldCoord, BursaTransParams);
//double[] result = BursaTransParams.Transform(new double[] { oldCoord.X, oldCoord.Y, oldCoord.Z });
IXYZ xyz = temp;
if (!TargetCrs.Datum.Equals(GeodeticDatum.WGS84))
{
xyz = GeodeticUtils.BursaTransform(temp, ((IGeodeticDatum)TargetCrs.Datum).TransParamsToWgs84.GetInverse());
}
return(CoordinateFactory.CreateXyzCoord(xyz.X, xyz.Y, xyz.Z));
}
/// <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);
}
/// <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>
/// Вычисление точки пересечения прямой с плоскостью.
/// </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);
}
}
private KdNode Insert(KdNode node, IXYZ p, bool vertical)
{
if (node == null)
{
size++;
return(new KdNode(p.X, p.Y, vertical));
}
if (node.X.Equals(p.X) && node.Y.Equals(p.Y))
{
return(node);
}
if (node.Vertical && p.X < node.X || !node.Vertical && p.Y < node.Y)
{
node.Left = Insert(node.Left, p, !node.Vertical);
}
else
{
node.Right = Insert(node.Right, p, !node.Vertical);
}
return(node);
}
public double DistanceSquaredTo(IXYZ p)
{
double dx = 0, dy = 0;
if (p.X < Xmin)
{
dx = p.X - Xmin;
}
else if (p.X > Xmax)
{
dx = p.X - Xmax;
}
if (p.Y < Ymin)
{
dy = p.Y - Ymin;
}
else if (p.Y > Ymax)
{
dy = p.Y - Ymax;
}
return(dx * dx + dy * dy);
}
/// <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);
}
public Circle2d(IXYZ cc, double r)
{
Center = new Point3d(cc);
Radius = r;
Length = 2 * Radius * Math.PI;
}
/// <summary>
/// Проверяет, близость координат точечного объекта к нулю.
/// </summary>
/// <param name="point">Координаты точечного объекта.</param>
/// <param name="threshold">Допуск.</param>
/// <returns>True if its close to one or false in any other case.</returns>
public static bool IsZero(IXYZ point, double threshold)
{
return(point.X >= -threshold && point.X <= threshold &&
point.Y >= -threshold && point.Y <= threshold &&
point.Z >= -threshold && point.Z <= threshold);
}
private int ClipFacet(IXYZ[] p, IXYZ n, IXYZ p0)
{
double A;
double B;
double C;
double D;
double l;
double[] side = new double[3];
IXYZ q = new Point3d();
/*
* Определение уравнения плоскости в виде
* Ax + By + Cz + D = 0
*/
l = Math.Sqrt(n.X * n.X + n.Y * n.Y + n.Z * n.Z);
A = n.X / l;
B = n.Y / l;
C = n.Z / l;
D = -(n.X * p0.X + n.Y * p0.Y + n.Z * p0.Z);
/*
* Оценка уравнения плоскости для каждой вершины.
* Если side < 0, то она находится на той стороне,
* которую необходимо сохранить,
* в противном случае она должна быть обрезана.
*/
side[0] = A * p[0].X + B * p[0].Y + C * p[0].Z + D;
side[1] = A * p[1].X + B * p[1].Y + C * p[1].Z + D;
side[2] = A * p[2].X + B * p[2].Y + C * p[2].Z + D;
// Все ли вершины на стононе обрезки
if (side[0] >= 0 && side[1] >= 0 && side[2] >= 0)
{
return(0);
}
// Все ли вершины на противоположной стороне
if (side[0] <= 0 && side[1] <= 0 && side[2] <= 0)
{
return(3);
}
// p0 единственная точка на стороне обрезки
if (side[0] > 0 && side[1] < 0 && side[2] < 0)
{
q.X = p[0].X - side[0] * (p[2].X - p[0].X) / (side[2] - side[0]);
q.Y = p[0].Y - side[0] * (p[2].Y - p[0].Y) / (side[2] - side[0]);
q.Z = p[0].Z - side[0] * (p[2].Z - p[0].Z) / (side[2] - side[0]);
p[3] = q;
q.X = p[0].X - side[0] * (p[1].X - p[0].X) / (side[1] - side[0]);
q.Y = p[0].Y - side[0] * (p[1].Y - p[0].Y) / (side[1] - side[0]);
q.Z = p[0].Z - side[0] * (p[1].Z - p[0].Z) / (side[1] - side[0]);
p[0] = q;
return(4);
}
// p1 единственная точка на стороне обрезки
if (side[1] > 0 && side[0] < 0 && side[2] < 0)
{
p[3] = p[2];
q.X = p[1].X - side[1] * (p[2].X - p[1].X) / (side[2] - side[1]);
q.Y = p[1].Y - side[1] * (p[2].Y - p[1].Y) / (side[2] - side[1]);
q.Z = p[1].Z - side[1] * (p[2].Z - p[1].Z) / (side[2] - side[1]);
p[2] = q;
q.X = p[1].X - side[1] * (p[0].X - p[1].X) / (side[0] - side[1]);
q.Y = p[1].Y - side[1] * (p[0].Y - p[1].Y) / (side[0] - side[1]);
q.Z = p[1].Z - side[1] * (p[0].Z - p[1].Z) / (side[0] - side[1]);
p[1] = q;
return(4);
}
// p2 единственная точка на стороне обрезки
if (side[2] > 0 && side[0] < 0 && side[1] < 0)
{
q.X = p[2].X - side[2] * (p[0].X - p[2].X) / (side[0] - side[2]);
q.Y = p[2].Y - side[2] * (p[0].Y - p[2].Y) / (side[0] - side[2]);
q.Z = p[2].Z - side[2] * (p[0].Z - p[2].Z) / (side[0] - side[2]);
p[3] = q;
q.X = p[2].X - side[2] * (p[1].X - p[2].X) / (side[1] - side[2]);
q.Y = p[2].Y - side[2] * (p[1].Y - p[2].Y) / (side[1] - side[2]);
q.Z = p[2].Z - side[2] * (p[1].Z - p[2].Z) / (side[1] - side[2]);
p[2] = q;
return(4);
}
// p0 единственная точка на обратной стороне обрезки
if (side[0] < 0 && side[1] > 0 && side[2] > 0)
{
q.X = p[0].X - side[0] * (p[1].X - p[0].X) / (side[1] - side[0]);
q.Y = p[0].Y - side[0] * (p[1].Y - p[0].Y) / (side[1] - side[0]);
q.Z = p[0].Z - side[0] * (p[1].Z - p[0].Z) / (side[1] - side[0]);
p[1] = q;
q.X = p[0].X - side[0] * (p[2].X - p[0].X) / (side[2] - side[0]);
q.Y = p[0].Y - side[0] * (p[2].Y - p[0].Y) / (side[2] - side[0]);
q.Z = p[0].Z - side[0] * (p[2].Z - p[0].Z) / (side[2] - side[0]);
p[2] = q;
return(3);
}
// p1 единственная точка на обратной стороне обрезки
if (side[1] < 0 && side[0] > 0 && side[2] > 0)
{
q.X = p[1].X - side[1] * (p[0].X - p[1].X) / (side[0] - side[1]);
q.Y = p[1].Y - side[1] * (p[0].Y - p[1].Y) / (side[0] - side[1]);
q.Z = p[1].Z - side[1] * (p[0].Z - p[1].Z) / (side[0] - side[1]);
p[0] = q;
q.X = p[1].X - side[1] * (p[2].X - p[1].X) / (side[2] - side[1]);
q.Y = p[1].Y - side[1] * (p[2].Y - p[1].Y) / (side[2] - side[1]);
q.Z = p[1].Z - side[1] * (p[2].Z - p[1].Z) / (side[2] - side[1]);
p[2] = q;
return(3);
}
// p2 единственная точка на обратной стороне обрезки
if (side[2] < 0 && side[0] > 0 && side[1] > 0)
{
q.X = p[2].X - side[2] * (p[1].X - p[2].X) / (side[1] - side[2]);
q.Y = p[2].Y - side[2] * (p[1].Y - p[2].Y) / (side[1] - side[2]);
q.Z = p[2].Z - side[2] * (p[1].Z - p[2].Z) / (side[1] - side[2]);
p[1] = q;
q.X = p[2].X - side[2] * (p[0].X - p[2].X) / (side[0] - side[2]);
q.Y = p[2].Y - side[2] * (p[0].Y - p[2].Y) / (side[0] - side[2]);
q.Z = p[2].Z - side[2] * (p[0].Z - p[2].Z) / (side[0] - side[2]);
p[0] = q;
return(3);
}
// Shouldn't get here
return(-1);
}
/// <summary>
/// Проверяет, совпадают ли координаты точечных объектов.
/// </summary>
/// <param name="a">Координаты точечного объекта.</param>
/// <param name="b">Координаты точечного объекта.</param>
/// <returns>True if its close to one or false in any other case.</returns>
public static bool IsEqual(IXYZ a, IXYZ b)
{
return(IsEqual(a, b, Epsilon));
}
/// <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));
}
/// <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>
/// <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)
{
return(FindIntersection(p1, p2, sc, r, out res1, out res2, epsilon));
}
/// <summary>
/// Проверяет, близость координат точечного объекта к нулю.
/// </summary>
/// <param name="point">Координаты точечного объекта.</param>
/// <returns>True if its close to one or false in any other case.</returns>
public static bool IsZero(IXYZ point)
{
return(IsZero(point, Epsilon));
}
/// <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>
/// <returns>TRUE, если точка пересечения найдена.</returns>
public static bool FindIntersection(IXYZ p1, IXYZ p2, IXYZ p3,
IXYZ p4, IXYZ p5,
out Point3d res, out double t)
{
return(FindIntersection(p1, p2, p3, p4, p5, out res, out t, epsilon));
}
/// <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>
/// <returns>TRUE, если линия пересечения найдена.</returns>
public static bool FindIntersection(IXYZ p1, IXYZ p2, IXYZ p3,
IXYZ p4, IXYZ p5, IXYZ p6,
out Line3d res)
{
return(FindIntersection(p1, p2, p3, p4, p5, p6, out res, epsilon));
}
