示例#1
0
        public bool IsOnShortLine(clsPoint3d pt1, double aTol = 0, bool excludeEnds = false)
        {
            clsLine3d l1 = default(clsLine3d);
            clsLine3d l2 = default(clsLine3d);
            double    d  = 0;

            if (aTol == 0)
            {
                aTol = mdlGeometry.myTol;
            }
            if (Abs(DistanceToPoint(pt1)) > aTol)
            {
                return(false);
            }
            l1 = new clsLine3d(P1, pt1);
            l2 = Copy();
            l2.Normalise();
            d = l1.Dot(l2);
            if (d < -aTol)
            {
                return(false);
            }
            if (d > Length + aTol)
            {
                return(false);
            }
            if (excludeEnds && (P1 == pt1 | P2 == pt1))
            {
                return(false);
            }
            return(true);
        }
示例#2
0
        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);
        }
示例#3
0
        public clsPoint3d DistanceAlongLine(double d)
        {
            clsLine3d l1 = default(clsLine3d);

            l1 = Copy();
            l1.Normalise();
            return(new clsPoint3d(X1 + d * l1.DX(), Y1 + d * l1.DY(), Z1 + d * l1.DZ()));
        }
示例#4
0
        public clsPoint3d PointFromLambda(double l)
        {
            clsLine3d l1 = default(clsLine3d);

            l1 = Copy();
            l1.Scale(l);
            return(l1.P2);
        }
示例#5
0
        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));
        }
示例#6
0
        public double Lambda(clsPoint3d pt1)
        {
            clsLine3d l1 = default(clsLine3d);
            clsLine3d l2 = default(clsLine3d);

            l1 = new clsLine3d(P1, pt1);
            l2 = Copy();
            l2.Normalise();
            return(l1.Dot(l2) / Length);
        }
示例#7
0
        public clsLine3d Cross(clsLine3d l1)
        {
            double x = 0;
            double y = 0;
            double z = 0;

            x = DY() * l1.DZ() - DZ() * l1.DY();
            y = -(DX() * l1.DZ() - DZ() * l1.DX());
            z = DX() * l1.DY() - DY() * l1.DX();
            return(new clsLine3d(0, 0, 0, x, y, z));
        }
示例#8
0
        public bool IsOnLine(clsPoint3d aPt)
        {
            clsLine3d l1 = default(clsLine3d);

            l1 = new clsLine3d(P2, aPt);
            if (mdlGeometry.IsSameDbl(Cross(l1).Length, 0))
            {
                return(true);
            }
            return(false);
        }
示例#9
0
        public static clsPoint3d ProjectPoint(clsPoint3d p1, clsLine3d l1)
        {
            double    d1 = 0;
            clsLine3d l2 = new clsLine3d();
            clsLine3d l3 = new clsLine3d();

            l2 = l1.Copy();
            l2.Normalise();
            l3        = new clsLine3d(l1.P1, p1);
            d1        = l2.Dot(l3);
            l2.Length = d1;
            return(l2.P2);
        }
示例#10
0
        public clsPoint3d IntersectQuick(clsLine3d l1)
        {
            //Both infinite
            double lambda = 0;

            if (Abs((X2 - X1) * (l1.Y1 - l1.Y2) + (Y2 - Y1) * (l1.X2 - l1.X1)) < mdlGeometry.myTol / 10)
            {
                return(null);
            }
            //No intersection

            lambda = ((l1.X2 - X1) * (l1.Y1 - l1.Y2) + (l1.Y2 - Y1) * (l1.X2 - l1.X1)) / ((X2 - X1) * (l1.Y1 - l1.Y2) + (Y2 - Y1) * (l1.X2 - l1.X1));
            return(new clsPoint3d(X1 + lambda * (X2 - X1), Y1 + lambda * (Y2 - Y1), 0));
        }
示例#11
0
        public double DistanceToPoint(clsPoint3d aPt)
        {
            double    d1 = 0;
            double    d2 = 0;
            clsLine3d l2 = default(clsLine3d);
            clsLine3d l3 = default(clsLine3d);

            l2 = Copy();
            l2.Normalise();
            l3 = new clsLine3d(P1, aPt);
            d1 = l2.Dot(l3);
            d2 = l3.Length;
            return(Sqrt(Pow(d2, 2) - Pow(d1, 2)));
        }
示例#12
0
        public static double Dist3d(clsPoint3d p1, clsLine3d l1)
        {
            double    d1 = 0;
            double    d2 = 0;
            clsLine3d l2 = new clsLine3d();
            clsLine3d l3 = new clsLine3d();

            l2 = l1.Copy();
            l2.Normalise();
            l3 = new clsLine3d(l1.P1, p1);
            d1 = l2.Dot(l3);
            d2 = l3.Length;
            return(Sqrt(Pow(d2, 2) - Pow(d1, 2)));
        }
示例#13
0
        public static double DistancePointLine3D(clsPoint3d p1, clsLine3d l1)
        {
            //Distance between 3D point and truncated 3D line
            clsPoint3d p3d = new clsPoint3d();
            double     l   = 0;

            p3d = ProjectPoint(p1, l1);
            l   = l1.Lambda(p3d);
            if (l < 0)
            {
                return(p1.Dist(l1.P1));
            }
            if (l > 1)
            {
                return(p1.Dist(l1.P2));
            }
            return(p1.Dist(p3d));
        }
示例#14
0
        public static clsPoint3d ProjectPointOntoPlaneAlongZ(clsPoint p, clsLine3d l1)
        {
            clsLine3d  v1 = new clsLine3d();
            double     u  = 0;
            double     r  = 0;
            double     z  = 0;
            clsPoint3d p1 = new clsPoint3d();

            v1 = new clsLine3d(new clsPoint3d(0, 0, 0), new clsPoint3d(0, 0, 1));
            u  = v1.Dot(l1);
            //This is the length of the normal vector, measured in the vertical direction

            p1 = new clsPoint3d(p.X - l1.X1, p.Y - l1.Y1, 0 - l1.Z1);
            r  = p1.Dot(l1.DP());
            //r is the distance of our point to the nearest point on the plane
            //r = v1.Dot(myV)
            z = r / u;
            //z is the vertical distance of our point to the plane. It is sign sensitive - positive is above the plane.

            return(new clsPoint3d(p.X, p.Y, -z));
        }
示例#15
0
        public bool Overlaps(clsLine3d aLine, double aTol = 0)
        {
            clsLine3d  l1 = default(clsLine3d);
            clsPoint3d p3 = default(clsPoint3d);
            clsPoint3d p4 = default(clsPoint3d);

            if (aTol == 0)
            {
                aTol = mdlGeometry.myTol;
            }
            l1 = aLine.Copy();
            if ((!IsOnLine(l1.P1)) | (!IsOnLine(l1.P2)))
            {
                return(false);
            }
            if (Dot(l1) < 0)
            {
                l1.Reverse();
            }
            p3 = l1.P1;
            p4 = l1.P2;
            if (P1 == p3)
            {
                return(true);
            }
            if (P1 == p4 | P2 == p3)
            {
                return(false);
            }
            if (IsOnShortLine(p4))
            {
                return(true);
            }
            if (l1.IsOnShortLine(P1, aTol) | l1.IsOnShortLine(P2, aTol))
            {
                return(true);
            }
            return(false);
        }
示例#16
0
        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;
        }
示例#17
0
        public clsPoint3d Intersect(clsLine3d l1)
        {
            //Both infinite. More accurate that IntersectQuick
            double    l  = 0;
            clsLine3d l2 = default(clsLine3d);
            clsLine3d l3 = default(clsLine3d);

            if (Length < mdlGeometry.myTol | l1.Length < mdlGeometry.myTol)
            {
                return(null);
            }
            l2        = Copy();
            l3        = l1.Copy();
            l2.Length = 1000;
            l3.Length = 1000;

            if (Abs(((l2.X2 - l2.X1) * (l3.Y1 - l3.Y2) + (l2.Y2 - l2.Y1) * (l3.X2 - l3.X1))) < mdlGeometry.myTol / 10)
            {
                return(null);
            }
            //No intersection
            l = ((l3.X2 - l2.X1) * (l3.Y1 - l3.Y2) + (l3.Y2 - l2.Y1) * (l3.X2 - l3.X1)) / ((l2.X2 - l2.X1) * (l3.Y1 - l3.Y2) + (l2.Y2 - l2.Y1) * (l3.X2 - l3.X1));
            return(new clsPoint3d(l2.X1 + l * (l2.X2 - l2.X1), l2.Y1 + l * (l2.Y2 - l2.Y1), 0));
        }
示例#18
0
 public double Dot(clsLine3d l1)
 {
     return((P2.X - P1.X) * (l1.P2.X - l1.P1.X) + (P2.Y - P1.Y) * (l1.P2.Y - l1.P1.Y) + (P2.Z - P1.Z) * (l1.P2.Z - l1.P1.Z));
 }
示例#19
0
 public void RotateAboutLine(clsLine3d l1, double a)
 {
     P1.RotateAboutLine(l1, a);
     P2.RotateAboutLine(l1, a);
 }
示例#20
0
        /// <summary>
        /// Processes the point list pair.
        /// </summary>
        /// <param name="estimatedLocationPoints">The estimated location points (from AR Toolkit).</param>
        /// <param name="axisStartPoints">The axis start point (from the camera).</param>
        /// <param name="axisEndPoints">The axis end point (from the camera).</param>
        /// <param name="myNumPtsCalculated">Nnumber of points calculated.</param>
        /// <returns>The average point of all processed points if successful, otherwise a zero length vector.</returns>
        /// <remarks>estimatedLocationPoints and seenFromCameraPoints must be matched lists.</remarks>
        /// <exception cref="ArgumentNullException"></exception>
        /// <exception cref="ArgumentException">estimatedLocationPoints and seenFromCameraPoints should be matched lists.</exception>
        private clsPoint3d ProcessPointListPair(List <clsPoint3d> estimatedLocationPoints, List <clsPoint3d> seenFromCameraPoints, ref int myNumPtsCalculated)
        {
            if (estimatedLocationPoints == null || seenFromCameraPoints == null)
            {
                throw new ArgumentNullException();
            }
            if (estimatedLocationPoints.Count != seenFromCameraPoints.Count)
            {
                throw new ArgumentException("estimatedLocationPoints and seenFromCameraPoints should be matched lists.");
            }
            myNumPtsCalculated = 0;

            // Take copies of the list, such as not to affect the passed list
            estimatedLocationPoints = estimatedLocationPoints.ToList();
            seenFromCameraPoints    = seenFromCameraPoints.ToList();

            // Get the points within 12.5mm of the average
            var points = GetPointsWithinDistanceOfAveragePoint(estimatedLocationPoints, 12.5);

            // Align the matched lists by removing the axis points matched to the estimated location points that have been removed
            var removedPoints = estimatedLocationPoints.Where(p => !points.Contains(p)).ToList();

            removedPoints.ForEach(p => {
                var index = estimatedLocationPoints.IndexOf(p);
                estimatedLocationPoints.RemoveAt(index);
                seenFromCameraPoints.RemoveAt(index);
            });

            // Don't process anything if we have less than 3 usable points
            if (points.Count <= 3)
            {
                return(new clsPoint3d(0, 0, 0));
            }


            var intersectionPoints = new List <clsPoint3d>();

            // Get the first line - don't loop through all as we compare to the next line
            for (var i = 0; i <= estimatedLocationPoints.Count - 2; i++)
            {
                var line1 = new clsLine3d(estimatedLocationPoints[i], seenFromCameraPoints[i]);

                // Compare to the next line
                for (var j = i + 1; j <= estimatedLocationPoints.Count - 1; j++)
                {
                    var line2 = new clsLine3d(estimatedLocationPoints[j].Copy(), seenFromCameraPoints[j].Copy());

                    // Get midpoint of each line
                    var p1 = line1.Point;
                    p1.Normalise();
                    var p2 = line2.Point;
                    p2.Normalise();

                    // Determine angle between lines
                    var a = Acos(p1.Dot(p2)) * 180 / PI;
                    if (a > myPGAngleTol)
                    {
                        // Angle is above minimum threshold, so determine the point on each line which is closest to the other line
                        var p3 = line1.ClosestPointToLine(line2);
                        var p4 = line2.ClosestPointToLine(line1);

                        // Determine the distance between the closest points
                        var d = p3.Dist(p4);
                        if (d < myPGPointTol)
                        {
                            // Distance is below threshold so add the intersection point as the midpoint between them
                            intersectionPoints.Add(new clsPoint3d((p3.X + p4.X) / 2, (p3.Y + p4.Y) / 2, (p3.Z + p4.Z) / 2));
                        }
                    }
                }
            }

            var intersectionPointsToFind = 10;

            if (intersectionPoints.Count >= intersectionPointsToFind)
            {
                myNumPtsCalculated = intersectionPoints.Count;
                return(GetAveragePoint(intersectionPoints));
            }
            else
            {
                DebugStringList.Add($"{intersectionPoints.Count} / {intersectionPointsToFind}");
            }
            return(new clsPoint3d(0, 0, 0));
        }