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 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 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 void RotateAboutLine(clsLine3d l1, double theta) { clsPoint3d p1 = default(clsPoint3d); clsPoint3d p2 = default(clsPoint3d); clsPoint3d p2a = default(clsPoint3d); clsPoint3d p2b = default(clsPoint3d); clsPoint3d p3 = default(clsPoint3d); clsLine3d l2 = default(clsLine3d); if (l1.IsOnLine(this)) { return; } //Setup a coordinate system with X running along l1 and the origin at l1.p2 l2 = new clsLine3d(l1.P2.Copy(), Copy()); p1 = new clsPoint3d(l1.DX(), l1.DY(), l1.DZ()); p1.Length = 1; p2 = new clsPoint3d(l2.DX(), l2.DY(), l2.DZ()); p2a = new clsPoint3d(l2.DX(), l2.DY(), l2.DZ()); p2.Length = 1; p3 = p1.Cross(p2); p3.Length = 1; p2 = p1.Cross(p3); p2.Length = 1; if (p2.Dot(p2a) < 0) { p2.Scale(-1); } p2b = new clsPoint3d(p1.Dot(p2a), p2.Dot(p2a), p3.Dot(p2a)); p2b.RotateAboutX(theta); myX = l1.X2 + p2b.x * p1.x + p2b.y * p2.x + p2b.z * p3.x; myY = l1.Y2 + p2b.x * p1.y + p2b.y * p2.y + p2b.z * p3.y; myZ = l1.Z2 + p2b.x * p1.z + p2b.y * p2.z + p2b.z * p3.z; }
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); }