public clsPoint3d ClosestPointToLine(clsLine3d l2)
{
clsPoint3d v1;
clsPoint3d v2;
clsPoint3d M;
double m2;
clsPoint3d R;
clsPoint3d P21;
double t1;
clsPoint3d Q1;
//Find the actual point on this line where the perpendicular distance hits.
v1 = new clsPoint3d(DX(), DY(), DZ());
v1.Normalise();
v2 = new clsPoint3d(l2.DX(), l2.DY(), l2.DZ());
v2.Normalise();
M = v2.Cross(v1);
m2 = M.Dot(M);
if (m2 < mdlGeometry.myTol)
{
return(l2.P1);
}
//Parallel
P21 = new clsPoint3d(l2.X1 - X1, l2.Y1 - Y1, l2.Z1 - Z1);
R = P21.Cross(M);
R.Scale(1 / m2);
t1 = R.Dot(v2);
Q1 = P1 + t1 * v1;
return(Q1);
}
public double DistanceToLine(clsLine3d l2)
{
//Perpendicular distance between 3d lines. Limited to the line segments.
double myNormalDist;
clsPoint3d v1;
clsPoint3d v2;
clsPoint3d M;
double m2;
clsPoint3d R;
clsPoint3d P21;
double t1;
double t2;
clsPoint3d Q1;
clsPoint3d Q2;
//Find the actual point on this line where the perpendicular distance hits. If it is off the line, then find the minimum distance between the end points
v1 = new clsPoint3d(DX(), DY(), DZ());
v1.Normalise();
v2 = new clsPoint3d(l2.DX(), l2.DY(), l2.DZ());
v2.Normalise();
P21 = new clsPoint3d(l2.X1 - X1, l2.X1 - X1, l2.X1 - X1);
M = v2.Cross(v1);
m2 = M.Dot(M);
if (m2 < mdlGeometry.myTol)
{
return(DistanceToPoint(l2.P1));
}
//Parallel
myNormalDist = Abs(P21.Dot(M)) / Sqrt(m2);
//Perpendicular distance
R = P21.Cross(M);
R.Scale(1 / m2);
t1 = R.Dot(v2);
Q1 = P1 + t1 * v1;
if (t1 < 0)
{
Q1 = P1;
}
if (t1 > Length)
{
Q1 = P2;
}
t2 = R.Dot(v1);
Q2 = l2.P1 + t2 * v2;
if (t2 < 0)
{
Q2 = l2.P1;
}
if (t2 > l2.Length)
{
Q2 = l2.P2;
}
return(Q1.Dist(Q2));
}
public clsPoint3d Normalised()
{
//Returns a copy of the normalised point
clsPoint3d p1 = default(clsPoint3d);
p1 = Copy();
p1.Normalise();
return(p1);
}
public double MaxAnglePerpendicular(ref clsPoint3d maxAV1, ref clsPoint3d maxAV2)
{
double a;
double maxA;
clsPoint3d py, pz;
clsPoint3d p1 = null, p2 = null, p3 = null, p4 = null;
if (MaxAngle(ref p3, ref p4) < myTol)
{
return(0);
}
pz = new clsPoint3d(0, 0, 1.0);
py = pz.Cross(p3);
if (IsSameDbl(py.Length, 0))
{
py = pz.Cross(p4);
}
if (IsSameDbl(py.Length, 0))
{
return(0);
}
py.Normalise();
maxA = 0;
for (int j = 0; j <= myCameraPoints.Count - 2; j++)
{
for (int k = j + 1; k <= myCameraPoints.Count - 1; k++)
{
p1 = myCameraPoints[j];
p1 = py * py.Dot(p1) + pz * pz.Dot(p1);
p1.Normalise();
p2 = myCameraPoints[k];
p2 = py * py.Dot(p2) + pz * pz.Dot(p2);
p2.Normalise();
a = Acos(p1.Dot(p2));
if (Abs(a) > maxA)
{
maxA = Abs(a);
maxAV1 = p1.Copy();
maxAV2 = p2.Copy();
}
}
}
return(maxA);
}
public double MaxDistance(ref clsPoint3d maxAV1, ref clsPoint3d maxAV2)
{
double d;
double maxD;
clsPoint3d p1, p2;
clsPoint3d px, py, pz;
maxD = 0;
pz = new clsPoint3d(0, 0, 1.0);
for (int j = 0; j <= myCameraPoints.Count - 2; j++)
{
p1 = myCameraPoints[j];
for (int k = j + 1; k <= myCameraPoints.Count - 1; k++)
{
p2 = myCameraPoints[k];
py = new clsPoint3d((p1.X + p2.X) / 2, (p1.Y + p2.Y) / 2, (p1.Z + p2.Z) / 2).Point2D().Point3d(0);
if (IsSameDbl(py.Length, 0))
{
continue;
}
py.Normalise();
px = pz.Cross(py);
px.Normalise();
p1 = px * p1.Dot(px) + pz * p1.Dot(pz);
p1.Normalise();
p2 = px * p2.Dot(px) + pz * p2.Dot(pz);
p2.Normalise();
d = myCameraPoints[j].Dist(myCameraPoints[k]);
if (Abs(d) > maxD)
{
maxD = Abs(d);
maxAV1 = p1.Copy();
maxAV2 = p2.Copy();
}
}
}
return(maxD);
}